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 static void Draw <C>(PolygonMsg message, Drawing3d drawing, Color color, float thickness = 0.01f) where C : ICoordinateSpace, new()
{
Vector3 prevPos = message.points[message.points.Length - 1].From <C>();
foreach (Point32Msg p in message.points)
{
Vector3 curPos = p.From <C>();
drawing.DrawLine(prevPos, curPos, color, thickness);
prevPos = curPos;
}
}
public static void DrawPointCloud <C>(PointMsg[] points, Drawing3d drawing, Color color, float radius = 0.01f)
where C : ICoordinateSpace, new()
{
PointCloudDrawing pointCloud = drawing.AddPointCloud(points.Length);
foreach (PointMsg p in points)
{
pointCloud.AddPoint(p.From <C>(), color, radius);
}
pointCloud.Bake();
}
public void DrawJointPaths(Drawing3d drawing, JointPlacement[][] jointPlacements, Color color, float pathThickness)
{
for (int pathIdx = 1; pathIdx < jointPlacements.Length; ++pathIdx)
{
JointPlacement[] pose1 = jointPlacements[pathIdx - 1];
JointPlacement[] pose2 = jointPlacements[pathIdx];
for (int jointIdx = 0; jointIdx < pose1.Length; ++jointIdx)
{
drawing.DrawLine(pose1[jointIdx].Position, pose2[jointIdx].Position, color, pathThickness);
}
}
}
public void DrawEffort(Drawing3d drawing, JointStateMsg message, Color color)
{
if (message.effort.Length > 0)
{
DrawEffort(drawing, GetJointPlacements(message), color, message.effort);
}
else
{
Debug.Log("This JointState message contains no Effort data!");
return;
}
}
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 void Redraw()
{
if (m_Drawing == null)
{
m_Drawing = Drawing3dManager.CreateDrawing();
}
else
{
m_Drawing.Clear();
}
m_Factory.Draw(this, m_Message, m_Meta);
}
public void OnChanged(TFStream stream)
{
Drawing3d drawing;
if (!drawings.TryGetValue(stream.Name, out drawing))
{
drawing = Drawing3d.Create();
drawings[stream.Name] = drawing;
if (stream.Parent != null)
{
OnChanged(stream.Parent);
Drawing3d parentStream;
if (drawings.TryGetValue(stream.Parent.Name, out parentStream))
{
drawing.transform.parent = parentStream.transform;
}
}
}
TFFrame frame = stream.GetLocalTF();
drawing.transform.localPosition = frame.translation;
drawing.transform.localRotation = frame.rotation;
drawing.Clear();
EnsureSettings(stream);
if (m_ShowAxes[stream])
{
VisualizationUtils.DrawAxisVectors <FLU>(drawing, Vector3.zero.To <FLU>(), Quaternion.identity.To <FLU>(), axesScale, false);
}
if (m_ShowLinks[stream])
{
drawing.DrawLine(Quaternion.Inverse(frame.rotation) * -frame.translation, Vector3.zero, color, lineThickness);
}
if (m_ShowNames[stream])
{
drawing.DrawLabel(stream.Name, Vector3.zero, color);
}
}
public static void DrawAxisVectors <C>(Drawing3d drawing, Vector3Msg position, QuaternionMsg rotation, float size, bool drawUnityAxes) where C : ICoordinateSpace, new()
{
Vector3 unityPosition = position.From <C>();
Quaternion unityRotation = rotation.From <C>();
Vector3 x, y, z;
if (drawUnityAxes)
{
x = unityRotation * new Vector3(1, 0, 0) * size;
y = unityRotation * new Vector3(0, 1, 0) * size;
z = unityRotation * new Vector3(0, 0, 1) * size;
}
else
{
x = unityRotation * new Vector3 <C>(1, 0, 0).toUnity *size;
y = unityRotation * new Vector3 <C>(0, 1, 0).toUnity *size;
z = unityRotation * new Vector3 <C>(0, 0, 1).toUnity *size;
}
drawing.DrawLine(unityPosition, unityPosition + x, Color.red, size * 0.1f);
drawing.DrawLine(unityPosition, unityPosition + y, Color.green, size * 0.1f);
drawing.DrawLine(unityPosition, unityPosition + z, Color.blue, size * 0.1f);
}
public override void Draw(Drawing3d drawing, Vector3StampedMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
Vector3DefaultVisualizer.Draw <FLU>(message.vector, drawing, SelectColor(m_Color, meta), SelectLabel(m_Label, meta), m_Radius);
}
public void DrawJointPath(Drawing3d drawing, JointPlacement[][] jointPlacements, int jointIndex, Color color, float pathThickness)
{
drawing.DrawLineStrip(jointPlacements.Select(p => p[jointIndex].Position).ToArray(), color, pathThickness);
}
public void DrawGhost(Drawing3d drawing, MultiDOFJointStateMsg message, Color color)
{
DrawGhost(drawing, GetJointPlacements(message), color);
}
public void DrawGhost(Drawing3d drawing, JointTrajectoryPointMsg message, string[] jointNames, Color color)
{
DrawGhost(drawing, GetJointPlacements(message, jointNames), color);
}
public void DrawGhost(Drawing3d drawing, JointTrajectoryMsg message, int pointIndex, Color color)
{
DrawGhost(drawing, GetJointPlacements(message.points[pointIndex], message.joint_names), color);
}
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, PointMsg message, MessageMetadata meta)
{
Draw <FLU>(message, drawing, SelectColor(m_Color, meta), SelectLabel(m_Label, meta), m_Radius);
}
public static void Draw <C>(TransformMsg transform, Drawing3d drawing, float size = 0.01f, bool drawUnityAxes = false) where C : ICoordinateSpace, new()
{
QuaternionDefaultVisualizer.Draw <C>(transform.rotation, drawing, transform.translation.From <C>(), size, drawUnityAxes);
}
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 void AddDirty(Drawing3d drawing)
{
m_Dirty.Add(drawing);
}
public override void Draw(Drawing3d drawing, WrenchStampedMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
WrenchDefaultVisualizer.Draw <FLU>(message.wrench, drawing, SelectColor(m_Color, meta), origin.transform.position, lengthScale, sphereRadius, thickness);
}
public static void Draw <C>(QuaternionMsg message, Drawing3d drawing, Vector3 position, float size = 0.1f, bool drawUnityAxes = false)
where C : ICoordinateSpace, new()
{
VisualizationUtils.DrawAxisVectors <C>(drawing, position.To <C>(), message, size, drawUnityAxes);
}
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 override void Draw(Drawing3d drawing, TwistMsg message, MessageMetadata meta)
{
var orig = origin == null ? Vector3.zero : origin.transform.position;
Draw <FLU>(message, drawing, SelectColor(m_Color, meta), orig, lengthScale, sphereRadius, thickness);
}
public void DrawJointPath(Drawing3d drawing, JointTrajectoryMsg message, int jointIndex, Color color, float pathThickness)
{
DrawJointPath(drawing, GetJointPlacements(message), jointIndex, color, pathThickness);
}
public static void Draw <C>(TwistMsg message, Drawing3d drawing, Color color, Vector3 origin, float lengthScale = 1, float sphereRadius = 1, float thickness = 0.01f) where C : ICoordinateSpace, new()
{
drawing.DrawArrow(origin, origin + message.linear.From <C>() * lengthScale, color, thickness);
VisualizationUtils.DrawAngularVelocityArrow(drawing, message.angular.From <C>(), origin, color, sphereRadius, thickness);
}
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 static void Draw <C>(PointMsg message, Drawing3d drawing, Color color, float size = 0.01f) where C : ICoordinateSpace, new()
{
drawing.DrawPoint(message.From <C>(), color, size);
}
public override void Draw(Drawing3d drawing, PolygonMsg message, MessageMetadata meta)
{
Draw <FLU>(message, drawing, SelectColor(m_Color, meta), m_Thickness);
}
public void DestroyDrawing(Drawing3d drawing)
{
m_Drawings.Remove(drawing);
GameObject.Destroy(drawing.gameObject);
}
public static void DrawAngularVelocityArrow(Drawing3d drawing, Vector3 angularVelocity, Vector3 sphereCenter, Color32 color, float sphereRadius = 1.0f, float arrowThickness = 0.01f)
{
DrawRotationArrow(drawing, angularVelocity.normalized, angularVelocity.magnitude * Mathf.Rad2Deg, sphereCenter, color, sphereRadius, arrowThickness);
}
