/// <summary>
/// Visualizes the constraints of the ragdoll (for debugging).
/// </summary>
/// <param name="debugRenderer">The debug renderer.</param>
/// <param name="ragdoll">The ragdoll.</param>
/// <param name="scale">
/// A scale factor that determines the size of the drawn elements.
/// </param>
/// <param name="drawOverScene">
/// If set to <see langword="true"/> the object is drawn over the graphics scene (depth-test
/// disabled).
/// </param>
/// <remarks>
/// Currently, only <see cref="TwistSwingLimit" />s and <see cref="AngularLimit" />s are
/// supported.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="debugRenderer" /> or <paramref name="ragdoll" /> is <see langword="null" />.
/// </exception>
public static void DrawConstraints(this DebugRenderer debugRenderer, Ragdoll ragdoll, float scale, bool drawOverScene)
{
if (debugRenderer == null)
{
throw new ArgumentNullException("debugRenderer");
}
if (ragdoll == null)
{
throw new ArgumentNullException("ragdoll");
}
// Render information for each limit.
foreach (Constraint limit in ragdoll.Limits)
{
// Get the ball joint constraint that connects the two bodies of the limit.
BallJoint joint = null;
foreach (Constraint constraint in ragdoll.Joints)
{
if (constraint.BodyA == limit.BodyA && constraint.BodyB == limit.BodyB ||
constraint.BodyA == limit.BodyB && constraint.BodyB == limit.BodyA)
{
joint = constraint as BallJoint;
break;
}
}
// Skip this limit if no joint was found.
if (joint == null)
{
continue;
}
TwistSwingLimit twistSwingLimit = limit as TwistSwingLimit;
if (twistSwingLimit != null)
{
DrawTwistSwingLimit(debugRenderer, joint, twistSwingLimit, scale, drawOverScene);
continue;
}
AngularLimit angularLimit = limit as AngularLimit;
if (angularLimit != null)
{
DrawAngularLimit(debugRenderer, joint, angularLimit, scale, drawOverScene);
continue;
}
}
}
private static void AddTwistSwingLimit(Ragdoll ragdoll, Skeleton skeleton, AvatarBone parentBone, AvatarBone childBone, Matrix33F orientationA, Matrix33F orientationB, Vector3F minimum, Vector3F maximum)
{
int parentIndex = (int)parentBone;
int childIndex = (int)childBone;
// The difficult part is to define the constraint anchor orientation.
// Here is how we do it:
// When we look at the front side of an Avatar in bind pose, the x-axis is parallel
// to the arms. y points up and z is normal to the those axes.
//
// To define orientationA/B:
// The anchor x-axis is the twist axis. That means, this is already the correct axis
// for the hands (wrist joints) and orientationA/B are therefore Matrix33F.Identity.
// For the Head, the twist axis must point up. Therefore orientationA/B must be a 90°
// rotation about z to rotate the twist axis up.
// For the shoulder-elbow connection, orientationA is Matrix.Identity. The swing cone must
// not be parallel to the arm axis (because the elbow cannot bend backwards). Therefore,
// orientationB defines a rotation that rotates the twist axis (= swing cone center) to the
// front.
//
// To define AnchorOrientationALocal/AnchorOrientationBLocal:
// AnchorOrientationALocal must be a rotation matrix that transforms a vector from local
// constraint anchor space to local body space of A.
// orientationA defines the constraint anchor orientation in model space.
// With jointPosesAbsolute[boneAIndex].Orientation.Transposed, we convert from model space
// to joint space. With ragdoll.BodyOffsets[boneAIndex].Orientation.Transposed, we convert from joint
// space to body space. The combined rotation matrix converts from constraint anchor space
// to body space.
var limit = new TwistSwingLimit
{
BodyA = ragdoll.Bodies[parentIndex],
BodyB = ragdoll.Bodies[childIndex],
AnchorOrientationALocal = ragdoll.BodyOffsets[parentIndex].Orientation.Transposed * skeleton.GetBindPoseAbsoluteInverse(parentIndex).Rotation.ToRotationMatrix33() * orientationA,
AnchorOrientationBLocal = ragdoll.BodyOffsets[childIndex].Orientation.Transposed * skeleton.GetBindPoseAbsoluteInverse(childIndex).Rotation.ToRotationMatrix33() * orientationB,
Minimum = minimum,
Maximum = maximum,
ErrorReduction = 0.2f,
Softness = 0.001f
};
ragdoll.Limits[childIndex] = limit;
}
/// <summary>
/// Adds a TwistSwingLimit between the specified bones.
/// </summary>
/// <param name="ragdoll">The ragdoll.</param>
/// <param name="parent">The parent bone.</param>
/// <param name="child">The child bone.</param>
/// <param name="parentAnchorOrientationLocal">The constraint anchor orientation relative to the parent bone.</param>
/// <param name="childAnchorOrientationLocal">The constraint anchor orientation relative to the child bone.</param>
/// <param name="minimum">The minimum limits (twist/swing/swing).</param>
/// <param name="maximum">The maximum limits (twist/swing/swing).</param>
private static void AddTwistSwingLimit(Ragdoll ragdoll, int parent, int child, Matrix33F parentAnchorOrientationLocal, Matrix33F childAnchorOrientationLocal, Vector3F minimum, Vector3F maximum)
{
var childBody = ragdoll.Bodies[child];
var childOffset = ragdoll.BodyOffsets[child];
var parentBody = ragdoll.Bodies[parent];
var parentOffset = ragdoll.BodyOffsets[parent];
var limit = new TwistSwingLimit
{
BodyA = parentBody,
BodyB = childBody,
AnchorOrientationALocal = parentOffset.Orientation.Transposed * parentAnchorOrientationLocal,
AnchorOrientationBLocal = childOffset.Orientation.Transposed * childAnchorOrientationLocal,
Minimum = minimum,
Maximum = maximum,
ErrorReduction = 0.2f,
Softness = 0.001f
};
ragdoll.Limits.Add(limit);
}
/// <summary>
/// Visualizes the <see cref="TwistSwingLimit"/> of a <see cref="BallJoint"/>.
/// </summary>
/// <param name="debugRenderer">The debug renderer.</param>
/// <param name="joint">The joint.</param>
/// <param name="limit">The limit.</param>
/// <param name="scale">
/// A scale factor that determines the size of the drawn elements.
/// </param>
/// <param name="drawOverScene">
/// If set to <see langword="true"/> the object is drawn over the graphics scene (depth-test
/// disabled).
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="debugRenderer" />, <paramref name="joint" />, or <paramref name="limit" />
/// is <see langword="null" />.
/// </exception>
public static void DrawTwistSwingLimit(this DebugRenderer debugRenderer, BallJoint joint, TwistSwingLimit limit, float scale, bool drawOverScene)
{
if (debugRenderer == null)
{
throw new ArgumentNullException("debugRenderer");
}
if (joint == null)
{
throw new ArgumentNullException("joint");
}
if (limit == null)
{
throw new ArgumentNullException("limit");
}
// ----- Draw swing cone.
// The tip of the swing cone:
Vector3 coneTip = joint.BodyA.Pose.ToWorldPosition(joint.AnchorPositionALocal);
// The first point on the swing cone:
var previousConePoint = limit.GetPointOnCone(0, coneTip, scale);
// Draw swing cone.
const int numberOfSegments = 24;
const float segmentAngle = ConstantsF.TwoPi / numberOfSegments;
Color color = Color.Violet;
for (int i = 0; i < numberOfSegments; i++)
{
var conePoint = limit.GetPointOnCone((i + 1) * segmentAngle, coneTip, scale);
// Line from cone tip to cone base.
debugRenderer.DrawLine(coneTip, conePoint, color, drawOverScene);
// Line on the cone base.
debugRenderer.DrawLine(previousConePoint, conePoint, color, drawOverScene);
previousConePoint = conePoint;
}
// ----- Draw twist axis.
// The x-axis is the twist direction.
Vector3 twistAxis = Vector3.UnitX;
// The twist axis relative to body B.
Vector3 twistAxisDirectionBLocal = limit.AnchorOrientationBLocal * twistAxis;
// The twist axis relative to world space.
Vector3 twistAxisDirection = limit.BodyB.Pose.ToWorldDirection(twistAxisDirectionBLocal);
// (A similar computation is used in DrawArc() below.)
// Line in twist direction.
debugRenderer.DrawLine(coneTip, coneTip + twistAxisDirection * scale, Color.Red, drawOverScene);
// A transformation that converts from constraint anchor space to world space.
Pose constraintToWorld = limit.BodyA.Pose * new Pose(limit.AnchorOrientationALocal);
// Draw an arc that visualizes the twist limits.
DrawArc(debugRenderer, constraintToWorld, coneTip, Vector3.UnitX, Vector3.UnitY, limit.Minimum.X, limit.Maximum.X, scale, Color.Red, drawOverScene);
}
public ConstraintSample3(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// ----- HingeJoint with AngularVelocityMotor
// A board is fixed on a pole like a wind wheel.
// An AngularVelocityMotor creates a rotation with constant angular velocity around the
// hinge axis.
RigidBody box0 = new RigidBody(new BoxShape(0.1f, 2f, 0.1f))
{
Pose = new Pose(new Vector3F(-2, 1, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box0);
RigidBody box1 = new RigidBody(new BoxShape(0.1f, 0.4f, 1f))
{
Pose = new Pose(new Vector3F(-2 + 0.05f, 1.8f, 0))
};
Simulation.RigidBodies.Add(box1);
HingeJoint hingeJoint = new HingeJoint
{
BodyA = box0,
BodyB = box1,
AnchorPoseALocal = new Pose(new Vector3F(0.05f, 0.8f, 0)),
AnchorPoseBLocal = new Pose(new Vector3F(-0.05f, 0, 0)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(hingeJoint);
AngularVelocityMotor angularVelocityMotor = new AngularVelocityMotor
{
BodyA = box0,
// The rotation axis is the local x axis of BodyA.
AxisALocal = Vector3F.UnitX,
BodyB = box1,
TargetVelocity = 10,
// The motor power is limit, so that the rotation can be stopped by other objects blocking
// the movement.
MaxForce = 10000,
// The HingeJoint controls all other axes. So this motor must only act on the hinge
// axis.
UseSingleAxisMode = true,
CollisionEnabled = false,
};
Simulation.Constraints.Add(angularVelocityMotor);
// ----- A PrismaticJoint with a LinearVelocityMotor.
RigidBody box2 = new RigidBody(new BoxShape(0.7f, 0.7f, 0.7f))
{
Pose = new Pose(new Vector3F(0, 2, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box2);
RigidBody box3 = new RigidBody(new BoxShape(0.5f, 1.5f, 0.5f))
{
Pose = new Pose(new Vector3F(0, 1, 0))
};
Simulation.RigidBodies.Add(box3);
_prismaticJoint = new PrismaticJoint
{
BodyA = box2,
BodyB = box3,
AnchorPoseALocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
AnchorPoseBLocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(_prismaticJoint);
_linearVelocityMotor = new LinearVelocityMotor
{
BodyA = box2,
AxisALocal = -Vector3F.UnitY,
BodyB = box3,
TargetVelocity = 1,
CollisionEnabled = false,
MaxForce = 10000,
UseSingleAxisMode = true,
};
Simulation.Constraints.Add(_linearVelocityMotor);
// ----- A BallJoint with a TwistSwingLimit and a QuaternionMotor
// The ball joint connects a cylinder to a static box.
// The twist-swing limits rotational movement.
// The quaternion motor acts like a spring that controls the angle of joint.
RigidBody box4 = new RigidBody(new BoxShape(0.5f, 0.5f, 0.5f))
{
Pose = new Pose(new Vector3F(2, 2, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box4);
RigidBody cylinder0 = new RigidBody(new CylinderShape(0.1f, 0.75f))
{
Pose = new Pose(new Vector3F(2, 2 - 0.75f / 2 - 0.25f, 0))
};
Simulation.RigidBodies.Add(cylinder0);
_ballJoint = new BallJoint
{
BodyA = box4,
BodyB = cylinder0,
AnchorPositionALocal = new Vector3F(0, -0.25f, 0),
AnchorPositionBLocal = new Vector3F(0, 0.75f / 2, 0),
CollisionEnabled = false,
};
Simulation.Constraints.Add(_ballJoint);
_twistSwingLimit = new TwistSwingLimit
{
BodyA = box4,
// The first column is the twist axis (-y). The other two columns are the swing axes.
AnchorOrientationALocal = new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1),
BodyB = cylinder0,
AnchorOrientationBLocal = new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1),
CollisionEnabled = false,
// The twist is limited to +/- 10°. The swing limits are +/- 40° and +/- 60°. This creates
// a deformed cone that limits the swing movements (see visualization).
Minimum = new Vector3F(-MathHelper.ToRadians(10), -MathHelper.ToRadians(40), -MathHelper.ToRadians(60)),
Maximum = new Vector3F(MathHelper.ToRadians(10), MathHelper.ToRadians(40), MathHelper.ToRadians(60)),
};
Simulation.Constraints.Add(_twistSwingLimit);
QuaternionMotor quaternionMotor = new QuaternionMotor
{
BodyA = box4,
AnchorOrientationALocal = Matrix33F.Identity,
BodyB = cylinder0,
AnchorOrientationBLocal = Matrix33F.Identity,
CollisionEnabled = false,
// The QuaternionMotor controls the orientation of the second body relative to the first
// body. Here, we define that the cylinder should swing 30° away from the default
// orientation.
TargetOrientation = QuaternionF.CreateRotationZ(MathHelper.ToRadians(30)),
// Position and orientation motors are similar to damped-springs. We can control
// the stiffness and damping of the spring. (It is also possible to set the SpringConstant
// to 0 if the QuaternionMotor should only act as a rotational damping.)
SpringConstant = 100,
DampingConstant = 20,
};
Simulation.Constraints.Add(quaternionMotor);
}
