public static void Draw <C>(Vector3Msg message, Drawing3d drawing, Color color, string label, float size = 0.01f) where C : ICoordinateSpace, new()
{
Vector3 point = message.From <C>();
drawing.DrawPoint(point, color, size);
drawing.DrawLabel(label, point, color, size * 1.5f);
}
public static void Draw <C>(Vector3Msg message, Drawing3d drawing, GameObject origin, Color color, string label, float size = 0.01f) where C : ICoordinateSpace, new()
{
Vector3 point = message.From <C>();
if (origin != null)
{
point = origin.transform.TransformPoint(point);
}
drawing.DrawPoint(point, color, size);
drawing.DrawLabel(label, point, color, size * 1.5f);
}
public override void Draw(Drawing3d drawing, IEnumerable <Tuple <PointMsg, MessageMetadata> > messages)
{
Vector3 prevPoint = Vector3.zero;
Color color = Color.white;
string label = "";
MessageMetadata meta = messages.FirstOrDefault().Item2;
color = VisualizationUtils.SelectColor(m_Color, meta);
label = VisualizationUtils.SelectLabel(m_Label, meta);
drawing.DrawPath(messages.Select(tuple => tuple.Item1.From <FLU>()), color, m_Thickness);
drawing.DrawLabel(label, prevPoint, color);
}
public override void Draw(Drawing3d drawing, QuaternionMsg message, MessageMetadata meta)
{
Draw <FLU>(message, drawing, m_DrawAtPosition, m_Size, m_DrawUnityAxes);
drawing.DrawLabel(SelectLabel(m_Label, meta), transform.position, SelectColor(m_Color, meta), m_Size);
}
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;
}
}
public override void Draw(Drawing3d drawing, TransformStampedMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
TransformDefaultVisualizer.Draw <FLU>(message.transform, drawing, m_Size, m_DrawUnityAxes);
drawing.DrawLabel(SelectLabel(m_Label, meta), message.transform.translation.From <FLU>(), SelectColor(m_Color, meta), m_Size);
}
public override void Draw(Drawing3d drawing, TransformMsg message, MessageMetadata meta)
{
Draw <FLU>(message, drawing, m_Size, m_DrawUnityAxes);
drawing.DrawLabel(SelectLabel(m_Label, meta), message.translation.From <FLU>(), SelectColor(m_Color, meta), m_Size);
}
public override void Draw(Drawing3d drawing, QuaternionStampedMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
QuaternionDefaultVisualizer.Draw <FLU>(message.quaternion, drawing, m_DrawAtPosition, m_Size, m_DrawUnityAxes);
drawing.DrawLabel(SelectLabel(m_Label, meta), transform.position, SelectColor(m_Color, meta), m_Size);
}
public static void Draw <C>(Point32Msg message, Drawing3d drawing, Color color, string label, float size = 0.01f) where C : ICoordinateSpace, new()
{
drawing.DrawPoint(message.From <C>(), color, size);
drawing.DrawLabel(label, message.From <C>(), color, size * 1.5f);
}