private static void DrawHingeJoint(HingeJoint joint)
{
Vector3 target = GetAnchor(joint, JointBody.Target);
Vector3 anchor = GetAnchor(joint, JointBody.Anchor);
Vector3 center = target;
Rigidbody rigidbody = joint.GetBody(JointBody.Target);
if (rigidbody != null)
{
center = rigidbody.SceneObject.Position;
}
Gizmos.Color = Color.White;
Gizmos.DrawSphere(center, 0.05f);
Gizmos.Color = Color.Yellow;
Gizmos.DrawSphere(target, 0.05f);
Gizmos.DrawSphere(anchor, 0.05f);
Gizmos.Color = Color.Green;
Gizmos.DrawLine(target, center);
const float radius = 0.25f;
const float height = 0.5f;
if (joint.HasFlag(HingeJointFlag.Limit))
{
Gizmos.Transform = joint.SceneObject.WorldTransform;
LimitAngularRange limit = joint.Limit;
Action <float> drawLimitedArc = x =>
{
Degree lower = MathEx.WrapAngle(limit.lower);
Degree upper = MathEx.WrapAngle(limit.upper);
lower = MathEx.Min(lower, upper);
upper = MathEx.Max(upper, lower);
// Arc zero to lower limit
Gizmos.Color = Color.Red;
Gizmos.DrawWireArc(Vector3.XAxis * x, Vector3.XAxis, radius, new Degree(0.0f), lower);
// Arc lower to upper limit
Degree validRange = upper - lower;
Gizmos.Color = Color.Green;
Gizmos.DrawWireArc(Vector3.XAxis * x, Vector3.XAxis, radius, lower, validRange);
// Arc upper to full circle
Degree remainingRange = new Degree(360) - upper;
Gizmos.Color = Color.Red;
Gizmos.DrawWireArc(Vector3.XAxis * x, Vector3.XAxis, radius, upper, remainingRange);
};
drawLimitedArc(-height);
drawLimitedArc(height);
}
else
{
Gizmos.Color = Color.Green;
Gizmos.Transform = joint.SceneObject.WorldTransform;
Gizmos.DrawWireDisc(Vector3.XAxis * -height, Vector3.XAxis, radius);
Gizmos.DrawWireDisc(Vector3.XAxis * height, Vector3.XAxis, radius);
}
Vector3[] lineStartPoints = new Vector3[4];
lineStartPoints[0] = new Vector3(-height, radius, 0);
lineStartPoints[1] = new Vector3(-height, -radius, 0);
lineStartPoints[2] = new Vector3(-height, 0, radius);
lineStartPoints[3] = new Vector3(-height, 0, -radius);
Vector3[] lineEndPoints = new Vector3[4];
lineEndPoints[0] = new Vector3(height, radius, 0);
lineEndPoints[1] = new Vector3(height, -radius, 0);
lineEndPoints[2] = new Vector3(height, 0, radius);
lineEndPoints[3] = new Vector3(height, 0, -radius);
Gizmos.Color = Color.Green;
for (int i = 0; i < 4; i++)
{
Gizmos.DrawLine(lineStartPoints[i], lineEndPoints[i]);
}
}