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