public void CompositeShapeWithRigidBodiesDoesNotSupportScaling()
{
// The first composite shape does not use rigid bodies.
var s = new CompositeShape();
s.Children.Add(new GeometricObject(new BoxShape(1, 2, 3), new Vector3F(1.1f, 0.3f, 0.8f), new Pose(new Vector3F(100, 10, 0), RandomHelper.Random.NextQuaternionF())));
s.Children.Add(new GeometricObject(new ConeShape(1, 2), new Vector3F(1.1f, 0.3f, 0.8f), new Pose(new Vector3F(-10, -10, 0), RandomHelper.Random.NextQuaternionF())));
// The second composite shape uses rigid bodies as children.
var r0 = new RigidBody(s.Children[0].Shape);
r0.Pose = s.Children[0].Pose;
r0.Scale = s.Children[0].Scale;
r0.MassFrame = MassFrame.FromShapeAndDensity(r0.Shape, r0.Scale, 0.7f, 0.001f, 10);
var r1 = new RigidBody(s.Children[1].Shape);
r1.Pose = s.Children[1].Pose;
r1.Scale = s.Children[1].Scale;
r1.MassFrame = MassFrame.FromShapeAndDensity(r1.Shape, r1.Scale, 0.7f, 0.001f, 10);
var s1 = new CompositeShape();
s1.Children.Add(r0);
s1.Children.Add(r1);
float m1;
Vector3F com1;
Matrix33F i1;
MassHelper.GetMass(s1,
new Vector3F(2), // !!!
100, true, 0.001f, 10, out m1, out com1, out i1);
}
private void InitializeBody(Vector3 upVector)
{
if (!upVector.TryNormalize())
{
throw new ArgumentException("The up vector must not be a zero vector.");
}
UpVector = upVector;
CapsuleShape shape = new CapsuleShape(0.4f, 1.8f);
MassFrame mass = new MassFrame {
Mass = 100
};
UniformMaterial material = new UniformMaterial
{
// The body should be frictionless, so that it can be easily pushed by the simulation to
// valid positions.
StaticFriction = 0.0f,
DynamicFriction = 0.0f,
// The body should not bounce when being hit or pushed.
Restitution = 0
};
Body = new RigidBody(shape, mass, material)
{
// We set the mass explicitly and it should not automatically change when the
// shape is changed; e.g. a ducked character has a smaller shape, but still the same mass.
AutoUpdateMass = false,
// This body is under our control and should never be deactivated by the simulation.
CanSleep = false,
CcdEnabled = true,
// The capsule does not rotate in any direction.
LockRotationX = true,
LockRotationY = true,
LockRotationZ = true,
Name = "CharacterController",
Pose = new Pose(shape.Height / 2 * upVector,
Quaternion.CreateFromRotationMatrix(Vector3.UnitY, upVector)),
};
// When the user changes the shape, we must re-compute all contacts.
Body.ShapeChanged += (s, e) => UpdateContacts();
}
public void CompositeShapeWithRigidBodies()
{
// The first composite shape does not use rigid bodies.
var s = new CompositeShape();
s.Children.Add(new GeometricObject(new BoxShape(1, 2, 3), new Vector3F(1.1f, 0.3f, 0.8f), new Pose(new Vector3F(100, 10, 0), RandomHelper.Random.NextQuaternionF())));
s.Children.Add(new GeometricObject(new ConeShape(1, 2), new Vector3F(1.1f, 0.3f, 0.8f), new Pose(new Vector3F(-10, -10, 0), RandomHelper.Random.NextQuaternionF())));
float m0;
Vector3F com0;
Matrix33F i0;
MassHelper.GetMass(s, new Vector3F(1), 0.7f, true, 0.001f, 10, out m0, out com0, out i0);
// The second composite shape uses rigid bodies as children.
var r0 = new RigidBody(s.Children[0].Shape);
r0.Pose = s.Children[0].Pose;
r0.Scale = s.Children[0].Scale;
r0.MassFrame = MassFrame.FromShapeAndDensity(r0.Shape, r0.Scale, 0.7f, 0.001f, 10);
var r1 = new RigidBody(s.Children[1].Shape);
r1.Pose = s.Children[1].Pose;
r1.Scale = s.Children[1].Scale;
r1.MassFrame = MassFrame.FromShapeAndDensity(r1.Shape, r1.Scale, 0.7f, 0.001f, 10);
var s1 = new CompositeShape();
s1.Children.Add(r0);
s1.Children.Add(r1);
float m1;
Vector3F com1;
Matrix33F i1;
MassHelper.GetMass(s1, new Vector3F(1), 100, true, 0.001f, 10, out m1, out com1, out i1);
const float e = 0.01f;
Assert.IsTrue(Numeric.AreEqual(m0, m1, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0, i1, e * (1 + i0.Trace)));
}
public void AddRagdoll(float scale, Vector3F ragdollPosition)
{
// Ragdolls are usually used in games to create realistic death animations of
// characters. The character is usually rendered using a skinned triangle mesh.
// But in the physics simulation the body parts of the character are represented
// using simple shapes, such as spheres, capsules, boxes, or convex polyhedra,
// which are connected with joints.
// The physics simulations computes how these parts collide and fall. The positions
// and orientations are then read back each frame to update the animation of the
// triangle mesh.
// In this example the ragdoll is built from spheres, capsules and boxes. The
// rigid bodies are created in code. In practice, ragdolls should be built using
// external tools, such as a 3D modeller or a game editor.
#region ----- Create rigid bodies for the most relevant body parts -----
BoxShape pelvisShape = new BoxShape(0.3f * scale, 0.22f * scale, 0.20f * scale);
MassFrame pelvisMass = MassFrame.FromShapeAndDensity(pelvisShape, Vector3F.One, Density, 0.01f, 3);
RigidBody pelvis = new RigidBody(pelvisShape, pelvisMass, null)
{
Pose = new Pose(new Vector3F(0f, 0.01f * scale, -0.03f * scale) + ragdollPosition),
};
Simulation.RigidBodies.Add(pelvis);
BoxShape torsoShape = new BoxShape(0.35f * scale, 0.22f * scale, 0.44f * scale);
MassFrame torsoMass = MassFrame.FromShapeAndDensity(torsoShape, Vector3F.One, Density, 0.01f, 3);
RigidBody torso = new RigidBody(torsoShape, torsoMass, null)
{
Pose = new Pose(new Vector3F(0f, 0.01f * scale, -0.4f * scale) + ragdollPosition),
};
Simulation.RigidBodies.Add(torso);
SphereShape headShape = new SphereShape(0.13f * scale);
MassFrame headMass = MassFrame.FromShapeAndDensity(headShape, Vector3F.One, Density, 0.01f, 3);
RigidBody head = new RigidBody(headShape, headMass, null)
{
Pose = new Pose(new Vector3F(0f * scale, 0f, -0.776f * scale) + ragdollPosition),
};
Simulation.RigidBodies.Add(head);
CapsuleShape upperArmShape = new CapsuleShape(0.08f * scale, 0.3f * scale);
MassFrame upperArmMass = MassFrame.FromShapeAndDensity(upperArmShape, Vector3F.One, Density, 0.01f, 3);
RigidBody leftUpperArm = new RigidBody(upperArmShape, upperArmMass, null)
{
Pose = new Pose(new Vector3F(-0.32f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(leftUpperArm);
RigidBody rightUpperArm = new RigidBody(upperArmShape, upperArmMass, null)
{
Pose = new Pose(new Vector3F(0.32f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(rightUpperArm);
CapsuleShape lowerArmShape = new CapsuleShape(0.08f * scale, 0.4f * scale);
MassFrame lowerArmMass = MassFrame.FromShapeAndDensity(lowerArmShape, Vector3F.One, Density, 0.01f, 3);
RigidBody leftLowerArm = new RigidBody(lowerArmShape, lowerArmMass, null)
{
Pose = new Pose(new Vector3F(-0.62f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(leftLowerArm);
RigidBody rightLowerArm = new RigidBody(lowerArmShape, lowerArmMass, null)
{
Pose = new Pose(new Vector3F(0.62f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(rightLowerArm);
CapsuleShape upperLegShape = new CapsuleShape(0.09f * scale, 0.5f * scale);
MassFrame upperLegMass = MassFrame.FromShapeAndDensity(upperLegShape, Vector3F.One, Density, 0.01f, 3);
RigidBody leftUpperLeg = new RigidBody(upperLegShape, upperLegMass, null)
{
Pose = new Pose(new Vector3F(-0.10f * scale, 0.01f * scale, 0.233f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(leftUpperLeg);
RigidBody rightUpperLeg = new RigidBody(upperLegShape, upperLegMass, null)
{
Pose = new Pose(new Vector3F(0.10f * scale, 0.01f * scale, 0.233f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(rightUpperLeg);
CapsuleShape lowerLegShape = new CapsuleShape(0.08f * scale, 0.4f * scale);
MassFrame lowerLegMass = MassFrame.FromShapeAndDensity(pelvisShape, Vector3F.One, Density, 0.01f, 3);
RigidBody leftLowerLeg = new RigidBody(lowerLegShape, lowerLegMass, null)
{
Pose = new Pose(new Vector3F(-0.11f * scale, 0.01f * scale, 0.7f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(leftLowerLeg);
RigidBody rightLowerLeg = new RigidBody(lowerLegShape, lowerLegMass, null)
{
Pose = new Pose(new Vector3F(0.11f * scale, 0.01f * scale, 0.7f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(rightLowerLeg);
BoxShape footShape = new BoxShape(0.12f * scale, 0.28f * scale, 0.07f * scale);
MassFrame footMass = MassFrame.FromShapeAndDensity(footShape, Vector3F.One, Density, 0.01f, 3);
RigidBody leftFoot = new RigidBody(footShape, footMass, null)
{
Pose = new Pose(new Vector3F(-0.11f * scale, -0.06f * scale, 0.94f * scale) + ragdollPosition),
};
Simulation.RigidBodies.Add(leftFoot);
RigidBody rightFoot = new RigidBody(footShape, footMass, null)
{
Pose = new Pose(new Vector3F(0.11f * scale, -0.06f * scale, 0.94f * scale) + ragdollPosition),
};
Simulation.RigidBodies.Add(rightFoot);
#endregion
#region ----- Add joints between body parts -----
Vector3F pelvisJointPosition = new Vector3F(0f, 0.026f * scale, -0.115f * scale) + ragdollPosition;
HingeJoint pelvisJoint = new HingeJoint
{
BodyA = torso,
BodyB = pelvis,
AnchorPoseALocal = new Pose(torso.Pose.ToLocalPosition(pelvisJointPosition)),
AnchorPoseBLocal = new Pose(pelvis.Pose.ToLocalPosition(pelvisJointPosition)),
Minimum = -0.5f,
Maximum = 1.1f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(pelvisJoint);
Vector3F neckJointPosition = new Vector3F(0f, 0.026f * scale, -0.690f * scale) + ragdollPosition;
HingeJoint neckJoint = new HingeJoint
{
BodyA = head,
BodyB = torso,
AnchorPoseALocal = new Pose(head.Pose.ToLocalPosition(neckJointPosition)),
AnchorPoseBLocal = new Pose(torso.Pose.ToLocalPosition(neckJointPosition)),
Minimum = -1f,
Maximum = 1f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(neckJoint);
Vector3F leftShoulderJointPosition = new Vector3F(-0.193f * scale, 0.056f * scale, -0.528f * scale) + ragdollPosition;
Vector3F leftShoulderJointAxis = new Vector3F(0, -1, -1).Normalized;
Matrix33F leftShoulderJointOrientation = new Matrix33F();
leftShoulderJointOrientation.SetColumn(0, leftShoulderJointAxis);
leftShoulderJointOrientation.SetColumn(1, leftShoulderJointAxis.Orthonormal1);
leftShoulderJointOrientation.SetColumn(2, leftShoulderJointAxis.Orthonormal2);
BallJoint leftShoulderJoint = new BallJoint
{
BodyA = leftUpperArm,
BodyB = torso,
AnchorPositionALocal = leftUpperArm.Pose.ToLocalPosition(leftShoulderJointPosition),
AnchorPositionBLocal = torso.Pose.ToLocalPosition(leftShoulderJointPosition),
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(leftShoulderJoint);
Vector3F rightShoulderJointPosition = new Vector3F(0.193f * scale, 0.056f * scale, -0.528f * scale) + ragdollPosition;
Vector3F rightShoulderJointAxis = new Vector3F(0, 1, 1).Normalized;
Matrix33F rightShoulderJointOrientation = new Matrix33F();
rightShoulderJointOrientation.SetColumn(0, rightShoulderJointAxis);
rightShoulderJointOrientation.SetColumn(1, rightShoulderJointAxis.Orthonormal1);
rightShoulderJointOrientation.SetColumn(2, rightShoulderJointAxis.Orthonormal2);
BallJoint rightShoulderJoint = new BallJoint
{
BodyA = rightUpperArm,
BodyB = torso,
AnchorPositionALocal = rightUpperArm.Pose.ToLocalPosition(rightShoulderJointPosition),
AnchorPositionBLocal = torso.Pose.ToLocalPosition(rightShoulderJointPosition),
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(rightShoulderJoint);
Vector3F leftElbowJointPosition = new Vector3F(-0.451f * scale, 0.071f * scale, -0.538f * scale) + ragdollPosition;
Matrix33F elbowAxisOrientation = new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0);
HingeJoint leftElbowJoint = new HingeJoint
{
BodyA = leftLowerArm,
BodyB = leftUpperArm,
AnchorPoseALocal = new Pose(leftLowerArm.Pose.ToLocalPosition(leftElbowJointPosition), leftLowerArm.Pose.Orientation.Inverse * elbowAxisOrientation),
AnchorPoseBLocal = new Pose(leftUpperArm.Pose.ToLocalPosition(leftElbowJointPosition), leftUpperArm.Pose.Orientation.Inverse * elbowAxisOrientation),
Minimum = -2,
Maximum = 0,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(leftElbowJoint);
Vector3F rightElbowJointPosition = new Vector3F(0.451f * scale, 0.071f * scale, -0.538f * scale) + ragdollPosition;
HingeJoint rightElbowJoint = new HingeJoint
{
BodyA = rightLowerArm,
BodyB = rightUpperArm,
AnchorPoseALocal = new Pose(rightLowerArm.Pose.ToLocalPosition(rightElbowJointPosition), rightLowerArm.Pose.Orientation.Inverse * elbowAxisOrientation),
AnchorPoseBLocal = new Pose(rightUpperArm.Pose.ToLocalPosition(rightElbowJointPosition), rightUpperArm.Pose.Orientation.Inverse * elbowAxisOrientation),
Minimum = 0,
Maximum = 2,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(rightElbowJoint);
Vector3F leftHipJointPosition = new Vector3F(-0.107f * scale, 0.049f * scale, 0.026f * scale) + ragdollPosition;
HingeJoint leftHipJoint = new HingeJoint
{
BodyA = pelvis,
BodyB = leftUpperLeg,
AnchorPoseALocal = new Pose(pelvis.Pose.ToLocalPosition(leftHipJointPosition)),
AnchorPoseBLocal = new Pose(leftUpperLeg.Pose.ToLocalPosition(leftHipJointPosition), leftUpperLeg.Pose.Orientation.Inverse),
Minimum = -0.1f,
Maximum = 1.2f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(leftHipJoint);
Vector3F rightHipJointPosition = new Vector3F(0.107f * scale, 0.049f * scale, 0.026f * scale) + ragdollPosition;
HingeJoint rightHipJoint = new HingeJoint
{
BodyA = pelvis,
BodyB = rightUpperLeg,
AnchorPoseALocal = new Pose(pelvis.Pose.ToLocalPosition(rightHipJointPosition)),
AnchorPoseBLocal = new Pose(rightUpperLeg.Pose.ToLocalPosition(rightHipJointPosition), rightUpperLeg.Pose.Orientation.Inverse),
Minimum = -0.1f,
Maximum = 1.2f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(rightHipJoint);
Vector3F leftKneeJointPosition = new Vector3F(-0.118f * scale, -0.012f * scale, 0.439f * scale) + ragdollPosition;
HingeJoint leftKneeJoint = new HingeJoint
{
BodyA = leftLowerLeg,
BodyB = leftUpperLeg,
AnchorPoseALocal = new Pose(leftLowerLeg.Pose.ToLocalPosition(leftKneeJointPosition)),
AnchorPoseBLocal = new Pose(leftUpperLeg.Pose.ToLocalPosition(leftKneeJointPosition)),
Minimum = 0,
Maximum = 1.7f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(leftKneeJoint);
Vector3F rightKneeJointPosition = new Vector3F(0.118f * scale, -0.012f * scale, 0.439f * scale) + ragdollPosition;
HingeJoint rightKneeJoint = new HingeJoint
{
BodyA = rightLowerLeg,
BodyB = rightUpperLeg,
AnchorPoseALocal = new Pose(rightLowerLeg.Pose.ToLocalPosition(rightKneeJointPosition)),
AnchorPoseBLocal = new Pose(rightUpperLeg.Pose.ToLocalPosition(rightKneeJointPosition)),
Minimum = 0,
Maximum = 1.7f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(rightKneeJoint);
Vector3F leftAnkleJointPosition = new Vector3F(-0.118f * scale, -0.016f * scale, 0.861f * scale) + ragdollPosition;
HingeJoint leftAnkleJoint = new HingeJoint
{
BodyA = leftFoot,
BodyB = leftLowerLeg,
AnchorPoseALocal = new Pose(leftFoot.Pose.ToLocalPosition(leftAnkleJointPosition)),
AnchorPoseBLocal = new Pose(leftLowerLeg.Pose.ToLocalPosition(leftAnkleJointPosition), leftLowerLeg.Pose.Orientation.Inverse),
Minimum = -0.4f,
Maximum = 0.9f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(leftAnkleJoint);
Vector3F rightAnkleJointPosition = new Vector3F(0.118f * scale, -0.016f * scale, 0.861f * scale) + ragdollPosition;
HingeJoint rightAnkleJoint = new HingeJoint
{
BodyA = rightFoot,
BodyB = rightLowerLeg,
AnchorPoseALocal = new Pose(rightFoot.Pose.ToLocalPosition(rightAnkleJointPosition)),
AnchorPoseBLocal = new Pose(rightLowerLeg.Pose.ToLocalPosition(rightAnkleJointPosition), rightLowerLeg.Pose.Orientation.Inverse),
Minimum = -0.4f,
Maximum = 0.9f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(rightAnkleJoint);
#endregion
#region ----- Add damping to improve stability -----
if (DampingEnabled)
{
// Damping removes jiggling and improves stability.
AddDamping(pelvis, torso);
AddDamping(torso, head);
AddDamping(torso, leftUpperArm);
AddDamping(leftUpperArm, leftLowerArm);
AddDamping(torso, rightUpperArm);
AddDamping(rightUpperArm, rightLowerArm);
AddDamping(pelvis, leftUpperLeg);
AddDamping(pelvis, rightUpperLeg);
AddDamping(leftUpperLeg, leftLowerLeg);
AddDamping(rightUpperLeg, rightLowerLeg);
AddDamping(leftLowerLeg, leftFoot);
AddDamping(rightLowerLeg, rightFoot);
}
#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>
/// <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
}
public BuoyancySample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// ----- Buoyancy Force Effect
// Buoyancy is a force effect that lets bodies swim in water. The water area is
// defined by two properties:
// - Buoyancy.AreaOfEffect defines which objects are affected.
// - Buoyancy.Surface defines the water level within this area.
// The area of effect can be defined in different ways. In this sample we will use
// a geometric object ("trigger volume").
// First, define the shape of the water area. We will create simple pool.
Shape poolShape = new BoxShape(16, 10, 16);
Vector3F poolCenter = new Vector3F(0, -5, 0);
// Then create a geometric object for the water area. (A GeometricObject is required
// to position the shape in the world. A GeometricObject stores shape, scale, position,
// orientation, ...)
GeometricObject waterGeometry = new GeometricObject(poolShape, new Pose(poolCenter));
// Then create a collision object for the geometric object. (A CollisionObject required
// because the geometry should be used for collision detection with other objects.)
_waterCollisionObject = new CollisionObject(waterGeometry)
{
// Assign the object to a different collision group:
// The Grab component (see Grab.cs) uses a ray to perform hit tests. We don't want the ray
// to collide with the water. Therefore, we need to assign the water collision object to a
// different collision group. The general geometry is in collision group 0. The rays are in
// collision group 2. Add the water to collision group 1. Collision between 0 and 2 are
// enabled. Collision between 1 and 2 need to be disabled - this collision filter was set
// in PhysicsGame.cs.
CollisionGroup = 1,
// Set the type to "Trigger". This improves the performance because the collision
// detection does not need to compute detailed contact information. The collision
// detection only returns whether an objects has contact with the water.
Type = CollisionObjectType.Trigger,
};
// The collision object needs to be added into the collision domain of the simulation.
Simulation.CollisionDomain.CollisionObjects.Add(_waterCollisionObject);
// Now we can add the buoyancy effect.
Buoyancy buoyancy = new Buoyancy
{
AreaOfEffect = new GeometricAreaOfEffect(_waterCollisionObject),
Surface = new Plane(Vector3F.Up, 0),
Density = 1000f, // The density of water (1000 kg/m³).
AngularDrag = 0.4f,
LinearDrag = 4f,
// Optional: Let the objects drift in the water by setting a flow velocity.
//Velocity = new Vector3F(-0.5f, 0, 0.5f),
};
Simulation.ForceEffects.Add(buoyancy);
// Add static area around the pool.
RigidBody bottom = new RigidBody(new BoxShape(36, 2, 36))
{
MotionType = MotionType.Static,
Pose = new Pose(new Vector3F(0, -11, 0)),
};
Simulation.RigidBodies.Add(bottom);
RigidBody left = new RigidBody(new BoxShape(10, 10, 36))
{
MotionType = MotionType.Static,
Pose = new Pose(new Vector3F(-13, -5, 0)),
};
Simulation.RigidBodies.Add(left);
RigidBody right = new RigidBody(new BoxShape(10, 10, 36))
{
MotionType = MotionType.Static,
Pose = new Pose(new Vector3F(13, -5, 0)),
};
Simulation.RigidBodies.Add(right);
RigidBody front = new RigidBody(new BoxShape(16, 10, 10))
{
MotionType = MotionType.Static,
Pose = new Pose(new Vector3F(0, -5, 13)),
};
Simulation.RigidBodies.Add(front);
RigidBody back = new RigidBody(new BoxShape(16, 10, 10))
{
MotionType = MotionType.Static,
Pose = new Pose(new Vector3F(0, -5, -13)),
};
Simulation.RigidBodies.Add(back);
// ----- Add some random objects to test the effect.
// Note: Objects swim if their density is less than the density of water. They sink
// if the density is greater than the density of water.
// We can define the density of objects by explicitly setting the mass.
// Add a swimming board.
BoxShape raftShape = new BoxShape(4, 0.3f, 4);
MassFrame raftMass = MassFrame.FromShapeAndDensity(raftShape, Vector3F.One, 700, 0.01f, 3);
RigidBody raft = new RigidBody(raftShape, raftMass, null)
{
Pose = new Pose(new Vector3F(0, 4, 0)),
};
Simulation.RigidBodies.Add(raft);
// Add some boxes on top of the swimming board.
BoxShape boxShape = new BoxShape(1, 1, 1);
MassFrame boxMass = MassFrame.FromShapeAndDensity(boxShape, Vector3F.One, 700, 0.01f, 3);
for (int i = 0; i < 5; i++)
{
RigidBody box = new RigidBody(boxShape, boxMass, null)
{
Pose = new Pose(new Vector3F(0, 5 + i * 1.1f, 0)),
};
Simulation.RigidBodies.Add(box);
}
// Add some "heavy stones" represented as spheres.
SphereShape stoneShape = new SphereShape(0.5f);
MassFrame stoneMass = MassFrame.FromShapeAndDensity(stoneShape, Vector3F.One, 2500, 0.01f, 3);
for (int i = 0; i < 10; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-9, 9);
position.Y = 5;
RigidBody stone = new RigidBody(stoneShape, stoneMass, null)
{
Pose = new Pose(position),
};
Simulation.RigidBodies.Add(stone);
}
// Add some very light objects.
CylinderShape cylinderShape = new CylinderShape(0.3f, 1);
MassFrame cylinderMass = MassFrame.FromShapeAndDensity(cylinderShape, Vector3F.One, 500, 0.01f, 3);
for (int i = 0; i < 10; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-9, 9);
position.Y = 5;
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
RigidBody cylinder = new RigidBody(cylinderShape, cylinderMass, null)
{
Pose = new Pose(position, orientation),
};
Simulation.RigidBodies.Add(cylinder);
}
}
public BridgeSample(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);
// We add another damping effect that acts only on the suspension bridge parts.
// This damping uses higher damping factors than the standard damping. It makes the
// bridge movement smoother and more stable.
// We use a ListAreaOfEffect. So the additional damping acts only on bodies in this list.
ListAreaOfEffect boardList = new ListAreaOfEffect(new List <RigidBody>());
Damping damping = new Damping
{
AreaOfEffect = boardList,
AngularDamping = 1f,
LinearDamping = 0.5f
};
Simulation.ForceEffects.Add(damping);
const int numberOfBoards = 20;
BoxShape boardShape = new BoxShape(0.8f, 0.1f, 1.5f);
RigidBody lastBoard = null;
for (int i = 0; i < numberOfBoards; i++)
{
// A single plank of the bridge.
RigidBody body = new RigidBody(boardShape)
{
Pose = new Pose(new Vector3F(-10 + boardShape.WidthX * i, 4, 0))
};
Simulation.RigidBodies.Add(body);
// Add the body to the list of the additional damping force effect.
boardList.RigidBodies.Add(body);
if (lastBoard != null)
{
// Connect the last body with current body using a hinge.
HingeJoint hinge = new HingeJoint
{
BodyA = lastBoard,
// The attachment point is at the right side of the board.
// --> To define the constraint anchor orientation:
// The columns are the axes. We set the local z axis in the first column. This is
// the hinge axis. In the other two columns we set two orthonormal vectors.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseALocal = new Pose(new Vector3F(boardShape.WidthX / 2, 0, 0),
new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0)),
BodyB = body,
// The attachment point is at the left side of the board.
// The anchor orientation is defined as above.
AnchorPoseBLocal = new Pose(new Vector3F(-boardShape.WidthX / 2, 0, 0),
new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0)),
CollisionEnabled = false,
// ErrorReduction and Softness are tweaked to get a stable and smooth bridge
// movement.
ErrorReduction = 0.3f,
Softness = 0.00005f,
};
Simulation.Constraints.Add(hinge);
}
else if (i == 0)
{
// To attach the bridge somewhere, connect the the first board to a fixed position in the
// world.
HingeJoint hinge = new HingeJoint
{
BodyA = Simulation.World,
AnchorPoseALocal = new Pose(new Vector3F(-9, 3, 0),
new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0)),
BodyB = body,
AnchorPoseBLocal = new Pose(new Vector3F(-boardShape.WidthX / 2, 0, 0),
new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0)),
};
Simulation.Constraints.Add(hinge);
}
if (i == numberOfBoards - 1)
{
// To attach the bridge somewhere, connect the the last board to a fixed position in the
// world.
HingeJoint hinge = new HingeJoint
{
BodyA = Simulation.World,
AnchorPoseALocal = new Pose(new Vector3F(9, 3, 0),
new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0)),
BodyB = body,
AnchorPoseBLocal = new Pose(new Vector3F(boardShape.WidthX / 2, 0, 0),
new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0)),
};
Simulation.Constraints.Add(hinge);
}
lastBoard = body;
}
// The bridge is ready.
// Now, add some ramps so that the character controller can walk up to the bridge.
BoxShape rampShape = new BoxShape(10, 10, 2);
RigidBody ramp0 = new RigidBody(rampShape)
{
Pose = new Pose(new Vector3F(-12.5f, -3f, 0), Matrix33F.CreateRotationZ(0.3f)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(ramp0);
RigidBody ramp1 = new RigidBody(rampShape)
{
Pose = new Pose(new Vector3F(12.5f, -3f, 0), Matrix33F.CreateRotationZ(-0.3f)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(ramp1);
// Drop a few light boxes onto the bridge.
BoxShape boxShape = new BoxShape(1, 1, 1);
MassFrame boxMass = MassFrame.FromShapeAndDensity(boxShape, Vector3F.One, 100, 0.01f, 3);
for (int i = 0; i < 10; i++)
{
Vector3F randomPosition = new Vector3F(RandomHelper.Random.NextFloat(-10, 10), 5, 0);
QuaternionF randomOrientation = RandomHelper.Random.NextQuaternionF();
RigidBody body = new RigidBody(boxShape, boxMass, null)
{
Pose = new Pose(randomPosition, randomOrientation),
};
Simulation.RigidBodies.Add(body);
}
}
public ConstraintVehicleObject(IServiceLocator services)
{
Name = "Vehicle";
_services = services;
_inputService = services.GetInstance <IInputService>();
_simulation = services.GetInstance <Simulation>();
// Load models for rendering.
var contentManager = services.GetInstance <ContentManager>();
_vehicleModelNode = contentManager.Load <ModelNode>("Car/Car").Clone();
_wheelModelNodes = new ModelNode[4];
_wheelModelNodes[0] = contentManager.Load <ModelNode>("Car/Wheel").Clone();
_wheelModelNodes[1] = _wheelModelNodes[0].Clone();
_wheelModelNodes[2] = _wheelModelNodes[0].Clone();
_wheelModelNodes[3] = _wheelModelNodes[0].Clone();
// Add wheels under the car model node.
_vehicleModelNode.Children.Add(_wheelModelNodes[0]);
_vehicleModelNode.Children.Add(_wheelModelNodes[1]);
_vehicleModelNode.Children.Add(_wheelModelNodes[2]);
_vehicleModelNode.Children.Add(_wheelModelNodes[3]);
// ----- Create the chassis of the car.
// The Vehicle needs a rigid body that represents the chassis. This can be any shape (e.g.
// a simple BoxShape). In this example we will build a convex polyhedron from the car model.
// 1. Extract the vertices from the car model.
// The car model has ~10,000 vertices. It consists of a MeshNode for the glass
// parts and a MeshNode "Car" for the chassis.
var meshNode = _vehicleModelNode.GetDescendants()
.OfType <MeshNode>()
.First(mn => mn.Name == "Car");
var mesh = MeshHelper.ToTriangleMesh(meshNode.Mesh);
// Apply the transformation of the mesh node.
mesh.Transform(meshNode.PoseWorld * Matrix.CreateScale(meshNode.ScaleWorld));
// 2. (Optional) Create simplified convex hull from mesh.
// We could also skip this step and directly create a convex polyhedron from the mesh using
// var chassisShape = new ConvexPolyhedron(mesh.Vertices);
// However, the convex polyhedron would still have 500-600 vertices.
// We can reduce the number of vertices by using the GeometryHelper.
// Create a convex hull for mesh with max. 64 vertices. Additional, shrink the hull by 4 cm.
var convexHull = GeometryHelper.CreateConvexHull(mesh.Vertices, 64, -0.04f);
// 3. Create convex polyhedron shape using the vertices of the convex hull.
var chassisShape = new ConvexPolyhedron(convexHull.Vertices.Select(v => v.Position));
// (Note: Building convex hulls and convex polyhedra are time-consuming. To save loading time
// we should build the shape in the XNA content pipeline. See other DigitalRune Physics
// Samples.)
// The mass properties of the car. We use a mass of 800 kg.
var mass = MassFrame.FromShapeAndMass(chassisShape, Vector3.One, 800, 0.1f, 1);
// Trick: We artificially modify the center of mass of the rigid body. Lowering the center
// of mass makes the car more stable against rolling in tight curves.
// We could also modify mass.Inertia for other effects.
var pose = mass.Pose;
pose.Position.Y -= 0.5f; // Lower the center of mass.
pose.Position.Z = -0.5f; // The center should be below the driver.
// (Note: The car model is not exactly centered.)
mass.Pose = pose;
// Material for the chassis.
var material = new UniformMaterial
{
Restitution = 0.1f,
StaticFriction = 0.2f,
DynamicFriction = 0.2f
};
var chassis = new RigidBody(chassisShape, mass, material)
{
Pose = new Pose(new Vector3(0, 2, 0)), // Start position
UserData = "NoDraw", // (Remove this line to render the collision model.)
};
// ----- Create the vehicle.
Vehicle = new ConstraintVehicle(_simulation, chassis);
// Add 4 wheels.
Vehicle.Wheels.Add(new ConstraintWheel {
Offset = new Vector3(-0.9f, 0.6f, -2.0f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 2
}); // Front left
Vehicle.Wheels.Add(new ConstraintWheel {
Offset = new Vector3(0.9f, 0.6f, -2.0f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 2
}); // Front right
Vehicle.Wheels.Add(new ConstraintWheel {
Offset = new Vector3(-0.9f, 0.6f, 0.98f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 1.8f
}); // Back left
Vehicle.Wheels.Add(new ConstraintWheel {
Offset = new Vector3(0.9f, 0.6f, 0.98f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 1.8f
}); // Back right
// Vehicles are disabled per default. This way we can create the vehicle and the simulation
// objects are only added when needed.
Vehicle.Enabled = false;
}
public ConstraintCarSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Create a material with high friction - this will be used for the ground and the wheels
// to give the wheels some traction.
UniformMaterial roughMaterial = new UniformMaterial
{
DynamicFriction = 1,
StaticFriction = 1,
};
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3.UnitY, 0), null, roughMaterial)
{
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// Now, we build a car out of one box for the chassis and 4 cylindric wheels.
// Front wheels are fixed with Hinge2Joints and motorized with AngularVelocityMotors.
// Back wheels are fixed with HingeJoints and not motorized. - This creates a sloppy
// car configuration. Please note that cars for racing games are not normally built with
// simple constraints - nevertheless, it is funny to do and play with it.
// Check out the "Vehicle Sample" (not included in this project)! The "Vehicle Sample"
// provides a robust ray-car implementation.)
// ----- Chassis
BoxShape chassisShape = new BoxShape(1.7f, 1, 4f);
MassFrame chassisMass = MassFrame.FromShapeAndDensity(chassisShape, Vector3.One, 200, 0.01f, 3);
// Here is a trick: The car topples over very easily. By lowering the center of mass we
// make it more stable.
chassisMass.Pose = new Pose(new Vector3(0, -1, 0));
RigidBody chassis = new RigidBody(chassisShape, chassisMass, null)
{
Pose = new Pose(new Vector3(0, 1, 0)),
};
Simulation.RigidBodies.Add(chassis);
// ------ Wheels
CylinderShape cylinderShape = new CylinderShape(0.4f, 0.3f);
MassFrame wheelMass = MassFrame.FromShapeAndDensity(cylinderShape, Vector3.One, 500, 0.01f, 3);
RigidBody wheelFrontLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3(0, 1, 0), Matrix.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelFrontLeft);
RigidBody wheelFrontRight = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3(0, 1, 0), Matrix.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelFrontRight);
RigidBody wheelBackLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3(0, 1, 0), Matrix.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelBackLeft);
RigidBody wheelBackRight = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3(0, 1, 0), Matrix.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelBackRight);
// ----- Hinge2Joints for the front wheels.
// A Hinge2Joint allows a limited rotation on the first constraint axis - the steering
// axis. The second constraint axis is locked and the third constraint axis is the
// rotation axis of the wheels.
_frontLeftHinge = new Hinge2Joint
{
BodyA = chassis,
// --> To define the constraint anchor orientation for the chassis:
// The columns are the axes. We set the local y axis in the first column. This is
// steering axis. In the last column we set the -x axis. This is the wheel rotation axis.
// In the middle column is a vector that is normal to the first and last axis.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseALocal = new Pose(new Vector3(-0.9f, -0.4f, -1.4f),
new Matrix(0, 0, -1,
1, 0, 0,
0, -1, 0)),
BodyB = wheelFrontLeft,
// --> To define the constraint anchor orientation for the chassis:
// The columns are the axes. We set the local x axis in the first column. This is
// steering axis. In the last column we set the y axis. This is the wheel rotation axis.
// (In local space of a cylinder the cylinder axis is the +y axis.)
// In the middle column is a vector that is normal to the first and last axis.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseBLocal = new Pose(new Matrix(1, 0, 0,
0, 0, 1,
0, -1, 0)),
CollisionEnabled = false,
Minimum = new Vector2F(-0.7f, float.NegativeInfinity),
Maximum = new Vector2F(0.7f, float.PositiveInfinity),
};
Simulation.Constraints.Add(_frontLeftHinge);
_frontRightHinge = new Hinge2Joint
{
BodyA = chassis,
BodyB = wheelFrontRight,
AnchorPoseALocal = new Pose(new Vector3(0.9f, -0.4f, -1.4f),
new Matrix(0, 0, -1,
1, 0, 0,
0, -1, 0)),
AnchorPoseBLocal = new Pose(new Matrix(1, 0, 0,
0, 0, 1,
0, -1, 0)),
CollisionEnabled = false,
Minimum = new Vector2F(-0.7f, float.NegativeInfinity),
Maximum = new Vector2F(0.7f, float.PositiveInfinity),
};
Simulation.Constraints.Add(_frontRightHinge);
// ----- HingeJoints for the back wheels.
// Hinges allow free rotation on the first constraint axis.
HingeJoint backLeftHinge = new HingeJoint
{
BodyA = chassis,
AnchorPoseALocal = new Pose(new Vector3(-0.9f, -0.4f, 1.4f)),
BodyB = wheelBackLeft,
// --> To define the constraint anchor orientation:
// The columns are the axes. We set the local y axis in the first column. This is
// cylinder axis and should be the hinge axis. In the other two columns we set two
// orthonormal vectors.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseBLocal = new Pose(new Matrix(0, 0, 1,
1, 0, 0,
0, 1, 0)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(backLeftHinge);
HingeJoint backRightHinge = new HingeJoint
{
BodyA = chassis,
AnchorPoseALocal = new Pose(new Vector3(0.9f, -0.4f, 1.4f)),
BodyB = wheelBackRight,
AnchorPoseBLocal = new Pose(new Matrix(0, 0, 1,
1, 0, 0,
0, 1, 0)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(backRightHinge);
// ----- Motors for the front wheels.
// (Motor axes and target velocities are set in Update() below.)
_frontLeftMotor = new AngularVelocityMotor
{
BodyA = chassis,
BodyB = wheelFrontLeft,
CollisionEnabled = false,
// We use "single axis mode", which means the motor drives only on axis and does not
// block motion orthogonal to this axis. - Rotation about the orthogonal axes is already
// controlled by the Hinge2Joint.
UseSingleAxisMode = true,
// The motor has only limited power:
MaxForce = 50000,
};
Simulation.Constraints.Add(_frontLeftMotor);
_frontRightMotor = new AngularVelocityMotor
{
BodyA = chassis,
BodyB = wheelFrontRight,
CollisionEnabled = false,
UseSingleAxisMode = true,
MaxForce = 50000,
};
Simulation.Constraints.Add(_frontRightMotor);
// ----- Drop a few boxes to create obstacles.
BoxShape boxShape = new BoxShape(1, 1, 1);
MassFrame boxMass = MassFrame.FromShapeAndDensity(boxShape, Vector3.One, 100, 0.01f, 3);
for (int i = 0; i < 20; i++)
{
Vector3 position = RandomHelper.Random.NextVector3(-20, 20);
position.Y = 5;
Quaternion orientation = RandomHelper.Random.NextQuaternion();
RigidBody body = new RigidBody(boxShape, boxMass, null)
{
Pose = new Pose(position, orientation),
};
Simulation.RigidBodies.Add(body);
}
}
/// <summary>
/// Creates a <see cref="Ragdoll"/> for an Xbox LIVE Avatar. (Only available on Xbox 360.)
/// </summary>
/// <param name="skeleton">The skeleton of the Xbox LIVE Avatar.</param>
/// <param name="simulation">The simulation.</param>
/// <returns>The avatar ragdoll.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeleton"/> or <paramref name="simulation"/> is
/// <see langword="null"/>.
/// </exception>
/// <remarks>
/// This method is available only in the Xbox 360 build of the
/// DigitalRune.Physics.Specialized.dll.
/// </remarks>
public static Ragdoll CreateAvatarRagdoll(Skeleton skeleton, Simulation simulation)
{
if (skeleton == null)
{
throw new ArgumentNullException("skeleton");
}
if (simulation == null)
{
throw new ArgumentNullException("simulation");
}
var ragdoll = new Ragdoll();
// The lists ragdoll.Bodies, ragdoll.BodyOffsets and _motors contain one entry per bone - even if there
// is no RigidBody for this bone. - This wastes memory but simplifies the code.
for (int i = 0; i < AvatarRenderer.BoneCount; i++)
{
ragdoll.Bodies.Add(null);
ragdoll.BodyOffsets.Add(Pose.Identity);
ragdoll.Joints.Add(null);
ragdoll.Limits.Add(null);
ragdoll.Motors.Add(null);
}
// ----- Create bodies.
// We use the same mass for all bodies. This is not physically correct but it makes the
// simulation more stable, for several reasons:
// - It is better to avoid large mass differences. Therefore, all limbs have the same mass.
// - Capsule shapes have a low inertia value about their height axis. This causes instability
// and it is better to use larger inertia values.
var massFrame = MassFrame.FromShapeAndMass(new SphereShape(0.2f), Vector3F.One, 4, 0.1f, 1);
// Use standard material.
var material = new UniformMaterial();
// Create rigid bodies for the important bones. The shapes have been manually adapted to
// produce useful results for thin and overweight avatars.
// Without offset, the bodies are centered at the joint. ragdoll.BodyOffsets stores an offset pose
// for each body. Instead, we could use TransformedShape but we can easily handle that
// ourselves.
// The collar bones are special, they use dummy shapes and are only used to connect the
// shoulder bones.
ragdoll.Bodies[(int)AvatarBone.Root] = new RigidBody(new BoxShape(0.22f, 0.16f, 0.16f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.Root] = new Pose(new Vector3F(0, -0.08f, -0.01f), QuaternionF.CreateRotationX(-0.0f));
ragdoll.Bodies[(int)AvatarBone.BackLower] = new RigidBody(new BoxShape(0.22f, 0.16f, 0.16f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.BackLower] = new Pose(new Vector3F(0, 0.08f, -0.01f), QuaternionF.CreateRotationX(-0.0f));
ragdoll.Bodies[(int)AvatarBone.BackUpper] = new RigidBody(new BoxShape(0.22f, 0.16f, 0.16f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.BackUpper] = new Pose(new Vector3F(0, 0.08f, -0.01f), QuaternionF.CreateRotationX(-0.1f));
ragdoll.Bodies[(int)AvatarBone.Neck] = new RigidBody(new CapsuleShape(0.04f, 0.09f), massFrame, material);
ragdoll.Bodies[(int)AvatarBone.Head] = new RigidBody(new SphereShape(0.15f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.Head] = new Pose(new Vector3F(0, 0.1f, 0));
ragdoll.Bodies[(int)AvatarBone.CollarLeft] = new RigidBody(Shape.Empty, massFrame, material);
ragdoll.Bodies[(int)AvatarBone.CollarRight] = new RigidBody(Shape.Empty, massFrame, material);
ragdoll.Bodies[(int)AvatarBone.ShoulderLeft] = new RigidBody(new CapsuleShape(0.04f, 0.25f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.ShoulderLeft] = new Pose(new Vector3F(0.08f, 0, -0.02f), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
ragdoll.Bodies[(int)AvatarBone.ShoulderRight] = new RigidBody(new CapsuleShape(0.04f, 0.25f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.ShoulderRight] = new Pose(new Vector3F(-0.08f, 0, -0.02f), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
ragdoll.Bodies[(int)AvatarBone.ElbowLeft] = new RigidBody(new CapsuleShape(0.04f, 0.21f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.ElbowLeft] = new Pose(new Vector3F(0.06f, 0, -0.02f), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
ragdoll.Bodies[(int)AvatarBone.ElbowRight] = new RigidBody(new CapsuleShape(0.04f, 0.21f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.ElbowRight] = new Pose(new Vector3F(-0.06f, 0, -0.02f), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
ragdoll.Bodies[(int)AvatarBone.WristLeft] = new RigidBody(new BoxShape(0.1f, 0.04f, 0.1f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.WristLeft] = new Pose(new Vector3F(0.06f, -0.02f, -0.01f), QuaternionF.CreateRotationZ(0.0f));
ragdoll.Bodies[(int)AvatarBone.WristRight] = new RigidBody(new BoxShape(0.1f, 0.04f, 0.1f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.WristRight] = new Pose(new Vector3F(-0.06f, -0.02f, -0.01f), QuaternionF.CreateRotationZ(0.0f));
ragdoll.Bodies[(int)AvatarBone.HipLeft] = new RigidBody(new CapsuleShape(0.06f, 0.34f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.HipLeft] = new Pose(new Vector3F(0, -0.14f, -0.02f), QuaternionF.CreateRotationX(0.1f));
ragdoll.Bodies[(int)AvatarBone.HipRight] = new RigidBody(new CapsuleShape(0.06f, 0.34f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.HipRight] = new Pose(new Vector3F(0, -0.14f, -0.02f), QuaternionF.CreateRotationX(0.1f));
ragdoll.Bodies[(int)AvatarBone.KneeLeft] = new RigidBody(new CapsuleShape(0.06f, 0.36f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.KneeLeft] = new Pose(new Vector3F(0, -0.18f, -0.04f), QuaternionF.CreateRotationX(0.1f));
ragdoll.Bodies[(int)AvatarBone.KneeRight] = new RigidBody(new CapsuleShape(0.06f, 0.36f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.KneeRight] = new Pose(new Vector3F(0, -0.18f, -0.04f), QuaternionF.CreateRotationX(0.1f));
ragdoll.Bodies[(int)AvatarBone.AnkleLeft] = new RigidBody(new BoxShape(0.1f, 0.06f, 0.22f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.AnkleLeft] = new Pose(new Vector3F(0, -0.07f, 0.05f), QuaternionF.CreateRotationZ(0));
ragdoll.Bodies[(int)AvatarBone.AnkleRight] = new RigidBody(new BoxShape(0.1f, 0.06f, 0.22f), massFrame, material);
ragdoll.BodyOffsets[(int)AvatarBone.AnkleRight] = new Pose(new Vector3F(0, -0.07f, 0.05f), QuaternionF.CreateRotationZ(0));
// ----- Add joint constraints.
const float jointErrorReduction = 0.2f;
const float jointSoftness = 0.0001f;
AddJoint(ragdoll, skeleton, AvatarBone.Root, AvatarBone.BackLower, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.BackLower, AvatarBone.BackUpper, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.Neck, 0.6f, 0.000001f);
AddJoint(ragdoll, skeleton, AvatarBone.Neck, AvatarBone.Head, 0.6f, 0.000001f);
AddJoint(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.CollarLeft, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.CollarRight, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.CollarLeft, AvatarBone.ShoulderLeft, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.CollarRight, AvatarBone.ShoulderRight, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.ShoulderLeft, AvatarBone.ElbowLeft, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.ShoulderRight, AvatarBone.ElbowRight, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.ElbowLeft, AvatarBone.WristLeft, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.ElbowRight, AvatarBone.WristRight, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.Root, AvatarBone.HipLeft, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.Root, AvatarBone.HipRight, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.HipLeft, AvatarBone.KneeLeft, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.HipRight, AvatarBone.KneeRight, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.KneeLeft, AvatarBone.AnkleLeft, jointErrorReduction, jointSoftness);
AddJoint(ragdoll, skeleton, AvatarBone.KneeRight, AvatarBone.AnkleRight, jointErrorReduction, jointSoftness);
// ----- Add constraint limits.
// We use TwistSwingLimits to define an allowed twist and swing cone for the joints.
// Exceptions are the back and knees, where we use AngularLimits to create hinges.
// (We could also create a hinge with a TwistSwingLimit where the twist axis is the hinge
// axis and no swing is allowed - but AngularLimits create more stable hinges.)
// Another exception are the collar bones joint. We use AngularLimits to disallow any
// rotations.
AddAngularLimit(ragdoll, skeleton, AvatarBone.Root, AvatarBone.BackLower,
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.BackLower).Rotation.Conjugated.ToRotationMatrix33(),
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.BackLower).Rotation.Conjugated.ToRotationMatrix33(),
new Vector3F(-0.3f, 0, 0), new Vector3F(0.3f, 0, 0));
AddAngularLimit(ragdoll, skeleton, AvatarBone.BackLower, AvatarBone.BackUpper,
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.BackUpper).Rotation.Conjugated.ToRotationMatrix33(),
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.BackUpper).Rotation.Conjugated.ToRotationMatrix33(),
new Vector3F(-0.3f, 0, 0), new Vector3F(0.4f, 0, 0));
var rotationZ90Degrees = Matrix33F.CreateRotationZ(ConstantsF.PiOver2);
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.Neck, rotationZ90Degrees, rotationZ90Degrees, new Vector3F(-0.1f, -0.3f, -0.3f), new Vector3F(+0.1f, +0.3f, +0.3f));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.Neck, AvatarBone.Head, rotationZ90Degrees, rotationZ90Degrees, new Vector3F(-0.1f, -0.6f, -0.6f), new Vector3F(+0.1f, +0.6f, +0.6f));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.CollarLeft,
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.CollarLeft).Rotation.Conjugated.ToRotationMatrix33(),
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.CollarLeft).Rotation.Conjugated.ToRotationMatrix33(),
new Vector3F(0), new Vector3F(0));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.CollarRight,
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.CollarRight).Rotation.Conjugated.ToRotationMatrix33(),
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.CollarRight).Rotation.Conjugated.ToRotationMatrix33(),
new Vector3F(0), new Vector3F(0));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.CollarLeft, AvatarBone.ShoulderLeft, Matrix33F.Identity, Matrix33F.CreateRotationY(0.7f), new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(+0.7f, +1.2f, +1.2f));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.CollarRight, AvatarBone.ShoulderRight, Matrix33F.Identity, Matrix33F.CreateRotationY(-0.7f), new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(+0.7f, +1.2f, +1.2f));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.ShoulderLeft, AvatarBone.ElbowLeft, Matrix33F.Identity, Matrix33F.CreateRotationY(1.2f), new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(+0.7f, +1.2f, +1.2f));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.ShoulderRight, AvatarBone.ElbowRight, Matrix33F.Identity, Matrix33F.CreateRotationY(-1.2f), new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(+0.7f, +1.2f, +1.2f));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.ElbowLeft, AvatarBone.WristLeft, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.7f, -0.7f, -0.7f), new Vector3F(+0.7f, +0.7f, +0.7f));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.ElbowRight, AvatarBone.WristRight, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.7f, -0.7f, -0.7f), new Vector3F(+0.7f, +0.7f, +0.7f));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.Root, AvatarBone.HipLeft, rotationZ90Degrees, Matrix33F.CreateRotationX(-1.2f) * Matrix33F.CreateRotationZ(ConstantsF.PiOver2 + 0.2f), new Vector3F(-0.1f, -1.5f, -0.7f), new Vector3F(+0.1f, +1.5f, +0.7f));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.Root, AvatarBone.HipRight, rotationZ90Degrees, Matrix33F.CreateRotationX(-1.2f) * Matrix33F.CreateRotationZ(ConstantsF.PiOver2 - 0.2f), new Vector3F(-0.1f, -1.5f, -0.7f), new Vector3F(+0.1f, +1.5f, +0.7f));
AddAngularLimit(ragdoll, skeleton, AvatarBone.HipLeft, AvatarBone.KneeLeft,
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.KneeLeft).Rotation.Conjugated.ToRotationMatrix33(),
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.KneeLeft).Rotation.Conjugated.ToRotationMatrix33(),
new Vector3F(0, 0, 0), new Vector3F(2.2f, 0, 0));
AddAngularLimit(ragdoll, skeleton, AvatarBone.HipRight, AvatarBone.KneeRight,
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.KneeRight).Rotation.Conjugated.ToRotationMatrix33(),
skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.KneeRight).Rotation.Conjugated.ToRotationMatrix33(),
new Vector3F(0, 0, 0), new Vector3F(2.2f, 0, 0));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.KneeLeft, AvatarBone.AnkleLeft, rotationZ90Degrees, rotationZ90Degrees, new Vector3F(-0.1f, -0.7f, -0.3f), new Vector3F(+0.1f, +0.7f, +0.3f));
AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.KneeRight, AvatarBone.AnkleRight, rotationZ90Degrees, rotationZ90Degrees, new Vector3F(-0.1f, -0.7f, -0.3f), new Vector3F(+0.1f, +0.7f, +0.3f));
// ----- Add motors
// We use QuaternionMotors to create forces that rotate the bones into desired poses.
// This can be used for damping, spring or animating the ragdoll.
AddMotor(ragdoll, (AvatarBone)(-1), AvatarBone.Root);
AddMotor(ragdoll, AvatarBone.Root, AvatarBone.BackLower);
AddMotor(ragdoll, AvatarBone.BackLower, AvatarBone.BackUpper);
AddMotor(ragdoll, AvatarBone.BackUpper, AvatarBone.Neck);
AddMotor(ragdoll, AvatarBone.Neck, AvatarBone.Head);
AddMotor(ragdoll, AvatarBone.BackUpper, AvatarBone.CollarLeft);
AddMotor(ragdoll, AvatarBone.BackUpper, AvatarBone.CollarRight);
AddMotor(ragdoll, AvatarBone.CollarLeft, AvatarBone.ShoulderLeft);
AddMotor(ragdoll, AvatarBone.CollarRight, AvatarBone.ShoulderRight);
AddMotor(ragdoll, AvatarBone.ShoulderLeft, AvatarBone.ElbowLeft);
AddMotor(ragdoll, AvatarBone.ShoulderRight, AvatarBone.ElbowRight);
AddMotor(ragdoll, AvatarBone.ElbowLeft, AvatarBone.WristLeft);
AddMotor(ragdoll, AvatarBone.ElbowRight, AvatarBone.WristRight);
AddMotor(ragdoll, AvatarBone.Root, AvatarBone.HipLeft);
AddMotor(ragdoll, AvatarBone.Root, AvatarBone.HipRight);
AddMotor(ragdoll, AvatarBone.HipLeft, AvatarBone.KneeLeft);
AddMotor(ragdoll, AvatarBone.HipRight, AvatarBone.KneeRight);
AddMotor(ragdoll, AvatarBone.KneeLeft, AvatarBone.AnkleLeft);
AddMotor(ragdoll, AvatarBone.KneeRight, AvatarBone.AnkleRight);
// ----- Set collision filters.
// Collisions between connected bones have been disabled with Constraint.CollisionEnabled
// = false in the joints. We need to disable a few other collisions.
// Following bodies do not collide with anything. They are only used to connect other
// bones.
ragdoll.Bodies[(int)AvatarBone.Neck].CollisionObject.Enabled = false;
ragdoll.Bodies[(int)AvatarBone.CollarLeft].CollisionObject.Enabled = false;
ragdoll.Bodies[(int)AvatarBone.CollarRight].CollisionObject.Enabled = false;
// We disable filters for following body pairs because they are usually penetrating each
// other, which needs to be ignored.
var filter = simulation.CollisionDomain.CollisionDetection.CollisionFilter as CollisionFilter;
if (filter != null)
{
filter.Set(ragdoll.Bodies[(int)AvatarBone.BackUpper].CollisionObject, ragdoll.Bodies[(int)AvatarBone.ShoulderLeft].CollisionObject, false);
filter.Set(ragdoll.Bodies[(int)AvatarBone.BackUpper].CollisionObject, ragdoll.Bodies[(int)AvatarBone.ShoulderRight].CollisionObject, false);
}
return(ragdoll);
}
private Ragdoll CreateRagdoll(MeshNode meshNode)
{
var mesh = meshNode.Mesh;
var skeleton = mesh.Skeleton;
// Extract the vertices from the mesh sorted per bone.
var verticesPerBone = new List <Vector3F> [skeleton.NumberOfBones];
// Also get the AABB of the model.
Aabb?aabb = null;
foreach (var submesh in mesh.Submeshes)
{
// Get vertex element info.
var vertexDeclaration = submesh.VertexBuffer.VertexDeclaration;
var vertexElements = vertexDeclaration.GetVertexElements();
// Get the vertex positions.
var positionElement = vertexElements.First(e => e.VertexElementUsage == VertexElementUsage.Position);
if (positionElement.VertexElementFormat != VertexElementFormat.Vector3)
{
throw new NotSupportedException("For vertex positions only VertexElementFormat.Vector3 is supported.");
}
var positions = new Vector3[submesh.VertexCount];
submesh.VertexBuffer.GetData(
submesh.StartVertex * vertexDeclaration.VertexStride + positionElement.Offset,
positions,
0,
submesh.VertexCount,
vertexDeclaration.VertexStride);
// Get the bone indices.
var boneIndexElement = vertexElements.First(e => e.VertexElementUsage == VertexElementUsage.BlendIndices);
if (boneIndexElement.VertexElementFormat != VertexElementFormat.Byte4)
{
throw new NotSupportedException();
}
var boneIndicesArray = new Byte4[submesh.VertexCount];
submesh.VertexBuffer.GetData(
submesh.StartVertex * vertexDeclaration.VertexStride + boneIndexElement.Offset,
boneIndicesArray,
0,
submesh.VertexCount,
vertexDeclaration.VertexStride);
// Get the bone weights.
var boneWeightElement = vertexElements.First(e => e.VertexElementUsage == VertexElementUsage.BlendWeight);
if (boneWeightElement.VertexElementFormat != VertexElementFormat.Vector4)
{
throw new NotSupportedException();
}
var boneWeightsArray = new Vector4[submesh.VertexCount];
submesh.VertexBuffer.GetData(
submesh.StartVertex * vertexDeclaration.VertexStride + boneWeightElement.Offset,
boneWeightsArray,
0,
submesh.VertexCount,
vertexDeclaration.VertexStride);
// Sort the vertices per bone.
for (int i = 0; i < submesh.VertexCount; i++)
{
var vertex = (Vector3F)positions[i];
// Here, we only check the first bone index. We could also check the
// bone weights to add the vertex to all bone vertex lists where the
// weight is high...
Vector4 boneIndices = boneIndicesArray[i].ToVector4();
//Vector4 boneWeights = boneWeightsArray[i];
int boneIndex = (int)boneIndices.X;
if (verticesPerBone[boneIndex] == null)
{
verticesPerBone[boneIndex] = new List <Vector3F>();
}
verticesPerBone[boneIndex].Add(vertex);
// Add vertex to AABB.
if (aabb == null)
{
aabb = new Aabb(vertex, vertex);
}
else
{
aabb.Value.Grow(vertex);
}
}
}
// We create a body for each bone with vertices.
int numberOfBodies = verticesPerBone.Count(vertices => vertices != null);
// 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.
const float totalMass = 80; // The total mass of the ragdoll.
var massFrame = MassFrame.FromShapeAndMass(new SphereShape(aabb.Value.Extent.Y / 8), Vector3F.One, totalMass / numberOfBodies, 0.1f, 1);
var material = new UniformMaterial();
Ragdoll ragdoll = new Ragdoll();
for (int boneIndex = 0; boneIndex < skeleton.NumberOfBones; boneIndex++)
{
var boneVertices = verticesPerBone[boneIndex];
if (boneVertices != null)
{
var bindPoseInverse = (Pose)skeleton.GetBindPoseAbsoluteInverse(boneIndex);
// Compute bounding capsule.
//float radius;
//float height;
//Pose pose;
//GeometryHelper.ComputeBoundingCapsule(boneVertices, out radius, out height, out pose);
//Shape shape = new TransformedShape(new GeometricObject(new CapsuleShape(radius, height), pose));
// Compute convex hull.
var points = GeometryHelper.CreateConvexHull(boneVertices, 32, 0).ToTriangleMesh().Vertices;
Shape shape = new ConvexHullOfPoints(points.Count > 0 ? points : boneVertices);
ragdoll.Bodies.Add(new RigidBody(shape, massFrame, material));
ragdoll.BodyOffsets.Add(bindPoseInverse);
}
else
{
ragdoll.Bodies.Add(null);
ragdoll.BodyOffsets.Add(Pose.Identity);
}
}
return(ragdoll);
}
public MassSample(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);
// Add a static body that serves as the base of the see-saw.
RigidBody body = new RigidBody(new BoxShape(0.1f, 1, 2))
{
MotionType = MotionType.Static,
Pose = new Pose(new Vector3F(0, 0.5f, 0))
};
Simulation.RigidBodies.Add(body);
// Create a plank.
body = new RigidBody(new BoxShape(5, 0.1f, 1.3f))
{
Pose = new Pose(new Vector3F(0, 1.05f, 0))
};
Simulation.RigidBodies.Add(body);
// ----- Create a few light bodies on the left.
Shape boxShape = new BoxShape(0.7f, 0.7f, 0.7f);
// The light bodies have a density of 200.
// (The first three parameters of FromShapeAndDensity are: shape, scale, density.
// The last two parameters are required for shapes where the mass properties can only
// be approximated using an iterative procedure: 0.01 --> The shape is approximated
// up to approx. 1%. The procedure aborts after 3 iterations.
// Since the shape is a box FromShapeAndDensity computes the exact mass and the last two
// parameters are irrelevant in this case.)
MassFrame mass = MassFrame.FromShapeAndDensity(boxShape, Vector3F.One, 200, 0.01f, 3);
body = new RigidBody(boxShape, mass, null)
{
Pose = new Pose(new Vector3F(-1.5f, 2f, 0))
};
Simulation.RigidBodies.Add(body);
body = new RigidBody(boxShape, mass, null)
{
Pose = new Pose(new Vector3F(-1.5f, 2.7f, 0))
};
Simulation.RigidBodies.Add(body);
body = new RigidBody(boxShape, mass, null)
{
Pose = new Pose(new Vector3F(-1.5f, 3.4f, 0))
};
Simulation.RigidBodies.Add(body);
// ----- Create a heavy body on the right.
// The heavy body has a density of 2000.
mass = MassFrame.FromShapeAndDensity(boxShape, Vector3F.One, 2000, 0.01f, 3);
body = new RigidBody(boxShape, mass, null)
{
Pose = new Pose(new Vector3F(1.5f, 3, 0))
};
Simulation.RigidBodies.Add(body);
}
/// <summary>
/// Initializes a new instance of the <see cref="KinematicCharacterController"/> class.
/// </summary>
/// <param name="simulation">The simulation.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="simulation" /> is <see langword="null"/>.
/// </exception>
public DynamicCharacterController(Simulation simulation)
{
if (simulation == null)
{
throw new ArgumentNullException("simulation");
}
Simulation = simulation;
CapsuleShape shape = new CapsuleShape(0.4f, 1.8f);
MassFrame mass = new MassFrame {
Mass = 80
}; // Push strength is proportional to the mass!
UniformMaterial material = new UniformMaterial
{
// The body should be frictionless, so that it can be easily pushed by the simulation to
// valid positions. And it does not slow down when sliding along walls.
StaticFriction = 0.0f,
DynamicFriction = 0.0f,
// The body should not bounce when being hit or pushed.
Restitution = 0
};
Body = new RigidBody(shape, mass, material)
{
// We set the mass explicitly and it should not automatically change when the
// shape is changed; e.g. a ducked character has a smaller shape, but still the same mass.
AutoUpdateMass = false,
// This body is under our control and should never be deactivated by the simulation.
CanSleep = false,
CcdEnabled = true,
// The capsule does not rotate in any direction.
LockRotationX = true,
LockRotationY = true,
LockRotationZ = true,
Name = "CharacterController",
Pose = new Pose(new Vector3(0, shape.Height / 2, 0)),
};
// Create a ray that senses the space below the capsule. The ray starts in the capsule
// center (to detect penetrations) and extends 0.4 units below the capsule bottom.
RayShape rayShape = new RayShape(Vector3.Zero, -Vector3.UnitY, shape.Height / 2 + 0.4f)
{
StopsAtFirstHit = true,
};
GeometricObject rayGeometry = new GeometricObject(rayShape, Body.Pose);
_ray = new CollisionObject(rayGeometry);
// Whenever the Body moves, the ray moves with it.
Body.PoseChanged += (s, e) => rayGeometry.Pose = Body.Pose;
// Enable the character controller. (Adds body to simulation.)
Enabled = true;
}
//--------------------------------------------------------------
private void InitializeBody(Vector3F upVector)
{
if (!upVector.TryNormalize())
throw new ArgumentException("The up vector must not be a zero vector.");
UpVector = upVector;
CapsuleShape shape = new CapsuleShape(0.4f, 1.8f);
MassFrame mass = new MassFrame { Mass = 100 };
UniformMaterial material = new UniformMaterial
{
// The body should be frictionless, so that it can be easily pushed by the simulation to
// valid positions.
StaticFriction = 0.0f,
DynamicFriction = 0.0f,
// The body should not bounce when being hit or pushed.
Restitution = 0
};
Body = new RigidBody(shape, mass, material)
{
// We set the mass explicitly and it should not automatically change when the
// shape is changed; e.g. a ducked character has a smaller shape, but still the same mass.
AutoUpdateMass = false,
// This body is under our control and should never be deactivated by the simulation.
CanSleep = false,
CcdEnabled = true,
// The capsule does not rotate in any direction.
LockRotationX = true,
LockRotationY = true,
LockRotationZ = true,
Name = "CharacterController",
Pose = new Pose(shape.Height / 2 * upVector,
QuaternionF.CreateRotation(Vector3F.UnitY, upVector)),
};
// When the user changes the shape, we must re-compute all contacts.
Body.ShapeChanged += (s, e) => UpdateContacts();
}
private float _slopeLimit = ConstantsF.PiOver4; // = 45°
#endregion Fields
#region Constructors
//--------------------------------------------------------------
/// <summary>
/// Initializes a new instance of the <see cref="KinematicCharacterController"/> class.
/// </summary>
/// <param name="simulation">The simulation.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="simulation" /> is <see langword="null"/>.
/// </exception>
public DynamicCharacterController(Simulation simulation)
{
if (simulation == null)
throw new ArgumentNullException("simulation");
Simulation = simulation;
CapsuleShape shape = new CapsuleShape(0.4f, 1.8f);
MassFrame mass = new MassFrame { Mass = 80 }; // Push strength is proportional to the mass!
UniformMaterial material = new UniformMaterial
{
// The body should be frictionless, so that it can be easily pushed by the simulation to
// valid positions. And it does not slow down when sliding along walls.
StaticFriction = 0.0f,
DynamicFriction = 0.0f,
// The body should not bounce when being hit or pushed.
Restitution = 0
};
Body = new RigidBody(shape, mass, material)
{
// We set the mass explicitly and it should not automatically change when the
// shape is changed; e.g. a ducked character has a smaller shape, but still the same mass.
AutoUpdateMass = false,
// This body is under our control and should never be deactivated by the simulation.
CanSleep = false,
CcdEnabled = true,
// The capsule does not rotate in any direction.
LockRotationX = true,
LockRotationY = true,
LockRotationZ = true,
Name = "CharacterController",
Pose = new Pose(new Vector3F(0, shape.Height / 2, 0)),
};
// Create a ray that senses the space below the capsule. The ray starts in the capsule
// center (to detect penetrations) and extends 0.4 units below the capsule bottom.
RayShape rayShape = new RayShape(Vector3F.Zero, -Vector3F.UnitY, shape.Height / 2 + 0.4f)
{
StopsAtFirstHit = true,
};
GeometricObject rayGeometry = new GeometricObject(rayShape, Body.Pose);
_ray = new CollisionObject(rayGeometry);
// Whenever the Body moves, the ray moves with it.
Body.PoseChanged += (s, e) => rayGeometry.Pose = Body.Pose;
// Enable the character controller. (Adds body to simulation.)
Enabled = true;
}
