/// <summary>
/// Draws two concentric arcs, depending on the <paramref name="thickness"/> parameter.
/// </summary>
/// <param name="center">Position of the arc center</param>
/// <param name="radius">Radius of the outer arc.</param>
/// <param name="arc">Length of the arc in range [0, 360] degrees.</param>
/// <param name="thickness">Determines radius of the inner arc, in range [0, 1]. 0 specifies the inner arc is
/// the same radius as the outer arc, while 1 specifies the inner arc has zero radius.</param>
/// <param name="color">Color to draw the arc with.</param>
private static void DrawArcWithThickness(Vector3 center, float radius, Radian arc, float thickness, Color color)
{
Gizmos.Color = color;
if (arc.Degrees > 0.0f)
{
Gizmos.DrawWireArc(center, -Vector3.ZAxis, radius, new Degree(0.0f), arc);
if (thickness > 0.0f)
{
float innerRadius = radius * (1.0f - MathEx.Clamp01(thickness));
if (thickness < 1.0f)
{
Gizmos.Color = color * new Color(0.5f, 0.5f, 0.5f);
Gizmos.DrawWireArc(center, -Vector3.ZAxis, innerRadius, new Degree(0.0f), arc);
}
Gizmos.Color = color * new Color(0.25f, 0.25f, 0.25f);
for (Radian x = new Radian(0.0f); x < arc; x += MathEx.HalfPi)
{
Vector3 dir = new Vector3(MathEx.Cos(x), MathEx.Sin(x), 0.0f);
Gizmos.DrawLine(center + dir * innerRadius, center + dir * radius);
}
if (!MathEx.ApproxEquals(arc.Degrees % 90.0f, 0.0f))
{
Vector3 dir = new Vector3(MathEx.Cos(arc), MathEx.Sin(arc), 0.0f);
Gizmos.DrawLine(center + dir * innerRadius, center + dir * radius);
}
}
}
}
private static void DrawSphericalJoint(SphericalJoint 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);
Gizmos.Color = Color.Green;
if (joint.HasFlag(SphericalJointFlag.Limit))
{
LimitConeRange limit = joint.Limit;
Radian zAngle = MathEx.Min(new Degree(360), limit.zLimitAngle * 2.0f);
Radian yAngle = MathEx.Min(new Degree(360), limit.yLimitAngle * 2.0f);
Gizmos.Transform = joint.SceneObject.WorldTransform;
Gizmos.DrawWireArc(Vector3.Zero, Vector3.ZAxis, 0.25f, zAngle * -0.5f + new Degree(90), zAngle);
Gizmos.DrawWireArc(Vector3.Zero, Vector3.YAxis, 0.25f, yAngle * -0.5f + new Degree(90), yAngle);
Gizmos.Color = Color.Red;
Radian remainingZAngle = new Degree(360) - zAngle;
Radian remainingYAngle = new Degree(360) - yAngle;
Gizmos.DrawWireArc(Vector3.Zero, Vector3.ZAxis, 0.25f, zAngle * 0.5f + new Degree(90), remainingZAngle);
Gizmos.DrawWireArc(Vector3.Zero, Vector3.YAxis, 0.25f, yAngle * 0.5f + new Degree(90), remainingYAngle);
}
else
{
Gizmos.Color = Color.Green;
Gizmos.Transform = joint.SceneObject.WorldTransform;
Gizmos.DrawWireDisc(Vector3.Zero, Vector3.ZAxis, 0.25f);
Gizmos.DrawWireDisc(Vector3.Zero, Vector3.YAxis, 0.25f);
}
}
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]);
}
}