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