public static void DrawRotationArrow(Drawing3d drawing, Vector3 rotationAxis, float rotationDegrees, Vector3 sphereCenter, Color32 color, float sphereRadius = 1.0f, float arrowThickness = 0.01f)
{
Vector3 startVector = Vector3.Cross(Vector3.up, rotationAxis);
if (startVector.sqrMagnitude < 0.01f)
{
startVector = Vector3.forward;
}
float degreesPerStep = 10.0f; // approximately
int numSteps = Mathf.Max((int)(rotationDegrees / degreesPerStep), 2);
float pushOutPerStep = 0;
if (rotationDegrees > 360)
{
float pushOutFinal = arrowThickness * 6 * rotationDegrees / 360;
pushOutPerStep = pushOutFinal / numSteps;
}
Quaternion deltaRotation = Quaternion.AngleAxis(rotationDegrees / (float)numSteps, rotationAxis);
List <Vector3> points = new List <Vector3>();
Quaternion currentRotation = Quaternion.LookRotation(startVector);
for (int step = 0; step < numSteps; ++step)
{
points.Add(sphereCenter + currentRotation * Vector3.forward * sphereRadius);
currentRotation = deltaRotation * currentRotation;
sphereRadius += pushOutPerStep;
}
drawing.DrawLineStrip(points.ToArray(), color, arrowThickness);
drawing.DrawArrow(points[points.Count - 1], sphereCenter + currentRotation * Vector3.forward * sphereRadius, color, arrowThickness);
}
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 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;
}
}
