예제 #1
0
        public static void DrawPointCloud <C>(Point32Msg[] points, Drawing3d drawing, Color color, float radius = 0.01f)
            where C : ICoordinateSpace, new()
        {
            PointCloudDrawing pointCloud = drawing.AddPointCloud(points.Length);

            foreach (Point32Msg p in points)
            {
                pointCloud.AddPoint(p.From <C>(), color, radius);
            }
            pointCloud.Bake();
        }
            public static void Draw <C>(MarkerMsg marker, Drawing3d drawing)
                where C : ICoordinateSpace, new()
            {
                switch (marker.type)
                {
                case MarkerMsg.ARROW:
                    Vector3 startPoint;
                    Vector3 endPoint;
                    if (marker.points.Length >= 2)
                    {
                        startPoint = marker.points[0].From <C>();
                        endPoint   = marker.points[1].From <C>();

                        float arrowheadGradient = 0.5f;
                        if (marker.scale.z != 0)
                        {
                            arrowheadGradient = (float)(marker.scale.y / marker.scale.z);
                        }

                        drawing.DrawArrow(startPoint, endPoint, marker.color.ToUnityColor(),
                                          (float)marker.scale.x, (float)(marker.scale.y / marker.scale.x), arrowheadGradient);
                    }
                    else
                    {
                        startPoint = marker.pose.position.From <C>();
                        endPoint   = startPoint + marker.pose.orientation.From <C>() * Vector3.forward * (float)marker.scale.x;

                        drawing.DrawArrow(startPoint, endPoint, marker.color.ToUnityColor(), (float)marker.scale.y);
                    }
                    break;

                case MarkerMsg.CUBE:
                    drawing.DrawCuboid(marker.pose.position.From <C>(), marker.scale.From <C>() * 0.5f, marker.pose.orientation.From <C>(), marker.color.ToUnityColor());
                    break;

                case MarkerMsg.SPHERE:
                    drawing.DrawSpheroid(marker.pose.position.From <C>(), marker.scale.From <C>() * 0.5f, marker.pose.orientation.From <C>(), marker.color.ToUnityColor());
                    break;

                case MarkerMsg.CYLINDER:
                    drawing.transform.position   = marker.pose.position.From <C>();
                    drawing.transform.rotation   = marker.pose.orientation.From <C>();
                    drawing.transform.localScale = marker.scale.From <C>();
                    drawing.DrawCylinder(new Vector3(0, -0.5f, 0), new Vector3(0, 0.5f, 0), marker.color.ToUnityColor(), 0.5f);
                    break;

                case MarkerMsg.LINE_STRIP:
                    drawing.transform.position = marker.pose.position.From <C>();
                    drawing.transform.rotation = marker.pose.orientation.From <C>();
                    if (marker.colors.Length == marker.points.Length)
                    {
                        drawing.DrawLineStrip(marker.points.Select(p => p.From <C>()).ToArray(), marker.colors.Select(c => (Color32)c.ToUnityColor()).ToArray(), (float)marker.scale.x);
                    }
                    else
                    {
                        drawing.DrawLineStrip(marker.points.Select(p => p.From <C>()).ToArray(), marker.color.ToUnityColor(), (float)marker.scale.x);
                    }
                    break;

                case MarkerMsg.LINE_LIST:
                    drawing.transform.position = marker.pose.position.From <C>();
                    drawing.transform.rotation = marker.pose.orientation.From <C>();
                    if (marker.colors.Length == marker.points.Length)
                    {
                        drawing.DrawLines(marker.points.Select(p => p.From <C>()).ToArray(), marker.colors.Select(c => (Color32)c.ToUnityColor()).ToArray(), (float)marker.scale.x);
                    }
                    else
                    {
                        drawing.DrawLines(marker.points.Select(p => p.From <C>()).ToArray(), marker.color.ToUnityColor(), (float)marker.scale.x);
                    }
                    break;

                case MarkerMsg.CUBE_LIST:
                {
                    drawing.transform.position = marker.pose.position.From <C>();
                    drawing.transform.rotation = marker.pose.orientation.From <C>();
                    Vector3 cubeScale = marker.scale.From <C>() * 0.5f;
                    if (marker.colors.Length == marker.points.Length)
                    {
                        for (int Idx = 0; Idx < marker.points.Length; ++Idx)
                        {
                            drawing.DrawCuboid(marker.points[Idx].From <C>(), cubeScale, marker.colors[Idx].ToUnityColor());
                        }
                    }
                    else
                    {
                        Color32 color = marker.color.ToUnityColor();
                        for (int Idx = 0; Idx < marker.points.Length; ++Idx)
                        {
                            drawing.DrawCuboid(marker.points[Idx].From <C>(), cubeScale, color);
                        }
                    }
                }
                break;

                case MarkerMsg.SPHERE_LIST:
                {
                    drawing.transform.position = marker.pose.position.From <C>();
                    drawing.transform.rotation = marker.pose.orientation.From <C>();
                    Vector3 radii = marker.scale.From <C>() * 0.5f;
                    if (marker.colors.Length == marker.points.Length)
                    {
                        for (int Idx = 0; Idx < marker.points.Length; ++Idx)
                        {
                            drawing.DrawSpheroid(marker.points[Idx].From <C>(), radii, Quaternion.identity, marker.colors[Idx].ToUnityColor());
                        }
                    }
                    else
                    {
                        Color32 color = marker.color.ToUnityColor();
                        for (int Idx = 0; Idx < marker.points.Length; ++Idx)
                        {
                            drawing.DrawSpheroid(marker.points[Idx].From <C>(), radii, Quaternion.identity, color);
                        }
                    }
                }
                break;

                case MarkerMsg.POINTS:
                {
                    PointCloudDrawing cloud = drawing.AddPointCloud(marker.points.Length);
                    cloud.transform.position = marker.pose.position.From <C>();
                    cloud.transform.rotation = marker.pose.orientation.From <C>();
                    float radius = (float)marker.scale.x;
                    if (marker.colors.Length == marker.points.Length)
                    {
                        for (int Idx = 0; Idx < marker.points.Length; ++Idx)
                        {
                            cloud.AddPoint(marker.points[Idx].From <C>(), marker.colors[Idx].ToUnityColor(), radius);
                        }
                    }
                    else
                    {
                        Color32 color = marker.color.ToUnityColor();
                        for (int Idx = 0; Idx < marker.points.Length; ++Idx)
                        {
                            cloud.AddPoint(marker.points[Idx].From <C>(), color, radius);
                        }
                    }
                    cloud.Bake();
                }
                break;

                case MarkerMsg.TEXT_VIEW_FACING:
                    drawing.DrawLabel(marker.text, marker.pose.position.From <C>(), marker.color.ToUnityColor());
                    break;

                case MarkerMsg.MESH_RESOURCE:
                    break;

                case MarkerMsg.TRIANGLE_LIST:
                {
                    drawing.transform.position = marker.pose.position.From <C>();
                    drawing.transform.rotation = marker.pose.orientation.From <C>();
                    float radius = (float)marker.scale.x;
                    if (marker.colors.Length == marker.points.Length)
                    {
                        for (int Idx = 2; Idx < marker.points.Length; Idx += 3)
                        {
                            drawing.DrawTriangle(
                                marker.points[Idx - 2].From <C>(),
                                marker.points[Idx - 1].From <C>(),
                                marker.points[Idx].From <C>(),
                                marker.colors[Idx - 2].ToUnityColor(),
                                marker.colors[Idx - 1].ToUnityColor(),
                                marker.colors[Idx].ToUnityColor());
                        }
                    }
                    else
                    {
                        Color32 color = marker.color.ToUnityColor();
                        for (int Idx = 2; Idx < marker.points.Length; Idx += 3)
                        {
                            drawing.DrawTriangle(
                                marker.points[Idx - 2].From <C>(),
                                marker.points[Idx - 1].From <C>(),
                                marker.points[Idx].From <C>(),
                                color);
                        }
                    }
                }
                break;
                }
            }