private void VisualizeKinematicJoint(KinematicJoint joint)
{
Vector3 connection = joint.ComputeConnectionInWorldSpace();
Handles.color = Color.magenta;
Handles.SphereCap(0, connection, Quaternion.identity, 1 / 100f);
GUIStyle style = new GUIStyle();
style.normal.textColor = Color.black;
Handles.Label(connection, joint.name, style);
if (joint.XMotion.State == JointState.Free)
{
Handles.color = Color.red;
Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.ComputeXAxis()), 0.1f);
}
if (joint.YMotion.State == JointState.Free)
{
Handles.color = Color.green;
Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.ComputeYAxis()), 0.1f);
}
if (joint.ZMotion.State == JointState.Free)
{
Handles.color = Color.blue;
Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.ComputeZAxis()), 0.1f);
}
}
private void DrawKinematicChain(Chain chain)
{
//Visualize Joints and Kinematic Chain
if (chain.Joints.Length > 0)
{
KinematicJoint reference = chain.Joints[0];
VisualizeKinematicJoint(reference);
for (int k = 1; k < chain.Joints.Length; k++)
{
Handles.color = Color.cyan;
Handles.DrawLine(reference.ComputeConnectionInWorldSpace(), chain.Joints[k].ComputeConnectionInWorldSpace());
reference = chain.Joints[k];
VisualizeKinematicJoint(reference);
}
Handles.color = Color.cyan;
Handles.DrawLine(chain.Joints[chain.Joints.Length - 1].ComputeConnectionInWorldSpace(), chain.Segments[chain.Segments.Length - 1].transform.position);
Handles.color = new Color(0.25f, 0.25f, 0.25f, 1f);
Handles.SphereCap(0, chain.Segments[chain.Segments.Length - 1].position, chain.Segments[chain.Segments.Length - 1].rotation, 0.01f);
}
}
void OnSceneGUI()
{
//Draw Connection
Vector3 connection = Target.ComputeConnectionInWorldSpace();
Handles.color = Color.magenta;
Handles.SphereCap(0, connection, Quaternion.identity, 1 / 100f);
Handles.Label(connection, "Connection");
//Draw Axes
Quaternion rotation = Quaternion.identity;
if (Application.isPlaying)
{
if (Target.transform.parent != null)
{
rotation = Target.transform.parent.rotation;
}
rotation *= new Quaternion(Target.RRX, Target.RRY, Target.RRZ, Target.RRW);
}
else
{
rotation = Target.transform.rotation;
}
if (Target.XMotion.State == JointState.Free)
{
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.Type == JointType.Prismatic)
{
Vector3 pivot = connection - Vector3.Scale(rotation * Quaternion.Euler(Target.AxisOrientation) * Target.GetCurrentValue(), scale);
Vector3 A = pivot + Vector3.Scale(Target.XMotion.GetLowerLimit() * (rotation * Target.ComputeXAxis()), scale);
Vector3 B = pivot + Vector3.Scale(Target.XMotion.GetUpperLimit() * (rotation * Target.ComputeXAxis()), scale);
Handles.DrawLine(connection, A);
Handles.CubeCap(0, A, rotation, 0.025f);
Handles.DrawLine(connection, B);
Handles.CubeCap(0, B, rotation, 0.025f);
}
else
{
float lowerLimit = Mathf.Rad2Deg * Target.XMotion.GetLowerLimit();
float upperLimit = Mathf.Rad2Deg * Target.XMotion.GetUpperLimit();
Handles.DrawSolidArc(connection, rotation * Target.ComputeXAxis(), Quaternion.AngleAxis(lowerLimit, rotation * Target.ComputeXAxis()) * rotation * Target.ComputeZAxis(), upperLimit - lowerLimit, 0.075f);
}
Handles.color = Color.red;
}
else
{
Handles.color = Color.grey;
}
Handles.ArrowCap(0, connection, rotation * Quaternion.LookRotation(Target.ComputeXAxis()), 0.1f);
if (Target.YMotion.State == JointState.Free)
{
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.Type == JointType.Prismatic)
{
Vector3 pivot = connection - Vector3.Scale(rotation * Quaternion.Euler(Target.AxisOrientation) * Target.GetCurrentValue(), scale);
Vector3 A = pivot + Vector3.Scale(Target.YMotion.GetLowerLimit() * (rotation * Target.ComputeYAxis()), scale);
Vector3 B = pivot + Vector3.Scale(Target.YMotion.GetLowerLimit() * (rotation * Target.ComputeYAxis()), scale);
Handles.DrawLine(connection, A);
Handles.CubeCap(0, A, rotation, 0.025f);
Handles.DrawLine(connection, B);
Handles.CubeCap(0, B, rotation, 0.025f);
}
else
{
float lowerLimit = Mathf.Rad2Deg * Target.YMotion.GetLowerLimit();
float upperLimit = Mathf.Rad2Deg * Target.YMotion.GetUpperLimit();
Handles.DrawSolidArc(connection, rotation * Target.ComputeYAxis(), Quaternion.AngleAxis(lowerLimit, rotation * Target.ComputeYAxis()) * rotation * Target.ComputeXAxis(), upperLimit - lowerLimit, 0.075f);
}
Handles.color = Color.green;
}
else
{
Handles.color = Color.grey;
}
Handles.ArrowCap(0, connection, rotation * Quaternion.LookRotation(Target.ComputeYAxis()), 0.1f);
if (Target.ZMotion.State == JointState.Free)
{
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.Type == JointType.Prismatic)
{
Vector3 pivot = connection - Vector3.Scale(rotation * Quaternion.Euler(Target.AxisOrientation) * Target.GetCurrentValue(), scale);
Vector3 A = pivot + Vector3.Scale(Target.ZMotion.GetLowerLimit() * (rotation * Target.ComputeZAxis()), scale);
Vector3 B = pivot + Vector3.Scale(Target.ZMotion.GetUpperLimit() * (rotation * Target.ComputeZAxis()), scale);
Handles.DrawLine(connection, A);
Handles.CubeCap(0, A, rotation, 0.025f);
Handles.DrawLine(connection, B);
Handles.CubeCap(0, B, rotation, 0.025f);
}
else
{
float lowerLimit = Mathf.Rad2Deg * Target.ZMotion.GetLowerLimit();
float upperLimit = Mathf.Rad2Deg * Target.ZMotion.GetUpperLimit();
Handles.DrawSolidArc(connection, rotation * Target.ComputeZAxis(), Quaternion.AngleAxis(lowerLimit, rotation * Target.ComputeZAxis()) * rotation * Target.ComputeYAxis(), upperLimit - lowerLimit, 0.075f);
}
Handles.color = Color.blue;
}
else
{
Handles.color = Color.grey;
}
Handles.ArrowCap(0, connection, rotation * Quaternion.LookRotation(Target.ComputeZAxis()), 0.1f);
}