private void DrawJoint(KinematicJoint joint) { Vector3 connection = joint.GetAnchorInWorldSpace(); //DrawSphere(connection, JointSize, JointColor); DrawCube(connection, joint.transform.rotation * Quaternion.Euler(joint.GetOrientation()), JointSize, JointColor); DrawLine(joint.transform.position, joint.GetAnchorInWorldSpace(), JointColor); //GUIStyle style = new GUIStyle(); //style.normal.textColor = Color.black; //Handles.Label(connection, joint.name, style); if (joint.GetXMotion().IsEnabled()) { Handles.color = Color.red; Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetXMotion().Axis), ArrowSize); } if (joint.GetYMotion().IsEnabled()) { Handles.color = Color.green; Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetYMotion().Axis), ArrowSize); } if (joint.GetZMotion().IsEnabled()) { Handles.color = Color.blue; Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetZMotion().Axis), ArrowSize); } }
void OnSceneGUI() { //Draw Anchor Vector3 anchor = Target.GetAnchorInWorldSpace(); Handles.color = Color.magenta; Handles.SphereCap(0, anchor, Quaternion.identity, 1 / 100f); Handles.Label(anchor, "Anchor"); //Draw Axes Quaternion rotation = Target.transform.rotation; if (Target.GetXMotion().IsEnabled()) { Handles.color = new Color(1f, 0f, 0f, 0.2f); Vector3 scale = Vector3.zero; if (Target.transform.root != Target.transform) { scale = Target.transform.parent.lossyScale; } if (Target.GetJointType() == JointType.Prismatic) { Vector3 pivot = anchor - Vector3.Scale(rotation * Quaternion.Euler(Target.GetOrientation()) * new Vector3(Target.GetXMotion().GetCurrentValue(), Target.GetYMotion().GetCurrentValue(), Target.GetZMotion().GetCurrentValue()), scale); Vector3 A = pivot + Vector3.Scale(Target.GetXMotion().GetLowerLimit() * (rotation * Target.GetXMotion().Axis), scale); Vector3 B = pivot + Vector3.Scale(Target.GetXMotion().GetUpperLimit() * (rotation * Target.GetXMotion().Axis), scale); Handles.DrawLine(anchor, A); Handles.CubeCap(0, A, rotation, 0.025f); Handles.DrawLine(anchor, B); Handles.CubeCap(0, B, rotation, 0.025f); } else { float lowerLimit = Mathf.Rad2Deg * Target.GetXMotion().GetLowerLimit(); float upperLimit = Mathf.Rad2Deg * Target.GetXMotion().GetUpperLimit(); Handles.DrawSolidArc(anchor, rotation * Target.GetXMotion().Axis, Quaternion.AngleAxis(lowerLimit, rotation * Target.GetXMotion().Axis) * rotation * Target.GetZMotion().Axis, upperLimit - lowerLimit, 0.2f); } Handles.color = Color.red; } else { Handles.color = Color.grey; } Handles.ArrowCap(0, anchor, rotation * Quaternion.LookRotation(Target.GetXMotion().Axis), 0.25f); if (Target.GetYMotion().IsEnabled()) { Handles.color = new Color(0f, 1f, 0f, 0.2f); Vector3 scale = Vector3.zero; if (Target.transform.root != Target.transform) { scale = Target.transform.parent.lossyScale; } if (Target.GetJointType() == JointType.Prismatic) { Vector3 pivot = anchor - Vector3.Scale(rotation * Quaternion.Euler(Target.GetOrientation()) * new Vector3(Target.GetXMotion().GetCurrentValue(), Target.GetYMotion().GetCurrentValue(), Target.GetZMotion().GetCurrentValue()), scale); Vector3 A = pivot + Vector3.Scale(Target.GetYMotion().GetLowerLimit() * (rotation * Target.GetYMotion().Axis), scale); Vector3 B = pivot + Vector3.Scale(Target.GetYMotion().GetLowerLimit() * (rotation * Target.GetYMotion().Axis), scale); Handles.DrawLine(anchor, A); Handles.CubeCap(0, A, rotation, 0.025f); Handles.DrawLine(anchor, B); Handles.CubeCap(0, B, rotation, 0.025f); } else { float lowerLimit = Mathf.Rad2Deg * Target.GetYMotion().GetLowerLimit(); float upperLimit = Mathf.Rad2Deg * Target.GetYMotion().GetUpperLimit(); Handles.DrawSolidArc(anchor, rotation * Target.GetYMotion().Axis, Quaternion.AngleAxis(lowerLimit, rotation * Target.GetYMotion().Axis) * rotation * Target.GetXMotion().Axis, upperLimit - lowerLimit, 0.2f); } Handles.color = Color.green; } else { Handles.color = Color.grey; } Handles.ArrowCap(0, anchor, rotation * Quaternion.LookRotation(Target.GetYMotion().Axis), 0.25f); if (Target.GetZMotion().IsEnabled()) { Handles.color = new Color(0f, 0f, 1f, 0.2f); Vector3 scale = Vector3.zero; if (Target.transform.root != Target.transform) { scale = Target.transform.parent.lossyScale; } if (Target.GetJointType() == JointType.Prismatic) { Vector3 pivot = anchor - Vector3.Scale(rotation * Quaternion.Euler(Target.GetOrientation()) * new Vector3(Target.GetXMotion().GetCurrentValue(), Target.GetYMotion().GetCurrentValue(), Target.GetZMotion().GetCurrentValue()), scale); Vector3 A = pivot + Vector3.Scale(Target.GetZMotion().GetLowerLimit() * (rotation * Target.GetZMotion().Axis), scale); Vector3 B = pivot + Vector3.Scale(Target.GetZMotion().GetUpperLimit() * (rotation * Target.GetZMotion().Axis), scale); Handles.DrawLine(anchor, A); Handles.CubeCap(0, A, rotation, 0.025f); Handles.DrawLine(anchor, B); Handles.CubeCap(0, B, rotation, 0.025f); } else { float lowerLimit = Mathf.Rad2Deg * Target.GetZMotion().GetLowerLimit(); float upperLimit = Mathf.Rad2Deg * Target.GetZMotion().GetUpperLimit(); Handles.DrawSolidArc(anchor, rotation * Target.GetZMotion().Axis, Quaternion.AngleAxis(lowerLimit, rotation * Target.GetZMotion().Axis) * rotation * Target.GetYMotion().Axis, upperLimit - lowerLimit, 0.2f); } Handles.color = Color.blue; } else { Handles.color = Color.grey; } Handles.ArrowCap(0, anchor, rotation * Quaternion.LookRotation(Target.GetZMotion().Axis), 0.25f); }