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