/// <summary>
/// Drives the controlled body.
/// </summary>
/// <param name="skeletonPose">The target skeleton pose.</param>
/// <param name="deltaTime">The time step.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose" /> is <see langword="null"/>.
/// </exception>
internal void DriveToPose(SkeletonPose skeletonPose, float deltaTime)
{
if (skeletonPose == null)
{
throw new ArgumentNullException("skeletonPose");
}
Debug.Assert(Ragdoll != null, "Motor must not be called when Ragdoll property is null.");
Debug.Assert(Ragdoll.Simulation != null, "Ragdoll was not added to a simulation.");
if (!Enabled)
{
return;
}
if (Mode == RagdollMotorMode.Velocity)
{
// ----- Velocity motor
Debug.Assert(_quaternionMotor.Simulation == null, "Velocity motors should not be added to the simulation.");
if (BoneIndex >= Ragdoll.Bodies.Count)
{
return;
}
var body = Ragdoll.Bodies[BoneIndex];
if (body == null)
{
return;
}
var childOffset = (BoneIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[BoneIndex] : Pose.Identity;
var childPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(BoneIndex)) * childOffset;
// Wake the body up. It should move, even if we move it very slowly.
body.WakeUp();
// Set velocities.
body.LinearVelocity = AnimationHelper.ComputeLinearVelocity(body.Pose.Position, childPose.Position, deltaTime);
body.AngularVelocity = AnimationHelper.ComputeAngularVelocity(body.Pose.Orientation, childPose.Orientation, deltaTime);
}
else
{
// ----- Constraint motor
if (_quaternionMotor.Simulation == null)
{
// The motor was not added to the simulation. (Invalid configuration)
return;
}
var parentOffset = (ParentIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[ParentIndex] : Pose.Identity;
var parentPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(ParentIndex)) * parentOffset;
var childOffset = (BoneIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[BoneIndex] : Pose.Identity;
var childPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(BoneIndex)) * childOffset;
// Set the relative motor target.
var rotationMatrix = parentPose.Orientation.Transposed * childPose.Orientation;
_quaternionMotor.TargetOrientation = Quaternion.CreateFromRotationMatrix(rotationMatrix);
}
}
/// <summary>
/// Updates the pose of a single body, so that the bodies match the bone transforms of the given
/// bone.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="boneIndex">The index of the bone.</param>
/// <remarks>
/// See also <see cref="UpdateBodiesFromSkeleton(SkeletonPose)"/>.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose" /> is <see langword="null"/>.
/// </exception>
public void UpdateBodyFromSkeleton(SkeletonPose skeletonPose, int boneIndex)
{
if (skeletonPose == null)
{
throw new ArgumentNullException("skeletonPose");
}
if (boneIndex < 0 || boneIndex >= Bodies.Count)
{
return;
}
var body = Bodies[boneIndex];
if (body == null)
{
return;
}
Pose offset = (boneIndex < BodyOffsets.Count) ? BodyOffsets[boneIndex] : Pose.Identity;
Pose bodyPose = Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(boneIndex)) * offset;
body.Pose = bodyPose;
body.LinearVelocity = Vector3F.Zero;
body.AngularVelocity = Vector3F.Zero;
}
/// <summary>
/// Updates the poses of the bodies, so that the bodies match the bone transforms of the given
/// skeleton pose.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <remarks>
/// The poses of the rigid bodies are changed instantly. The bodies will "teleport" instantly to
/// the target positions. They will not interact correctly with other physics objects. The
/// velocities of the rigid bodies are set to zero. The bodies will be positioned relative to
/// the world space pose defined by <see cref="Pose"/>.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose" /> is <see langword="null"/>.
/// </exception>
public void UpdateBodiesFromSkeleton(SkeletonPose skeletonPose) // = Teleport of bodies.
{
if (skeletonPose == null)
{
throw new ArgumentNullException("skeletonPose");
}
var skeleton = skeletonPose.Skeleton;
for (int i = 0; i < Bodies.Count && i < skeleton.NumberOfBones; i++)
{
var body = Bodies[i];
if (body == null)
{
continue;
}
Pose offset = (i < BodyOffsets.Count) ? BodyOffsets[i] : Pose.Identity;
Pose bodyPose = Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(i)) * offset;
body.Pose = bodyPose;
body.LinearVelocity = Vector3F.Zero;
body.AngularVelocity = Vector3F.Zero;
}
}
public void Update(float deltaTime, Matrix world)
{
if (deltaTime <= 0)
{
return;
}
// Reset bone transform.
SkeletonPose.SetBoneTransform(BoneIndex, SrtTransform.Identity);
// Get new fixed point position in world space.
var bonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(BoneIndex);
var bonePoseWorld = world * bonePoseAbsolute;
var fixedPointPosition = bonePoseWorld.TransformPosition(Offset);
// If we haven't set the fixed point position before, then store the position
// and we are done.
if (_fixedPointPosition.IsNaN)
{
_fixedPointPosition = fixedPointPosition;
return;
}
// New position and velocity of fixed point.
_fixedPointVelocity = (fixedPointPosition - _fixedPointPosition) / deltaTime;
_fixedPointPosition = fixedPointPosition;
// If the particle position was not set before, then we only store the current values.
// The real work starts in the next frame.
if (_particlePosition.IsNaN)
{
_particlePosition = _fixedPointPosition;
_particleVelocity = _fixedPointVelocity;
return;
}
// Compute the spring force between the particle and the fixed point.
var force = Spring * (_fixedPointPosition - _particlePosition) + Damping * (_fixedPointVelocity - _particleVelocity);
// Update velocity and position of the particle using symplectic Euler.
_particleVelocity = _particleVelocity + force * deltaTime;
_particlePosition = _particlePosition + _particleVelocity * deltaTime;
// Convert particle position back to bone space.
var particleLocal = bonePoseWorld.Inverse.TransformPosition(_particlePosition);
// Create rotation between the fixed point vector and the particle vector.
var boneTransform = new SrtTransform(Quaternion.CreateFromRotationMatrix(Offset, particleLocal));
SkeletonPose.SetBoneTransform(BoneIndex, boneTransform);
}
private void CorrectWorldSpacePose()
{
// Notes:
// The Ragdoll class is simply a container for rigid bodies, joints, limits, motors, etc.
// It has a Ragdoll.Pose property that determines the world space pose of the model.
// The Ragdoll class does not update this property. It only reads it.
// Let's say the ragdoll and model are created at the world space origin. Then the user
// grabs the ragdoll and throws it 100 units away. Then the Ragdoll.Pose (and the root bone)
// is still at the origin and the first body (the pelvis) is 100 units away.
// You can observe this if you comment out this method and look at the debug rendering of
// the skeleton.
// To avoid this we correct the Ragdoll.Pose and make sure that it is always near the
// pelvis bone.
var pelvis = _targetSkeletonPose.Skeleton.GetIndex("Pelvis");
// This is different from the PassiveRagdollSample:
// We use the _targetSkeletonPose to define the distance between pelvis and the Pose.
var pelvisBonePoseAbsoluteInverse = _targetSkeletonPose.GetBonePoseAbsolute(pelvis).Inverse;
_ragdoll.Pose = _ragdoll.Bodies[pelvis].Pose * _ragdoll.BodyOffsets[pelvis].Inverse * (Pose)pelvisBonePoseAbsoluteInverse;
_meshNode.PoseWorld = _ragdoll.Pose;
}
/// <summary>
/// Initializes the ragdoll for the given skeleton pose.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="ragdoll">The ragdoll.</param>
/// <param name="simulation">The simulation in which the ragdoll will be used.</param>
/// <param name="scale">A scaling factor to scale the size of the ragdoll.</param>
public static void Create(SkeletonPose skeletonPose, Ragdoll ragdoll, Simulation simulation, float scale)
{
var skeleton = skeletonPose.Skeleton;
const float totalMass = 80; // The total mass of the ragdoll.
const int numberOfBodies = 17;
// Get distance from foot to head as a measure for the size of the ragdoll.
int head = skeleton.GetIndex("Head");
int footLeft = skeleton.GetIndex("L_Ankle1");
var headPosition = skeletonPose.GetBonePoseAbsolute(head).Translation;
var footPosition = skeletonPose.GetBonePoseAbsolute(footLeft).Translation;
var headToFootDistance = (headPosition - footPosition).Length;
// We use the same mass properties for all bodies. This is not realistic but more stable
// because large mass differences or thin bodies (arms!) are less stable.
// We use the mass properties of sphere proportional to the size of the model.
var massFrame = MassFrame.FromShapeAndMass(new SphereShape(headToFootDistance / 8), Vector3F.One, totalMass / numberOfBodies, 0.1f, 1);
var material = new UniformMaterial();
#region ----- Add Bodies and Body Offsets -----
var numberOfBones = skeleton.NumberOfBones;
ragdoll.Bodies.AddRange(Enumerable.Repeat <RigidBody>(null, numberOfBones));
ragdoll.BodyOffsets.AddRange(Enumerable.Repeat(Pose.Identity, numberOfBones));
var pelvis = skeleton.GetIndex("Pelvis");
ragdoll.Bodies[pelvis] = new RigidBody(new BoxShape(0.3f * scale, 0.4f * scale, 0.55f * scale), massFrame, material);
ragdoll.BodyOffsets[pelvis] = new Pose(new Vector3F(0, 0, 0));
var backLower = skeleton.GetIndex("Spine");
ragdoll.Bodies[backLower] = new RigidBody(new BoxShape(0.36f * scale, 0.4f * scale, 0.55f * scale), massFrame, material);
ragdoll.BodyOffsets[backLower] = new Pose(new Vector3F(0.18f * scale, 0, 0));
var backUpper = skeleton.GetIndex("Spine2");
ragdoll.Bodies[backUpper] = new RigidBody(new BoxShape(0.5f * scale, 0.4f * scale, 0.65f * scale), massFrame, material);
ragdoll.BodyOffsets[backUpper] = new Pose(new Vector3F(0.25f * scale, 0, 0));
var neck = skeleton.GetIndex("Neck");
ragdoll.Bodies[neck] = new RigidBody(new CapsuleShape(0.12f * scale, 0.3f * scale), massFrame, material);
ragdoll.BodyOffsets[neck] = new Pose(new Vector3F(0.15f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
ragdoll.Bodies[neck].CollisionObject.Enabled = false;
ragdoll.Bodies[head] = new RigidBody(new SphereShape(0.2f * scale), massFrame, material);
ragdoll.BodyOffsets[head] = new Pose(new Vector3F(0.15f * scale, 0.02f * scale, 0));
var armUpperLeft = skeleton.GetIndex("L_UpperArm");
ragdoll.Bodies[armUpperLeft] = new RigidBody(new CapsuleShape(0.12f * scale, 0.6f * scale), massFrame, material);
ragdoll.BodyOffsets[armUpperLeft] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var armLowerLeft = skeleton.GetIndex("L_Forearm");
ragdoll.Bodies[armLowerLeft] = new RigidBody(new CapsuleShape(0.08f * scale, 0.5f * scale), massFrame, material);
ragdoll.BodyOffsets[armLowerLeft] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var handLeft = skeleton.GetIndex("L_Hand");
ragdoll.Bodies[handLeft] = new RigidBody(new BoxShape(0.2f * scale, 0.06f * scale, 0.15f * scale), massFrame, material);
ragdoll.BodyOffsets[handLeft] = new Pose(new Vector3F(0.1f * scale, 0, 0));
var armUpperRight = skeleton.GetIndex("R_UpperArm");
ragdoll.Bodies[armUpperRight] = new RigidBody(new CapsuleShape(0.12f * scale, 0.6f * scale), massFrame, material);
ragdoll.BodyOffsets[armUpperRight] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var armLowerRight = skeleton.GetIndex("R_Forearm");
ragdoll.Bodies[armLowerRight] = new RigidBody(new CapsuleShape(0.08f * scale, 0.5f * scale), massFrame, material);
ragdoll.BodyOffsets[armLowerRight] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var handRight = skeleton.GetIndex("R_Hand");
ragdoll.Bodies[handRight] = new RigidBody(new BoxShape(0.2f * scale, 0.06f * scale, 0.15f * scale), massFrame, material);
ragdoll.BodyOffsets[handRight] = new Pose(new Vector3F(0.1f * scale, 0, 0));
var legUpperLeft = skeleton.GetIndex("L_Thigh1");
ragdoll.Bodies[legUpperLeft] = new RigidBody(new CapsuleShape(0.16f * scale, 0.8f * scale), massFrame, material);
ragdoll.BodyOffsets[legUpperLeft] = new Pose(new Vector3F(0.4f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var legLowerLeft = skeleton.GetIndex("L_Knee2");
ragdoll.Bodies[legLowerLeft] = new RigidBody(new CapsuleShape(0.12f * scale, 0.65f * scale), massFrame, material);
ragdoll.BodyOffsets[legLowerLeft] = new Pose(new Vector3F(0.32f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
//var footLeft = skeleton.GetIndex("L_Ankle1");
ragdoll.Bodies[footLeft] = new RigidBody(new BoxShape(0.20f * scale, 0.5f * scale, 0.3f * scale), massFrame, material);
ragdoll.BodyOffsets[footLeft] = new Pose(new Vector3F(0.16f * scale, 0.15f * scale, 0));
var legUpperRight = skeleton.GetIndex("R_Thigh");
ragdoll.Bodies[legUpperRight] = new RigidBody(new CapsuleShape(0.16f * scale, 0.8f * scale), massFrame, material);
ragdoll.BodyOffsets[legUpperRight] = new Pose(new Vector3F(0.4f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var legLowerRight = skeleton.GetIndex("R_Knee");
ragdoll.Bodies[legLowerRight] = new RigidBody(new CapsuleShape(0.12f * scale, 0.65f * scale), massFrame, material);
ragdoll.BodyOffsets[legLowerRight] = new Pose(new Vector3F(0.32f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var footRight = skeleton.GetIndex("R_Ankle");
ragdoll.Bodies[footRight] = new RigidBody(new BoxShape(0.20f * scale, 0.5f * scale, 0.3f * scale), massFrame, material);
ragdoll.BodyOffsets[footRight] = new Pose(new Vector3F(0.16f * scale, 0.15f * scale, 0));
#endregion
#region ----- Set Collision Filters -----
// Collisions between connected bodies will be disabled in AddJoint(). (A BallJoint
// has a property CollisionEnabled which decides whether connected bodies can
// collide.)
// But we need to disable some more collision between bodies that are not directly
// connected but still too close to each other.
var filter = (ICollisionFilter)simulation.CollisionDomain.CollisionDetection.CollisionFilter;
filter.Set(ragdoll.Bodies[backUpper].CollisionObject, ragdoll.Bodies[head].CollisionObject, false);
filter.Set(ragdoll.Bodies[armUpperRight].CollisionObject, ragdoll.Bodies[backLower].CollisionObject, false);
filter.Set(ragdoll.Bodies[armUpperLeft].CollisionObject, ragdoll.Bodies[backLower].CollisionObject, false);
filter.Set(ragdoll.Bodies[legUpperLeft].CollisionObject, ragdoll.Bodies[legUpperRight].CollisionObject, false);
#endregion
#region ----- Add Joints -----
AddJoint(skeletonPose, ragdoll, pelvis, backLower);
AddJoint(skeletonPose, ragdoll, backLower, backUpper);
AddJoint(skeletonPose, ragdoll, backUpper, neck);
AddJoint(skeletonPose, ragdoll, neck, head);
AddJoint(skeletonPose, ragdoll, backUpper, armUpperLeft);
AddJoint(skeletonPose, ragdoll, armUpperLeft, armLowerLeft);
AddJoint(skeletonPose, ragdoll, armLowerLeft, handLeft);
AddJoint(skeletonPose, ragdoll, backUpper, armUpperRight);
AddJoint(skeletonPose, ragdoll, armUpperRight, armLowerRight);
AddJoint(skeletonPose, ragdoll, armLowerRight, handRight);
AddJoint(skeletonPose, ragdoll, pelvis, legUpperLeft);
AddJoint(skeletonPose, ragdoll, legUpperLeft, legLowerLeft);
AddJoint(skeletonPose, ragdoll, legLowerLeft, footLeft);
AddJoint(skeletonPose, ragdoll, pelvis, legUpperRight);
AddJoint(skeletonPose, ragdoll, legUpperRight, legLowerRight);
AddJoint(skeletonPose, ragdoll, legLowerRight, footRight);
#endregion
#region ----- Add Limits -----
// Choosing limits is difficult.
// We create hinge limits with AngularLimits in the back and in the knee.
// For all other joints we use TwistSwingLimits with symmetric cones.
AddAngularLimit(skeletonPose, ragdoll, pelvis, backLower, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.3f));
AddAngularLimit(skeletonPose, ragdoll, backLower, backUpper, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.4f));
AddAngularLimit(skeletonPose, ragdoll, backUpper, neck, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.3f));
AddTwistSwingLimit(ragdoll, neck, head, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.5f, -0.7f), new Vector3F(0.1f, 0.5f, 0.7f));
var parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(backUpper).Inverse;
var childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(armUpperLeft).Inverse;
var bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
AddTwistSwingLimit(ragdoll, backUpper, armUpperLeft, bindPoseRelative.Orientation * Matrix33F.CreateRotationY(-0.5f) * Matrix33F.CreateRotationZ(-0.5f), Matrix33F.Identity, new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(0.7f, 1.2f, 1.2f));
AddTwistSwingLimit(ragdoll, armUpperLeft, armLowerLeft, Matrix33F.CreateRotationZ(-1.2f), Matrix33F.Identity, new Vector3F(-0.3f, -1.2f, -1.2f), new Vector3F(0.3f, 1.2f, 1.2f));
AddTwistSwingLimit(ragdoll, armLowerLeft, handLeft, Matrix33F.Identity, Matrix33F.CreateRotationX(+ConstantsF.PiOver2), new Vector3F(-0.3f, -0.7f, -0.7f), new Vector3F(0.3f, 0.7f, 0.7f));
parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(backUpper).Inverse;
childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(armUpperRight).Inverse;
bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
AddTwistSwingLimit(ragdoll, backUpper, armUpperRight, bindPoseRelative.Orientation * Matrix33F.CreateRotationY(0.5f) * Matrix33F.CreateRotationZ(-0.5f), Matrix33F.Identity, new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(0.7f, 1.2f, 1.2f));
AddTwistSwingLimit(ragdoll, armUpperRight, armLowerRight, Matrix33F.CreateRotationZ(-1.2f), Matrix33F.Identity, new Vector3F(-0.3f, -1.2f, -1.2f), new Vector3F(0.3f, 1.2f, 1.2f));
AddTwistSwingLimit(ragdoll, armLowerRight, handRight, Matrix33F.Identity, Matrix33F.CreateRotationX(-ConstantsF.PiOver2), new Vector3F(-0.3f, -0.7f, -0.7f), new Vector3F(0.3f, 0.7f, 0.7f));
parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(pelvis).Inverse;
childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(legUpperLeft).Inverse;
bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
AddTwistSwingLimit(ragdoll, pelvis, legUpperLeft, bindPoseRelative.Orientation * Matrix33F.CreateRotationZ(1.2f), Matrix33F.Identity, new Vector3F(-0.1f, -0.7f, -1.5f), new Vector3F(+0.1f, +0.7f, +1.5f));
AddAngularLimit(skeletonPose, ragdoll, legUpperLeft, legLowerLeft, new Vector3F(0, 0, -2.2f), new Vector3F(0, 0, 0.0f));
AddTwistSwingLimit(ragdoll, legLowerLeft, footLeft, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.3f, -0.7f), new Vector3F(0.1f, 0.3f, 0.7f));
parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(pelvis).Inverse;
childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(legUpperRight).Inverse;
bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
AddTwistSwingLimit(ragdoll, pelvis, legUpperRight, bindPoseRelative.Orientation * Matrix33F.CreateRotationZ(1.2f), Matrix33F.Identity, new Vector3F(-0.1f, -0.7f, -1.5f), new Vector3F(+0.1f, +0.7f, +1.5f));
AddAngularLimit(skeletonPose, ragdoll, legUpperRight, legLowerRight, new Vector3F(0, 0, -2.2f), new Vector3F(0, 0, 0.0f));
AddTwistSwingLimit(ragdoll, legLowerRight, footRight, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.3f, -0.7f), new Vector3F(0.1f, 0.3f, 0.7f));
#endregion
#region ----- Add Motors -----
ragdoll.Motors.AddRange(Enumerable.Repeat <RagdollMotor>(null, numberOfBones));
ragdoll.Motors[pelvis] = new RagdollMotor(pelvis, -1);
ragdoll.Motors[backLower] = new RagdollMotor(backLower, pelvis);
ragdoll.Motors[backUpper] = new RagdollMotor(backUpper, backLower);
ragdoll.Motors[neck] = new RagdollMotor(neck, backUpper);
ragdoll.Motors[head] = new RagdollMotor(head, neck);
ragdoll.Motors[armUpperLeft] = new RagdollMotor(armUpperLeft, backUpper);
ragdoll.Motors[armLowerLeft] = new RagdollMotor(armLowerLeft, armUpperLeft);
ragdoll.Motors[handLeft] = new RagdollMotor(handLeft, armLowerLeft);
ragdoll.Motors[armUpperRight] = new RagdollMotor(armUpperRight, backUpper);
ragdoll.Motors[armLowerRight] = new RagdollMotor(armLowerRight, armUpperRight);
ragdoll.Motors[handRight] = new RagdollMotor(handRight, armLowerRight);
ragdoll.Motors[legUpperLeft] = new RagdollMotor(legUpperLeft, pelvis);
ragdoll.Motors[legLowerLeft] = new RagdollMotor(legLowerLeft, legUpperLeft);
ragdoll.Motors[footLeft] = new RagdollMotor(footLeft, legLowerLeft);
ragdoll.Motors[legUpperRight] = new RagdollMotor(legUpperRight, pelvis);
ragdoll.Motors[legLowerRight] = new RagdollMotor(legLowerRight, legUpperRight);
ragdoll.Motors[footRight] = new RagdollMotor(footRight, legLowerRight);
#endregion
}
/// <summary>
/// Initializes the ragdoll for the given skeleton pose.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="ragdoll">The ragdoll.</param>
/// <param name="simulation">The simulation in which the ragdoll will be used.</param>
public static void Create(SkeletonPose skeletonPose, Ragdoll ragdoll, Simulation simulation)
{
var skeleton = skeletonPose.Skeleton;
const float totalMass = 80; // The total mass of the ragdoll.
const int numberOfBodies = 17;
// Get distance from foot to head as a measure for the size of the ragdoll.
int head = skeleton.GetIndex("Head");
int footLeft = skeleton.GetIndex("L_Ankle1");
var headPosition = skeletonPose.GetBonePoseAbsolute(head).Translation;
var footPosition = skeletonPose.GetBonePoseAbsolute(footLeft).Translation;
var headToFootDistance = (headPosition - footPosition).Length;
// We use the same mass properties for all bodies. This is not realistic but more stable
// because large mass differences or thin bodies (arms!) are less stable.
// We use the mass properties of sphere proportional to the size of the model.
var massFrame = MassFrame.FromShapeAndMass(new SphereShape(headToFootDistance / 8), Vector3F.One, totalMass / numberOfBodies, 0.1f, 1);
var material = new UniformMaterial();
#region ----- Add Bodies and Body Offsets -----
var numberOfBones = skeleton.NumberOfBones;
ragdoll.Bodies.AddRange(Enumerable.Repeat<RigidBody>(null, numberOfBones));
ragdoll.BodyOffsets.AddRange(Enumerable.Repeat(Pose.Identity, numberOfBones));
var pelvis = skeleton.GetIndex("Pelvis");
ragdoll.Bodies[pelvis] = new RigidBody(new BoxShape(0.3f, 0.4f, 0.55f), massFrame, material);
ragdoll.BodyOffsets[pelvis] = new Pose(new Vector3F(0.0f, 0, 0));
var backLower = skeleton.GetIndex("Spine");
ragdoll.Bodies[backLower] = new RigidBody(new BoxShape(0.36f, 0.4f, 0.55f), massFrame, material);
ragdoll.BodyOffsets[backLower] = new Pose(new Vector3F(0.18f, 0, 0));
var backUpper = skeleton.GetIndex("Spine2");
ragdoll.Bodies[backUpper] = new RigidBody(new BoxShape(0.5f, 0.4f, 0.65f), massFrame, material);
ragdoll.BodyOffsets[backUpper] = new Pose(new Vector3F(0.25f, 0, 0));
var neck = skeleton.GetIndex("Neck");
ragdoll.Bodies[neck] = new RigidBody(new CapsuleShape(0.12f, 0.3f), massFrame, material);
ragdoll.BodyOffsets[neck] = new Pose(new Vector3F(0.15f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
ragdoll.Bodies[neck].CollisionObject.Enabled = false;
ragdoll.Bodies[head] = new RigidBody(new SphereShape(0.2f), massFrame, material);
ragdoll.BodyOffsets[head] = new Pose(new Vector3F(0.15f, 0.02f, 0));
var armUpperLeft = skeleton.GetIndex("L_UpperArm");
ragdoll.Bodies[armUpperLeft] = new RigidBody(new CapsuleShape(0.12f, 0.6f), massFrame, material);
ragdoll.BodyOffsets[armUpperLeft] = new Pose(new Vector3F(0.2f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var armLowerLeft = skeleton.GetIndex("L_Forearm");
ragdoll.Bodies[armLowerLeft] = new RigidBody(new CapsuleShape(0.08f, 0.5f), massFrame, material);
ragdoll.BodyOffsets[armLowerLeft] = new Pose(new Vector3F(0.2f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var handLeft = skeleton.GetIndex("L_Hand");
ragdoll.Bodies[handLeft] = new RigidBody(new BoxShape(0.2f, 0.06f, 0.15f), massFrame, material);
ragdoll.BodyOffsets[handLeft] = new Pose(new Vector3F(0.1f, 0, 0));
var armUpperRight = skeleton.GetIndex("R_UpperArm");
ragdoll.Bodies[armUpperRight] = new RigidBody(new CapsuleShape(0.12f, 0.6f), massFrame, material);
ragdoll.BodyOffsets[armUpperRight] = new Pose(new Vector3F(0.2f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var armLowerRight = skeleton.GetIndex("R_Forearm");
ragdoll.Bodies[armLowerRight] = new RigidBody(new CapsuleShape(0.08f, 0.5f), massFrame, material);
ragdoll.BodyOffsets[armLowerRight] = new Pose(new Vector3F(0.2f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var handRight = skeleton.GetIndex("R_Hand");
ragdoll.Bodies[handRight] = new RigidBody(new BoxShape(0.2f, 0.06f, 0.15f), massFrame, material);
ragdoll.BodyOffsets[handRight] = new Pose(new Vector3F(0.1f, 0, 0));
var legUpperLeft = skeleton.GetIndex("L_Thigh1");
ragdoll.Bodies[legUpperLeft] = new RigidBody(new CapsuleShape(0.16f, 0.8f), massFrame, material);
ragdoll.BodyOffsets[legUpperLeft] = new Pose(new Vector3F(0.4f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var legLowerLeft = skeleton.GetIndex("L_Knee2");
ragdoll.Bodies[legLowerLeft] = new RigidBody(new CapsuleShape(0.12f, 0.65f), massFrame, material);
ragdoll.BodyOffsets[legLowerLeft] = new Pose(new Vector3F(0.32f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
//var footLeft = skeleton.GetIndex("L_Ankle1");
ragdoll.Bodies[footLeft] = new RigidBody(new BoxShape(0.20f, 0.5f, 0.3f), massFrame, material);
ragdoll.BodyOffsets[footLeft] = new Pose(new Vector3F(0.16f, 0.15f, 0));
var legUpperRight = skeleton.GetIndex("R_Thigh");
ragdoll.Bodies[legUpperRight] = new RigidBody(new CapsuleShape(0.16f, 0.8f), massFrame, material);
ragdoll.BodyOffsets[legUpperRight] = new Pose(new Vector3F(0.4f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var legLowerRight = skeleton.GetIndex("R_Knee");
ragdoll.Bodies[legLowerRight] = new RigidBody(new CapsuleShape(0.12f, 0.65f), massFrame, material);
ragdoll.BodyOffsets[legLowerRight] = new Pose(new Vector3F(0.32f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
var footRight = skeleton.GetIndex("R_Ankle");
ragdoll.Bodies[footRight] = new RigidBody(new BoxShape(0.20f, 0.5f, 0.3f), massFrame, material);
ragdoll.BodyOffsets[footRight] = new Pose(new Vector3F(0.16f, 0.15f, 0));
#endregion
#region ----- Set Collision Filters -----
// Collisions between connected bodies will be disabled in AddJoint(). (A BallJoint
// has a property CollisionEnabled which decides whether connected bodies can
// collide.)
// But we need to disable some more collision between bodies that are not directly
// connected but still too close to each other.
var filter = (ICollisionFilter)simulation.CollisionDomain.CollisionDetection.CollisionFilter;
filter.Set(ragdoll.Bodies[backUpper].CollisionObject, ragdoll.Bodies[head].CollisionObject, false);
filter.Set(ragdoll.Bodies[armUpperRight].CollisionObject, ragdoll.Bodies[backLower].CollisionObject, false);
filter.Set(ragdoll.Bodies[armUpperLeft].CollisionObject, ragdoll.Bodies[backLower].CollisionObject, false);
filter.Set(ragdoll.Bodies[legUpperLeft].CollisionObject, ragdoll.Bodies[legUpperRight].CollisionObject, false);
#endregion
#region ----- Add Joints -----
AddJoint(skeletonPose, ragdoll, pelvis, backLower);
AddJoint(skeletonPose, ragdoll, backLower, backUpper);
AddJoint(skeletonPose, ragdoll, backUpper, neck);
AddJoint(skeletonPose, ragdoll, neck, head);
AddJoint(skeletonPose, ragdoll, backUpper, armUpperLeft);
AddJoint(skeletonPose, ragdoll, armUpperLeft, armLowerLeft);
AddJoint(skeletonPose, ragdoll, armLowerLeft, handLeft);
AddJoint(skeletonPose, ragdoll, backUpper, armUpperRight);
AddJoint(skeletonPose, ragdoll, armUpperRight, armLowerRight);
AddJoint(skeletonPose, ragdoll, armLowerRight, handRight);
AddJoint(skeletonPose, ragdoll, pelvis, legUpperLeft);
AddJoint(skeletonPose, ragdoll, legUpperLeft, legLowerLeft);
AddJoint(skeletonPose, ragdoll, legLowerLeft, footLeft);
AddJoint(skeletonPose, ragdoll, pelvis, legUpperRight);
AddJoint(skeletonPose, ragdoll, legUpperRight, legLowerRight);
AddJoint(skeletonPose, ragdoll, legLowerRight, footRight);
#endregion
#region ----- Add Limits -----
// Choosing limits is difficult.
// We create hinge limits with AngularLimits in the back and in the knee.
// For all other joints we use TwistSwingLimits with symmetric cones.
AddAngularLimit(skeletonPose, ragdoll, pelvis, backLower, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.3f));
AddAngularLimit(skeletonPose, ragdoll, backLower, backUpper, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.4f));
AddAngularLimit(skeletonPose, ragdoll, backUpper, neck, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.3f));
AddTwistSwingLimit(ragdoll, neck, head, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.5f, -0.7f), new Vector3F(0.1f, 0.5f, 0.7f));
var parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(backUpper).Inverse;
var childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(armUpperLeft).Inverse;
var bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
AddTwistSwingLimit(ragdoll, backUpper, armUpperLeft, bindPoseRelative.Orientation * Matrix33F.CreateRotationY(-0.5f) * Matrix33F.CreateRotationZ(-0.5f), Matrix33F.Identity, new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(0.7f, 1.2f, 1.2f));
AddTwistSwingLimit(ragdoll, armUpperLeft, armLowerLeft, Matrix33F.CreateRotationZ(-1.2f), Matrix33F.Identity, new Vector3F(-0.3f, -1.2f, -1.2f), new Vector3F(0.3f, 1.2f, 1.2f));
AddTwistSwingLimit(ragdoll, armLowerLeft, handLeft, Matrix33F.Identity, Matrix33F.CreateRotationX(+ConstantsF.PiOver2), new Vector3F(-0.3f, -0.7f, -0.7f), new Vector3F(0.3f, 0.7f, 0.7f));
parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(backUpper).Inverse;
childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(armUpperRight).Inverse;
bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
AddTwistSwingLimit(ragdoll, backUpper, armUpperRight, bindPoseRelative.Orientation * Matrix33F.CreateRotationY(0.5f) * Matrix33F.CreateRotationZ(-0.5f), Matrix33F.Identity, new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(0.7f, 1.2f, 1.2f));
AddTwistSwingLimit(ragdoll, armUpperRight, armLowerRight, Matrix33F.CreateRotationZ(-1.2f), Matrix33F.Identity, new Vector3F(-0.3f, -1.2f, -1.2f), new Vector3F(0.3f, 1.2f, 1.2f));
AddTwistSwingLimit(ragdoll, armLowerRight, handRight, Matrix33F.Identity, Matrix33F.CreateRotationX(-ConstantsF.PiOver2), new Vector3F(-0.3f, -0.7f, -0.7f), new Vector3F(0.3f, 0.7f, 0.7f));
parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(pelvis).Inverse;
childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(legUpperLeft).Inverse;
bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
AddTwistSwingLimit(ragdoll, pelvis, legUpperLeft, bindPoseRelative.Orientation * Matrix33F.CreateRotationZ(1.2f), Matrix33F.Identity, new Vector3F(-0.1f, -0.7f, -1.5f), new Vector3F(+0.1f, +0.7f, +1.5f));
AddAngularLimit(skeletonPose, ragdoll, legUpperLeft, legLowerLeft, new Vector3F(0, 0, -2.2f), new Vector3F(0, 0, 0.0f));
AddTwistSwingLimit(ragdoll, legLowerLeft, footLeft, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.3f, -0.7f), new Vector3F(0.1f, 0.3f, 0.7f));
parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(pelvis).Inverse;
childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(legUpperRight).Inverse;
bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
AddTwistSwingLimit(ragdoll, pelvis, legUpperRight, bindPoseRelative.Orientation * Matrix33F.CreateRotationZ(1.2f), Matrix33F.Identity, new Vector3F(-0.1f, -0.7f, -1.5f), new Vector3F(+0.1f, +0.7f, +1.5f));
AddAngularLimit(skeletonPose, ragdoll, legUpperRight, legLowerRight, new Vector3F(0, 0, -2.2f), new Vector3F(0, 0, 0.0f));
AddTwistSwingLimit(ragdoll, legLowerRight, footRight, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.3f, -0.7f), new Vector3F(0.1f, 0.3f, 0.7f));
#endregion
#region ----- Add Motors -----
ragdoll.Motors.AddRange(Enumerable.Repeat<RagdollMotor>(null, numberOfBones));
ragdoll.Motors[pelvis] = new RagdollMotor(pelvis, -1);
ragdoll.Motors[backLower] = new RagdollMotor(backLower, pelvis);
ragdoll.Motors[backUpper] = new RagdollMotor(backUpper, backLower);
ragdoll.Motors[neck] = new RagdollMotor(neck, backUpper);
ragdoll.Motors[head] = new RagdollMotor(head, neck);
ragdoll.Motors[armUpperLeft] = new RagdollMotor(armUpperLeft, backUpper);
ragdoll.Motors[armLowerLeft] = new RagdollMotor(armLowerLeft, armUpperLeft);
ragdoll.Motors[handLeft] = new RagdollMotor(handLeft, armLowerLeft);
ragdoll.Motors[armUpperRight] = new RagdollMotor(armUpperRight, backUpper);
ragdoll.Motors[armLowerRight] = new RagdollMotor(armLowerRight, armUpperRight);
ragdoll.Motors[handRight] = new RagdollMotor(handRight, armLowerRight);
ragdoll.Motors[legUpperLeft] = new RagdollMotor(legUpperLeft, pelvis);
ragdoll.Motors[legLowerLeft] = new RagdollMotor(legLowerLeft, legUpperLeft);
ragdoll.Motors[footLeft] = new RagdollMotor(footLeft, legLowerLeft);
ragdoll.Motors[legUpperRight] = new RagdollMotor(legUpperRight, pelvis);
ragdoll.Motors[legLowerRight] = new RagdollMotor(legLowerRight, legUpperRight);
ragdoll.Motors[footRight] = new RagdollMotor(footRight, legLowerRight);
#endregion
}
/// <summary>
/// Drives the controlled body.
/// </summary>
/// <param name="skeletonPose">The target skeleton pose.</param>
/// <param name="deltaTime">The time step.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose" /> is <see langword="null"/>.
/// </exception>
internal void DriveToPose(SkeletonPose skeletonPose, float deltaTime)
{
if (skeletonPose == null)
throw new ArgumentNullException("skeletonPose");
Debug.Assert(Ragdoll != null, "Motor must not be called when Ragdoll property is null.");
Debug.Assert(Ragdoll.Simulation != null, "Ragdoll was not added to a simulation.");
if (!Enabled)
return;
if (Mode == RagdollMotorMode.Velocity)
{
// ----- Velocity motor
Debug.Assert(_quaternionMotor.Simulation == null, "Velocity motors should not be added to the simulation.");
if (BoneIndex >= Ragdoll.Bodies.Count)
return;
var body = Ragdoll.Bodies[BoneIndex];
if (body == null)
return;
var childOffset = (BoneIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[BoneIndex] : Pose.Identity;
var childPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(BoneIndex)) * childOffset;
// Wake the body up. It should move, even if we move it very slowly.
body.WakeUp();
// Set velocities.
body.LinearVelocity = AnimationHelper.ComputeLinearVelocity(body.Pose.Position, childPose.Position, deltaTime);
body.AngularVelocity = AnimationHelper.ComputeAngularVelocity(body.Pose.Orientation, childPose.Orientation, deltaTime);
}
else
{
// ----- Constraint motor
if (_quaternionMotor.Simulation == null)
{
// The motor was not added to the simulation. (Invalid configuration)
return;
}
var parentOffset = (ParentIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[ParentIndex] : Pose.Identity;
var parentPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(ParentIndex)) * parentOffset;
var childOffset = (BoneIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[BoneIndex] : Pose.Identity;
var childPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(BoneIndex)) * childOffset;
// Set the relative motor target.
var rotationMatrix = parentPose.Orientation.Transposed * childPose.Orientation;
_quaternionMotor.TargetOrientation = QuaternionF.CreateRotation(rotationMatrix);
}
}
/// <summary>
/// Perform mapping in absolute space.
/// </summary>
private static void MapAbsolute(bool mapTranslations, SkeletonPose skeletonA, int boneIndexA, SkeletonPose skeletonB, int boneIndexB, float scaleAToB, QuaternionF rotationBToA, QuaternionF rotationAToB)
{
// The current absolute bone pose of bone A.
var boneAActualRotationAbsolute = skeletonA.GetBonePoseAbsolute(boneIndexA).Rotation;
var boneABindRotationAbsolute = skeletonA.Skeleton.GetBindPoseAbsoluteInverse(boneIndexA).Rotation;
var boneBBindRotationAbsolute = skeletonB.Skeleton.GetBindPoseAbsoluteInverse(boneIndexB).Rotation.Inverse;
var relativeRotation = boneAActualRotationAbsolute * boneABindRotationAbsolute;
// Rotation: Using similarity transformation: (Read from right to left.)
// Rotate from model B space to model A space.
// Apply the bone transform rotation in model A space
// Rotate back from model A space to model B space.
relativeRotation = rotationAToB * relativeRotation * rotationBToA;
skeletonB.SetBoneRotationAbsolute(boneIndexB, relativeRotation * boneBBindRotationAbsolute);
// TODO: Map translations.
// How?
// Map translation relative to model space?
// Map translation relative to the next common bone ancestor that is in both skeletons
// (then we would need a mechanism or user input that gives us this ancestor, complicated)?
// Map translation relative to local space (the bone transform translation as in MapLocal)?
}
private static void DoWork(QuaternionF skeletonOffset, SkeletonPose skeletonA, SkeletonPose skeletonB, int boneIndexA, int neckBoneIndexA, int leftShoulderBoneIndexA, int rightShoulderBoneIndexA, int boneIndexB, int neckBoneIndexB, int leftShoulderBoneIndexB, int rightShoulderBoneIndexB)
{
// Reset root bone.
skeletonB.ResetBoneTransforms(boneIndexB, boneIndexB, false, true, false);
// Get absolute positions all bones.
var boneA = skeletonA.GetBonePoseAbsolute(boneIndexA).Translation;
var neckA = skeletonA.GetBonePoseAbsolute(neckBoneIndexA).Translation;
var leftShoulderA = skeletonA.GetBonePoseAbsolute(leftShoulderBoneIndexA).Translation;
var rightShoulderA = skeletonA.GetBonePoseAbsolute(rightShoulderBoneIndexA).Translation;
var boneB = skeletonB.GetBonePoseAbsolute(boneIndexB).Translation;
var neckB = skeletonB.GetBonePoseAbsolute(neckBoneIndexB).Translation;
var leftShoulderB = skeletonB.GetBonePoseAbsolute(leftShoulderBoneIndexB).Translation;
var rightShoulderB = skeletonB.GetBonePoseAbsolute(rightShoulderBoneIndexB).Translation;
// Abort if any bone to bone distance is 0.
if (Vector3F.AreNumericallyEqual(boneA, neckA)
|| Vector3F.AreNumericallyEqual(boneA, rightShoulderA)
|| Vector3F.AreNumericallyEqual(leftShoulderA, rightShoulderA)
|| Vector3F.AreNumericallyEqual(boneB, neckB)
|| Vector3F.AreNumericallyEqual(boneB, rightShoulderB)
|| Vector3F.AreNumericallyEqual(leftShoulderB, rightShoulderB))
{
return;
}
// Get shoulder axis vectors in model B space.
var shoulderAxisA = rightShoulderA - leftShoulderA;
shoulderAxisA = skeletonOffset.Rotate(shoulderAxisA);
var shoulderAxisB = rightShoulderB - leftShoulderB;
// Create a twist rotation from the shoulder vectors.
var shoulderRotation = QuaternionF.CreateRotation(shoulderAxisB, shoulderAxisA);
// Apply this twist to the spine. (Modifies the neckB position.)
neckB = boneB + shoulderRotation.Rotate(neckB - boneB);
// Get spine vectors in model B space.
var spineAxisA = neckA - boneA;
spineAxisA = skeletonOffset.Rotate(spineAxisA);
var spineAxisB = neckB - boneB;
// Create swing rotation from spine vectors.
var spineRotation = QuaternionF.CreateRotation(spineAxisB, spineAxisA);
// Apply the shoulder twist rotation followed by the spine swing rotation.
skeletonB.RotateBoneAbsolute(boneIndexB, spineRotation * shoulderRotation);
}
/// <summary>
/// Updates the pose of a single body, so that the bodies match the bone transforms of the given
/// bone.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="boneIndex">The index of the bone.</param>
/// <remarks>
/// See also <see cref="UpdateBodiesFromSkeleton(SkeletonPose)"/>.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose" /> is <see langword="null"/>.
/// </exception>
public void UpdateBodyFromSkeleton(SkeletonPose skeletonPose, int boneIndex)
{
if (skeletonPose == null)
throw new ArgumentNullException("skeletonPose");
if (boneIndex < 0 || boneIndex >= Bodies.Count)
return;
var body = Bodies[boneIndex];
if (body == null)
return;
Pose offset = (boneIndex < BodyOffsets.Count) ? BodyOffsets[boneIndex] : Pose.Identity;
Pose bodyPose = Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(boneIndex)) * offset;
body.Pose = bodyPose;
body.LinearVelocity = Vector3F.Zero;
body.AngularVelocity = Vector3F.Zero;
}
// = Teleport of bodies.
/// <summary>
/// Updates the poses of the bodies, so that the bodies match the bone transforms of the given
/// skeleton pose.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <remarks>
/// The poses of the rigid bodies are changed instantly. The bodies will "teleport" instantly to
/// the target positions. They will not interact correctly with other physics objects. The
/// velocities of the rigid bodies are set to zero. The bodies will be positioned relative to
/// the world space pose defined by <see cref="Pose"/>.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose" /> is <see langword="null"/>.
/// </exception>
public void UpdateBodiesFromSkeleton(SkeletonPose skeletonPose)
{
if (skeletonPose == null)
throw new ArgumentNullException("skeletonPose");
var skeleton = skeletonPose.Skeleton;
for (int i = 0; i < Bodies.Count && i < skeleton.NumberOfBones; i++)
{
var body = Bodies[i];
if (body == null)
continue;
Pose offset = (i < BodyOffsets.Count) ? BodyOffsets[i] : Pose.Identity;
Pose bodyPose = Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(i)) * offset;
body.Pose = bodyPose;
body.LinearVelocity = Vector3F.Zero;
body.AngularVelocity = Vector3F.Zero;
}
}
public void DrawSkeleton(SkeletonPose skeletonPose, Pose pose, Vector3F scale, float axisLength, Color color, bool drawOverScene)
{
if (!Enabled || skeletonPose == null)
return;
LineBatch lineBatch;
TextBatch textBatch;
if (drawOverScene)
{
lineBatch = OverSceneLineBatch;
textBatch = OverScene3DTextBatch;
}
else
{
lineBatch = InSceneLineBatch;
textBatch = InSceneTextBatch;
}
var skeleton = skeletonPose.Skeleton;
var world = pose * Matrix44F.CreateScale(scale);
for (int i = 0; i < skeleton.NumberOfBones; i++)
{
string name = skeleton.GetName(i);
SrtTransform bonePose = skeletonPose.GetBonePoseAbsolute(i);
var translation = bonePose.Translation;
var rotation = bonePose.Rotation.ToRotationMatrix33();
var translationWorld = world.TransformPosition(translation);
int parentIndex = skeleton.GetParent(i);
if (parentIndex >= 0)
{
// Draw line to parent joint representing the parent bone.
SrtTransform parentPose = skeletonPose.GetBonePoseAbsolute(parentIndex);
lineBatch.Add(translationWorld, world.TransformPosition(parentPose.Translation), color);
}
// Add three lines in Red, Green and Blue.
lineBatch.Add(translationWorld, world.TransformPosition(translation + rotation.GetColumn(0) * axisLength), Color.Red);
lineBatch.Add(translationWorld, world.TransformPosition(translation + rotation.GetColumn(1) * axisLength), Color.Green);
lineBatch.Add(translationWorld, world.TransformPosition(translation + rotation.GetColumn(2) * axisLength), Color.Blue);
// Draw name.
if (string.IsNullOrEmpty(name))
name = "-";
textBatch.Add(
string.Format(CultureInfo.InvariantCulture, "{0} {1}", name, i),
world.TransformPosition(translation + rotation.GetColumn(0) * axisLength * 0.5f),
new Vector2F(0, 1),
color);
}
}