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;
}
}