private void CreateGround(UniformMaterial material)
{
var ground = new RigidBody
{
MotionType = DigitalRune.Physics.MotionType.Static,
Shape = new PlaneShape(new Vector3F(0, 1, 0), 0),
Material = material,
UserData = _planeModel
};
_simulation.RigidBodies.Add(ground);
}
public SimpleFixture(GeometricObject geometricObject, List <UniformMaterial> materialCollection, UniformMaterial material, FixtureDescriptor descriptor, Matrix4x4 realParentPose)
{
_wrappedGeometricObject = geometricObject;
_materialCollection = materialCollection;
_material = material;
UserData = descriptor.UserData;
ShapeFactory = new SimpleFixtureShapeFactory(this);
MaterialFactory = new SimpleFixtureMaterialFactory(this);
_pose = descriptor.Pose;
_root = false;
_realParentPose = realParentPose;
}
public CompositeMaterial2Sample(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(new Vector3(0, 1, 0.25f).Normalized, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
// Adjust the coefficients of friction of the ground plane.
((UniformMaterial)groundPlane.Material).DynamicFriction = 0.5f;
((UniformMaterial)groundPlane.Material).StaticFriction = 0.5f;
Simulation.RigidBodies.Add(groundPlane);
// Prepare two materials: a slippery material and a rough material.
UniformMaterial slipperyMaterial = new UniformMaterial
{
DynamicFriction = 0.001f,
StaticFriction = 0.001f,
};
UniformMaterial roughMaterial = new UniformMaterial
{
DynamicFriction = 1,
StaticFriction = 1,
};
// Create a rigid body that consists of multiple shapes: Two boxes and a cylinder between them.
CompositeShape compositeShape = new CompositeShape();
compositeShape.Children.Add(new GeometricObject(new BoxShape(1f, 1f, 1f), new Pose(new Vector3(1.5f, 0f, 0f))));
compositeShape.Children.Add(new GeometricObject(new BoxShape(1f, 1, 1f), new Pose(new Vector3(-1.5f, 0f, 0f))));
compositeShape.Children.Add(new GeometricObject(new CylinderShape(0.1f, 2), new Pose(Matrix.CreateRotationZ(ConstantsF.PiOver2))));
// A CompositeMaterial is used to assign a different material to each shape.
CompositeMaterial compositeMaterial = new CompositeMaterial();
compositeMaterial.Materials.Add(roughMaterial); // Assign the rough material to the first box.
compositeMaterial.Materials.Add(slipperyMaterial); // Assign the slippery material to the second box.
compositeMaterial.Materials.Add(null); // Use whatever is default for the handle between the boxes.
RigidBody body = new RigidBody(compositeShape, null, compositeMaterial)
{
Pose = new Pose(new Vector3(0, 2.2f, -5)),
};
Simulation.RigidBodies.Add(body);
}
ISimpleFixture IFactoryOf <ISimpleFixture, FixtureDescriptor> .Create(FixtureDescriptor descriptor)
{
var wrappedCompositeShape = _compositeFixture._wrappedCompositeShape;
var wrappedCompositeMaterial = _compositeFixture._wrappedCompositeMaterial;
var childPose = GMath.mul(descriptor.Pose, _compositeFixture._realPose);
var geometricObject = new GeometricObject(new EmptyShape(), childPose.ToDigitalRune());
var uniformMaterial = new UniformMaterial();
wrappedCompositeShape.Children.Add(geometricObject);
wrappedCompositeMaterial.Materials.Add(uniformMaterial);
return(new SimpleFixture(geometricObject, wrappedCompositeMaterial.Materials, uniformMaterial, descriptor, _compositeFixture._realPose));
}
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();
}
private void DigitalRuneAdaptorScene()
{
//creating the simulation
_simulation = new Simulation();
_simulation.ForceEffects.Add(new Gravity());
_simulation.ForceEffects.Add(new Damping());
//definig a material
var material = new UniformMaterial {
StaticFriction = 0.5f, DynamicFriction = 0.5f, Restitution = 0.7f
};
//creating the ground
CreateGround(material);
////creating a Tower
CreateTower(material, new BoxShapeDescriptor(1, 1, 1),
xCount: 3, yCount: 3, zCount: 3,
xSpace: 2, ySpace: 2, zSpace: 2,
xOffset: 0, yOffset: 2 + 10 / 2, zOffset: 0);
}
private void CreateTower(UniformMaterial material, BoxShapeDescriptor descriptor, int xCount, int yCount, int zCount, float xSpace, float ySpace, float zSpace, float xOffset, float yOffset, float zOffset)
{
for (int x = 0; x < xCount; x++)
{
for (int y = 0; y < yCount; y++)
{
for (int z = 0; z < zCount; z++)
{
var box = new RigidBody
{
MotionType = DigitalRune.Physics.MotionType.Dynamic,
Pose = new Pose(new Vector3F(xOffset + x * xSpace - ((xCount - 1) * xSpace / 2),
yOffset + y * ySpace - ((yCount - 1) * ySpace / 2),
zOffset + z * zSpace - ((zCount - 1) * zSpace / 2))),
Shape = new BoxShape(descriptor.WidthX, descriptor.WidthY, descriptor.WidthZ),
Material = material,
UserData = _boxModel
};
_simulation.RigidBodies.Add(box);
}
}
}
}
public SurfaceMotionSample(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);
// Create a material with surface motion.
UniformMaterial material = new UniformMaterial("ConveyorBelt", true) // Important: The second parameter enables the surface
{ // motion. It has to be set to true in the constructor!
SurfaceMotion = new Vector3F(-1, 0, 0), // The surface motion relative to the object.
};
// Create conveyor belt.
RigidBody conveyorBelt = new RigidBody(new BoxShape(8, 0.51f, 1.1f), null, material)
{
Pose = new Pose(new Vector3F(0, 0.25f, 0)),
// If the conveyor belt is dynamic, it would "drive away" ;-).
// Therefore, we make it static or kinematic so that it stays in place.
MotionType = MotionType.Kinematic,
};
Simulation.RigidBodies.Add(conveyorBelt);
// Two static boxes on the sides.
RigidBody body0 = new RigidBody(new BoxShape(8, 0.5f, 0.8f))
{
Pose = new Pose(new Vector3F(0, 0.25f, -0.6f - 0.4f))
};
Simulation.RigidBodies.Add(body0);
RigidBody body1 = new RigidBody(new BoxShape(8, 0.5f, 0.8f))
{
Pose = new Pose(new Vector3F(0, 0.25f, 0.6f + 0.4f))
};
Simulation.RigidBodies.Add(body1);
// Add a few random boxes at the top of the conveyor.
BoxShape boxShape = new BoxShape(0.6f, 0.6f, 0.6f);
for (int i = 0; i < 20; i++)
{
Vector3F randomPosition = new Vector3F(
RandomHelper.Random.NextFloat(-4, 4),
RandomHelper.Random.NextFloat(1, 3),
RandomHelper.Random.NextFloat(-1, 1));
QuaternionF randomOrientation = RandomHelper.Random.NextQuaternionF();
RigidBody body = new RigidBody(boxShape)
{
Pose = new Pose(randomPosition, randomOrientation),
};
Simulation.RigidBodies.Add(body);
}
}
public CompositeMaterialSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// ----- Create a simple height field.
const int resolution = 20;
HeightField heightField = new HeightField();
// Set the size of the height field in world space. (WidthX/Z determine the extent
// of the height field but not the resolution.)
heightField.WidthX = 40;
heightField.WidthZ = 40;
// Create the height field data.
// The size of the array determines the resolution of the height field.
heightField.Array = new float[resolution, resolution];
for (int x = 0; x < heightField.Array.GetLength(0); x++)
{
for (int z = 0; z < heightField.Array.GetLength(1); z++)
{
// Set the y value (height) at the given index.
heightField.Array[x, z] = 20 - z;
}
}
RigidBody ground = new RigidBody(heightField)
{
Pose = new Pose(new Vector3F(-20, -10, -20f)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(ground);
// Assign two different materials to the height field.
// A rough material (high friction) should be used for the left cells of the height field.
UniformMaterial roughMaterial = new UniformMaterial
{
DynamicFriction = 1,
StaticFriction = 1,
};
// A slippery material (low friction) should be used for the right cells of the height field.
UniformMaterial slipperyMaterial = new UniformMaterial
{
DynamicFriction = 0,
StaticFriction = 0,
};
// Use a CompositeMaterial two assign the materials to the features of the height field.
CompositeMaterial compositeMaterial = new CompositeMaterial();
// A "feature" of a height field is a triangle:
// The height field is triangulated. Each cell consists of two triangles. The triangles are
// numbered from left-to-right and top-to-bottom.
// (For more information: See the description of HeightField.)
// Loop over the cells.
// (If the resolution is 20, we have 20 height values in one row. Between these height
// values are 19 cells.)
for (int z = 0; z < resolution - 1; z++)
{
for (int x = 0; x < resolution - 1; x++)
{
// Assign the rough material to the left cells and the slippery material to the
// right cells.
if (x < resolution / 2)
{
// Each cell contains 2 triangles, therefore we have to add 2 entries to the
// CompositeMaterial.
compositeMaterial.Materials.Add(roughMaterial);
compositeMaterial.Materials.Add(roughMaterial);
}
else
{
compositeMaterial.Materials.Add(slipperyMaterial);
compositeMaterial.Materials.Add(slipperyMaterial);
}
}
}
ground.Material = compositeMaterial;
// Create a few boxes on the height field.
// The left boxes will roll or stop on the height field because of the high friction.
// The right boxes will slide down because of the low friction.
BoxShape boxShape = new BoxShape(1, 1, 1);
for (int i = 0; i < 10; i++)
{
RigidBody body = new RigidBody(boxShape, null, roughMaterial)
{
Pose = new Pose(new Vector3F(-19 + i * 4, 10, -10)),
};
Simulation.RigidBodies.Add(body);
}
}
/// <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
}
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
protected override void OnLoad()
{
// Add rigid bodies to simulation.
var simulation = _services.GetInstance <Simulation>();
// We use a random number generator with a custom seed.
RandomHelper.Random = new Random(123);
// ----- Add a ground plane.
AddBody(simulation, "GroundPlane", Pose.Identity, new PlaneShape(Vector3F.UnitY, 0), MotionType.Static);
// ----- Create a small flying sphere.
AddBody(simulation, "Sphere", new Pose(new Vector3F(0, 1f, 0)), new SphereShape(0.2f), MotionType.Static);
// ----- Create small walls at the level boundary.
AddBody(simulation, "WallLeft", new Pose(new Vector3F(-30, 1, 0)), new BoxShape(0.3f, 2, 60), MotionType.Static);
AddBody(simulation, "WallRight", new Pose(new Vector3F(30, 1, 0)), new BoxShape(0.3f, 2, 60), MotionType.Static);
AddBody(simulation, "WallFront", new Pose(new Vector3F(0, 1, -30)), new BoxShape(60, 2, 0.3f), MotionType.Static);
AddBody(simulation, "WallBack", new Pose(new Vector3F(0, 1, 30)), new BoxShape(60, 2, 0.3f), MotionType.Static);
// ----- Create a few bigger objects.
// We position the boxes so that we have a few corners we can run into. Character controllers
// should be stable when the user runs into corners.
AddBody(simulation, "House0", new Pose(new Vector3F(10, 1, -10)), new BoxShape(8, 2, 8f), MotionType.Static);
AddBody(simulation, "House1", new Pose(new Vector3F(13, 1, -4)), new BoxShape(2, 2, 4), MotionType.Static);
AddBody(simulation, "House2", new Pose(new Vector3F(10, 2, -15), Matrix33F.CreateRotationY(-0.3f)), new BoxShape(8, 4, 2), MotionType.Static);
// ----- Create stairs with increasing step height.
// Each step is a box. With this object we can test if our character can climb up
// stairs. The character controller has a step height limit. Increasing step heights
// let us test if the step height limit works.
float startHeight = 0;
const float stepDepth = 1f;
for (int i = 0; i < 10; i++)
{
float stepHeight = 0.1f + i * 0.05f;
Vector3F position = new Vector3F(0, startHeight + stepHeight / 2, -2 - i * stepDepth);
AddBody(simulation, "Step" + i, new Pose(position), new BoxShape(2, stepHeight, stepDepth), MotionType.Static);
startHeight += stepHeight;
}
// ----- V obstacle to test if we get stuck.
AddBody(simulation, "V0", new Pose(new Vector3F(-5.5f, 0, 10), QuaternionF.CreateRotationZ(0.2f)), new BoxShape(1f, 2f, 2), MotionType.Static);
AddBody(simulation, "V1", new Pose(new Vector3F(-4, 0, 10), QuaternionF.CreateRotationZ(-0.2f)), new BoxShape(1f, 2f, 2), MotionType.Static);
// ----- Create a height field.
// Terrain that is uneven is best modeled with a height field. Height fields are faster
// than general triangle meshes.
// The height direction is the y direction.
// The height field lies in the x/z plane.
var numberOfSamplesX = 20;
var numberOfSamplesZ = 20;
var samples = new float[numberOfSamplesX * numberOfSamplesZ];
// Create arbitrary height values.
for (int z = 0; z < numberOfSamplesZ; z++)
{
for (int x = 0; x < numberOfSamplesX; x++)
{
if (x == 0 || z == 0 || x == 19 || z == 19)
{
// Set this boundary elements to a low height, so that the height field is connected
// to the ground.
samples[z * numberOfSamplesX + x] = -1;
}
else
{
// A sine/cosine function that creates some interesting waves.
samples[z * numberOfSamplesX + x] = 1.0f + (float)(Math.Cos(z / 2f) * Math.Sin(x / 2f) * 1.0f);
}
}
}
var heightField = new HeightField(0, 0, 20, 20, samples, numberOfSamplesX, numberOfSamplesZ);
AddBody(simulation, "HeightField", new Pose(new Vector3F(10, 0, 10)), heightField, MotionType.Static);
// ----- Create rubble on the floor (small random objects on the floor).
// Our character should be able to move over small bumps on the ground.
for (int i = 0; i < 50; i++)
{
Vector3F position = new Vector3F(RandomHelper.Random.NextFloat(-5, 5), 0, RandomHelper.Random.NextFloat(10, 20));
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
Vector3F size = RandomHelper.Random.NextVector3F(0.05f, 0.8f);
AddBody(simulation, "Stone" + i, new Pose(position, orientation), new BoxShape(size), MotionType.Static);
}
// ----- Create some slopes to see how our character performs on/under sloped surfaces.
// Here we can test how the character controller behaves if the head touches an inclined
// ceiling.
AddBody(simulation, "SlopeGround", new Pose(new Vector3F(-2, 1.8f, -12), QuaternionF.CreateRotationX(0.4f)), new BoxShape(2, 0.5f, 10), MotionType.Static);
AddBody(simulation, "SlopeRoof", new Pose(new Vector3F(-2, 5.6f, -12), QuaternionF.CreateRotationX(-0.4f)), new BoxShape(2, 0.5f, 10), MotionType.Static);
// Create slopes with increasing tilt angles.
// The character controller has a slope limit. Only flat slopes should be climbable.
// Movement between slopes should be smooth.
Vector3F slopePosition = new Vector3F(-17, -0.25f, 6);
BoxShape slopeShape = new BoxShape(8, 0.5f, 5);
for (int i = 1; i < 8; i++)
{
Matrix33F oldRotation = Matrix33F.CreateRotationX((i - 1) * MathHelper.ToRadians(10));
Matrix33F rotation = Matrix33F.CreateRotationX(i * MathHelper.ToRadians(10));
slopePosition += (oldRotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2
+ (rotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2;
AddBody(simulation, "Slope" + i, new Pose(slopePosition, rotation), slopeShape, MotionType.Static);
}
// Create slopes with decreasing tilt angles.
slopePosition = new Vector3F(-8, -2, 5);
slopeShape = new BoxShape(8f, 0.5f, 5);
for (int i = 1; i < 8; i++)
{
Matrix33F oldRotation = Matrix33F.CreateRotationX(MathHelper.ToRadians(40) - (i - 1) * MathHelper.ToRadians(10));
Matrix33F rotation = Matrix33F.CreateRotationX(MathHelper.ToRadians(40) - i * MathHelper.ToRadians(10));
slopePosition += (oldRotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2
+ (rotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2;
Vector3F position = slopePosition - rotation * new Vector3F(0, slopeShape.WidthY / 2, 0);
AddBody(simulation, "Slope2" + i, new Pose(position, rotation), slopeShape, MotionType.Static);
}
// ----- Create a slope with a wall on one side.
// This objects let's us test how the character controller behaves while falling and
// sliding along a vertical wall. (Run up the slope and then jump down while moving into
// the wall.)
AddBody(simulation, "LongSlope", new Pose(new Vector3F(-24, 3, -10), Matrix33F.CreateRotationX(0.4f)), new BoxShape(4, 5f, 30), MotionType.Static);
AddBody(simulation, "LongSlopeWall", new Pose(new Vector3F(-26, 5, -10)), new BoxShape(0.5f, 10f, 25), MotionType.Static);
// ----- Create a trigger object that represents a ladder.
var ladder = AddBody(simulation, "Ladder", new Pose(new Vector3F(-25.7f, 5, 0)), new BoxShape(0.5f, 10f, 1), MotionType.Static);
ladder.CollisionObject.Type = CollisionObjectType.Trigger;
// ----- Create a mesh object to test walking on triangle meshes.
// Normally, the mesh would be loaded from a file. Here, we make a composite shape and
// let DigitalRune Geometry compute a mesh for it. Then we throw away the composite
// shape and use only the mesh. (We do this to test triangle meshes. Using the composite
// shape instead of the triangle mesh would be a lot faster.)
CompositeShape compositeShape = new CompositeShape();
compositeShape.Children.Add(new GeometricObject(heightField, Pose.Identity));
compositeShape.Children.Add(new GeometricObject(new CylinderShape(1, 2), new Pose(new Vector3F(10, 1, 10))));
compositeShape.Children.Add(new GeometricObject(new SphereShape(3), new Pose(new Vector3F(15, 0, 15))));
compositeShape.Children.Add(new GeometricObject(new BoxShape(4, 4, 3), new Pose(new Vector3F(15, 1.5f, 5))));
compositeShape.Children.Add(new GeometricObject(new BoxShape(4, 4, 3), new Pose(new Vector3F(15, 1.5f, 0))));
ITriangleMesh mesh = compositeShape.GetMesh(0.01f, 3);
TriangleMeshShape meshShape = new TriangleMeshShape(mesh);
// Collision detection speed for triangle meshes can be improved by using a spatial
// partition. Here, we assign an AabbTree to the triangle mesh shape. The tree is
// built automatically when needed and it stores triangle indices (therefore the generic
// parameter of the AabbTree is int).
meshShape.Partition = new AabbTree <int>()
{
// The tree is automatically built using a mixed top-down/bottom-up approach. Bottom-up
// building is slower but produces better trees. If the tree building takes too long,
// we can lower the BottomUpBuildThreshold (default is 128).
BottomUpBuildThreshold = 0,
};
// Contact welding creates smoother contact normals - but it costs a bit of performance.
meshShape.EnableContactWelding = true;
AddBody(simulation, "Mesh", new Pose(new Vector3F(-30, 0, 10)), meshShape, MotionType.Static);
// ----- Create a seesaw.
var seesawBase = AddBody(simulation, "SeesawBase", new Pose(new Vector3F(5, 0.5f, 0)), new BoxShape(0.2f, 1, 1), MotionType.Static);
var seesaw = AddBody(simulation, "Seesaw", new Pose(new Vector3F(5, 1.05f, 0)), new BoxShape(5, 0.1f, 1), MotionType.Dynamic);
// Attach the seesaw to the base using a hinge joint.
simulation.Constraints.Add(new HingeJoint
{
BodyA = seesaw,
BodyB = seesawBase,
AnchorPoseALocal = new Pose(new Vector3F(0, 0, 0),
new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0)),
AnchorPoseBLocal = new Pose(new Vector3F(0, 0.5f, 0),
new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0)),
CollisionEnabled = false,
});
// ----- A platform that is moving up/down.
_elevator = AddBody(simulation, "Elevator", new Pose(new Vector3F(5, -1f, 5)), new BoxShape(3, 1f, 3), MotionType.Kinematic);
_elevator.LinearVelocity = new Vector3F(2, 2, 0);
// ----- A platform that is moving sideways.
_pusher = AddBody(simulation, "Pusher", new Pose(new Vector3F(15, 0.5f, 0)), new BoxShape(3, 1f, 3), MotionType.Kinematic);
_pusher.LinearVelocity = new Vector3F(0, 0, 2);
// ----- Create conveyor belt with two static boxes on the sides.
AddBody(simulation, "ConveyorSide0", new Pose(new Vector3F(19, 0.25f, 0)), new BoxShape(0.8f, 0.5f, 8f), MotionType.Static);
AddBody(simulation, "ConveyorSide1", new Pose(new Vector3F(21, 0.25f, 0)), new BoxShape(0.8f, 0.5f, 8f), MotionType.Static);
// The conveyor belt is a simple box with a special material.
var conveyorBelt = AddBody(simulation, "ConveyorBelt", new Pose(new Vector3F(20, 0.25f, 0)), new BoxShape(1f, 0.51f, 8f), MotionType.Static);
UniformMaterial materialWithSurfaceMotion = new UniformMaterial("ConveyorBelt", true) // Important: The second parameter enables the surface
{ // motion. It has to be set to true in the constructor!
SurfaceMotion = new Vector3F(0, 0, 1), // The surface motion relative to the object.
};
conveyorBelt.Material = materialWithSurfaceMotion;
// ----- Distribute a few dynamic spheres and boxes across the landscape.
SphereShape sphereShape = new SphereShape(0.5f);
for (int i = 0; i < 10; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-15, 15);
position.Y = 20;
AddBody(simulation, "Sphere" + i, new Pose(position), sphereShape, MotionType.Dynamic);
}
BoxShape boxShape = new BoxShape(1, 1, 1);
for (int i = 0; i < 10; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-15, 15);
position.Y = 20;
AddBody(simulation, "Box" + i, new Pose(position), boxShape, MotionType.Dynamic);
}
}
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);
}
/// <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 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 MaterialSample(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,
};
// Adjust the coefficient of restitution of the ground plane.
// (By default, the material of a rigid body is of type UniformMaterial.)
((UniformMaterial)groundPlane.Material).Restitution = 1; // Max. bounciness. (The simulation actually
// accepts higher values, but these usually
// lead to unnatural bounciness.)
Simulation.RigidBodies.Add(groundPlane);
// Add a static inclined ground plane.
RigidBody inclinedPlane = new RigidBody(new PlaneShape(new Vector3F(-0.3f, 1f, 0).Normalized, 0))
{
Name = "InclinedPlane",
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(inclinedPlane);
// Create a few boxes with different coefficient of friction.
BoxShape boxShape = new BoxShape(1, 1, 1);
for (int i = 0; i < 5; i++)
{
// Each box gets a different friction value.
UniformMaterial material = new UniformMaterial
{
DynamicFriction = i * 0.2f,
StaticFriction = i * 0.2f,
};
RigidBody box = new RigidBody(boxShape, null, material) // The second argument (the mass frame) is null. The
{ // simulation will automatically compute a default mass.
Name = "Box" + i,
Pose = new Pose(new Vector3F(5, 6, -5 + i * 2)),
};
Simulation.RigidBodies.Add(box);
}
// Create a few balls with different coefficient of restitution (= bounciness).
Shape sphereShape = new SphereShape(0.5f);
for (int i = 0; i < 6; i++)
{
// Vary restitution between 0 and 1.
UniformMaterial material = new UniformMaterial
{
Restitution = i * 0.2f
};
RigidBody body = new RigidBody(sphereShape, null, material)
{
Name = "Ball" + i,
Pose = new Pose(new Vector3F(-1 - i * 2, 5, 0)),
};
Simulation.RigidBodies.Add(body);
}
}
public PerformanceSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// ----- Multithreading
// If the scene contains many active (= moving objects), using multithreading is
// recommended. Multithreading does not improve very static scenes where most bodies
// are not moving or scenes with very little bodies.
// If multithreading is enabled in the collision domain, the narrow phase algorithms
// (which determines contacts) are executed in parallel.
Simulation.CollisionDomain.EnableMultithreading = true;
// If multithreading is enabled in the simulation, the simulation islands are solved
// in parallel and a few other things run in parallel.
Simulation.Settings.EnableMultithreading = true;
// The processor affinity and the number of threads can be controlled with these properties:
//Parallel.ProcessorAffinity = new[] { 4, 3, 5 }; // Threads use the cores 3, 4, 5.
//Parallel.Scheduler = new WorkStealingScheduler(3); // Use 3 worker threads.
// The default settings should be ok for most scenarios.
// ----- Collision Detection Settings
// We disable the flag FullContactSetPerFrame. If the flag is disabled, the collision
// detection is faster because in the narrow phase some algorithms will compute only
// one new contact between two touching bodies.
// If the flag is enabled, the simulation is more stable because the narrow phase computes
// more contacts per pair of touching bodies.
Simulation.CollisionDomain.CollisionDetection.FullContactSetPerFrame = false;
// ----- Physics Settings
// If SynchronizeCollisionDomain is false, the collision detection is run only at the
// beginning of Simulation.Update(). If SynchronizeCollisionDomain is set, the collision
// detection is also performed at the end. This is necessary in case you need to make manual
// collision detection queries and need up-to-date collision detection info.
// Disable this flag if you do not need it.
Simulation.Settings.SynchronizeCollisionDomain = false;
// The MinConstraintImpulse defines when the constraint solver will stop its iterative
// process. A higher limit will make the solver stop earlier (=> faster, but less stable).
Simulation.Settings.Constraints.MinConstraintImpulse = 0.0001f;
// NumberOfConstraintIterations defines how many iterations the solver performs at max.
// Values from 4 to 20 are normal. Use higher values if stable stacking is required.
Simulation.Settings.Constraints.NumberOfConstraintIterations = 4;
// Randomization of constraints takes a tiny bit of time and helps to make stacks and
// complex scenes more stable. For simple scenes we can disable it.
Simulation.Settings.Constraints.RandomizeConstraints = false;
// Continuous collision detection cost a bit performance. We are faster if we disable it
// but with disabled CCD balls (right mouse button) will fly through objects because of
// their high speed.
Simulation.Settings.Motion.CcdEnabled = false;
// If RemoveBodiesOutsideWorld is set, the simulation automatically removes bodies that
// leave the simulation (defined with Simulation.World). Disable it if not needed.
Simulation.Settings.Motion.RemoveBodiesOutsideWorld = false;
// TimeThreshold defines how fast bodies are deactivated. Normal values are 1 or 2 seconds.
// We can set it to a low value, e.g. 0.5 s, for a very aggressive sleeping. The negative
// effects of this are that bodies that are slowly falling over, can freeze in a tilted
// position.
// You can also try to disable sleeping by setting TimeThreshold to float.MaxValue. But the
// simulation will run significantly slower. You can run the PhysicsSample and compare the
// simulation times with enabled and disabled sleeping.
Simulation.Settings.Sleeping.TimeThreshold = 0.5f;
// FixedTimeStep defines the size of a single simulation step. Per default, the smallest
// step is 1 / 60 s (60 fps). In some cases it is ok to use an even larger time step
// like 1 / 30. But with large time steps stacks and walls will not be stable.
Simulation.Settings.Timing.FixedTimeStep = 1.0f / 60.0f;
// If the simulation gets complex the game will need more time to compute each frame.
// If the game becomes very slow, Simulation.Update(elapsedTime) will be called with
// a large elapsedTime. If our frame rate drops to 30 fps, Simulation.Update(1/30) will
// internally make 2 sub-time steps (if FixedTimeStep = 1/60). This could make the problem
// worse and if we expect such a situation we should limit the number of sub steps to 1.
// Then, if the game is running slowly, the physics simulation will run in slow motion -
// but at least it will not freeze the game.
Simulation.Settings.Timing.MaxNumberOfSteps = 1;
// ----- Force Effects.
// Using a low gravity is common trick to make the simulation more stable:
Simulation.ForceEffects.Add(new Gravity {
Acceleration = new Vector3F(0, -5, 0)
});
// Using high damping coefficients helps to make your simulation faster and more stable
// because objects will come the rest much quicker. - But too high values can create a
// very unrealistic damped body movement.
Simulation.ForceEffects.Add(new Damping {
LinearDamping = 0.3f, AngularDamping = 0.3f
});
// ----- Rigid Body Prototypes
// Here we create 3 rigid bodies that will serve as templates for the new random bodies
// that are created in Update().
// We use the same material instance for all rigid bodies to avoid the creation of several
// material instances.
var material = new UniformMaterial();
_prototypes = new RigidBody[3];
_prototypes[0] = new RigidBody(new SphereShape(0.5f), null, material);
_prototypes[1] = new RigidBody(new CylinderShape(0.4f, 0.9f), null, material);
_prototypes[2] = new RigidBody(new BoxShape(0.9f, 0.9f, 0.9f), null, material);
// ----- Height Field
// Create a height field.
HeightField heightField = new HeightField
{
WidthX = 100,
WidthZ = 100,
Array = new float[30, 30]
};
for (int x = 0; x < heightField.Array.GetLength(0); x++)
{
for (int z = 0; z < heightField.Array.GetLength(1); z++)
{
// Set the y values (height).
heightField.Array[x, z] = (float)(Math.Cos(z / 2f) * Math.Sin(x / 2f) * 3f + 5f);
}
}
// We can set following flag to get a significant performance gain - but the collision
// detection will be less accurate. For smooth height fields this flag can be set.
heightField.UseFastCollisionApproximation = true;
// Create a static rigid body using the height field and add it to the simulation.
// The mass of static rigid bodies is not relevant, therefore we use a default
// mass frame instance as the second constructor parameter. If we do not specify
// the mass frame, the physics library will try to compute a suitable mass frame
// which can take some time for large meshes.
RigidBody landscape = new RigidBody(heightField, new MassFrame(), material)
{
Pose = new Pose(new Vector3F(-50, 0, -50f)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(landscape);
}
public CompositeMaterialSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// ----- Create a simple height field.
// The height data consists of height samples with a resolution of 20 entries per dimension.
// (Height samples are stored in a 1-dimensional array.)
var numberOfSamplesX = 20;
var numberOfSamplesZ = 20;
var samples = new float[numberOfSamplesX * numberOfSamplesZ];
for (int z = 0; z < numberOfSamplesZ; z++)
{
for (int x = 0; x < numberOfSamplesX; x++)
{
// Set the y values (height).
samples[z * numberOfSamplesX + x] = 20 - z;
}
}
// Set the size of the height field in world space. (WidthX/Z determine the extent
// of the height field but not the resolution of the height samples.)
float widthX = 40;
float widthZ = 40;
// The origin is at (-20, -20) to center the height field at the world space origin.
float originX = -20;
float originZ = -20;
// Create the height field shape.
HeightField heightField = new HeightField(
originX, originZ, widthX, widthZ, samples, numberOfSamplesX, numberOfSamplesZ);
RigidBody ground = new RigidBody(heightField)
{
Pose = new Pose(new Vector3F(0, -10, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(ground);
// Assign two different materials to the height field.
// A rough material (high friction) should be used for the left cells of the height field.
UniformMaterial roughMaterial = new UniformMaterial
{
DynamicFriction = 1,
StaticFriction = 1,
};
// A slippery material (low friction) should be used for the right cells of the height field.
UniformMaterial slipperyMaterial = new UniformMaterial
{
DynamicFriction = 0,
StaticFriction = 0,
};
// Use a CompositeMaterial two assign the materials to the features of the height field.
CompositeMaterial compositeMaterial = new CompositeMaterial();
// A "feature" of a height field is a triangle:
// The height field is triangulated. Each cell consists of two triangles. The triangles are
// numbered from left-to-right and top-to-bottom.
// (For more information: See the description of HeightField.)
// Loop over the cells.
// (If the resolution is 20, we have 20 height values in one row. Between these height
// values are 19 cells.)
for (int z = 0; z < numberOfSamplesZ - 1; z++)
{
for (int x = 0; x < numberOfSamplesX - 1; x++)
{
// Assign the rough material to the left cells and the slippery material to the
// right cells.
if (x < numberOfSamplesX / 2)
{
// Each cell contains 2 triangles, therefore we have to add 2 entries to the
// CompositeMaterial.
compositeMaterial.Materials.Add(roughMaterial);
compositeMaterial.Materials.Add(roughMaterial);
}
else
{
compositeMaterial.Materials.Add(slipperyMaterial);
compositeMaterial.Materials.Add(slipperyMaterial);
}
}
}
ground.Material = compositeMaterial;
// Create a few boxes on the height field.
// The left boxes will roll or stop on the height field because of the high friction.
// The right boxes will slide down because of the low friction.
BoxShape boxShape = new BoxShape(1, 1, 1);
for (int i = 0; i < 10; i++)
{
RigidBody body = new RigidBody(boxShape, null, roughMaterial)
{
Pose = new Pose(new Vector3F(-19 + i * 4, 10, -10)),
};
Simulation.RigidBodies.Add(body);
}
}
Material(MaterialDescriptor descriptor)
{
WrappedUniformMaterial = new UniformMaterial();
Configurator = new MaterialConfigurator(this);
Descriptor = descriptor;
}
