public void DrawGhost(Drawing3d drawing, JointPlacement[] placements, Color color)
{
foreach (JointPlacement jointPlacement in placements)
{
UrdfJoint joint = jointPlacement.Joint;
int numChildren = joint.transform.childCount;
for (int Idx = 0; Idx < numChildren; ++Idx)
{
Transform child = joint.transform.GetChild(Idx);
if (child.GetComponent <UrdfJoint>()) // don't want to draw the other joints with this jointPlacement
{
continue;
}
foreach (MeshFilter mfilter in child.GetComponentsInChildren <MeshFilter>())
{
Vector3 localMeshOffset = jointPlacement.Rotation * joint.transform.InverseTransformPoint(mfilter.transform.position);
Quaternion localMeshRotation = Quaternion.Inverse(joint.transform.rotation) * mfilter.transform.rotation;
drawing.DrawMesh(
mfilter.sharedMesh,
jointPlacement.Position + localMeshOffset,
jointPlacement.Rotation * localMeshRotation,
Vector3.one,
color
);
}
}
}
}
