public override void Update(GameTime gameTime)
{
// If <Space> is pressed, we fire a shot.
if (InputService.IsPressed(Keys.Space, true))
{
// Get a random angle and a random distance from the target.
float angle = _angleDistribution.Next(_random);
float distance = _distanceDistribution.Next(_random);
// Create a vector v with the length of distance.
Vector3F v = new Vector3F(0, distance, 0);
// Rotate v.
QuaternionF rotation = QuaternionF.CreateRotationZ(MathHelper.ToRadians(angle));
v = rotation.Rotate(v);
// Draw a small cross for the hit.
var debugRenderer = GraphicsScreen.DebugRenderer2D;
debugRenderer.DrawLine(
_center + v + new Vector3F(-10, -10, 0),
_center + v + new Vector3F(10, 10, 0),
Color.Black,
true);
debugRenderer.DrawLine(
_center + v + new Vector3F(10, -10, 0),
_center + v + new Vector3F(-10, 10, 0),
Color.Black, true);
}
base.Update(gameTime);
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Estimate required segment angle for given accuracy.
// (Easy to derive from simple drawing of a circle segment with a triangle used to represent
// the segment.)
float alpha = (float)Math.Acos((_radius - absoluteDistanceThreshold) / _radius) * 2;
int numberOfSegments = (int)Math.Ceiling(ConstantsF.TwoPi / alpha);
alpha = ConstantsF.TwoPi / numberOfSegments;
Vector3F r0 = new Vector3F(_radius, 0, 0);
QuaternionF rotation = QuaternionF.CreateRotationZ(alpha);
TriangleMesh mesh = new TriangleMesh();
for (int i = 1; i <= numberOfSegments; i++)
{
Vector3F r1 = rotation.Rotate(r0);
mesh.Add(new Triangle
{
Vertex0 = Vector3F.Zero,
Vertex1 = r0,
Vertex2 = r1,
}, true);
r0 = r1;
}
return(mesh);
}
public override void Update(GameTime gameTime)
{
_debugRenderer.Clear();
if (_avatarPose == null)
{
// Must wait till renderer is ready. Before that we do not get skeleton info.
if (_avatarRenderer.State == AvatarRendererState.Ready)
{
// Create AvatarPose.
_avatarPose = new AvatarPose(_avatarRenderer);
// A 'bone transform' is the transformation of a bone relative to its bind pose.
// Bone transforms define the pose of a skeleton.
// Rotate arm of avatar.
SkeletonPose skeletonPose = _avatarPose.SkeletonPose;
int shoulderIndex = skeletonPose.Skeleton.GetIndex("ShoulderLeft");
skeletonPose.SetBoneTransform(shoulderIndex, new SrtTransform(QuaternionF.CreateRotationZ(-0.9f)));
// The class SkeletonHelper provides some useful extension methods.
// One is SetBoneRotationAbsolute() which sets the orientation of a bone relative
// to model space.
// Rotate elbow to make the lower arm point forward.
int elbowIndex = skeletonPose.Skeleton.GetIndex("ElbowLeft");
SkeletonHelper.SetBoneRotationAbsolute(skeletonPose, elbowIndex, QuaternionF.CreateRotationY(ConstantsF.PiOver2));
// Draw avatar skeleton for debugging.
_debugRenderer.DrawSkeleton(skeletonPose, _pose, Vector3F.One, 0.02f, Color.Orange, true);
}
}
}
public void CreateRotationZ()
{
float angle = 0.3f;
QuaternionF q = QuaternionF.CreateRotation(Vector3F.UnitZ, angle);
QuaternionF qz = QuaternionF.CreateRotationZ(angle);
Assert.AreEqual(q, qz);
}
public void MultiplyOperator()
{
Pose p1 = new Pose(new Vector3F(1, 2, 3), QuaternionF.CreateRotationY(0.3f));
Pose p2 = new Pose(new Vector3F(-4, 5, -6), QuaternionF.CreateRotationZ(-0.1f));
Assert.IsTrue(Vector4F.AreNumericallyEqual(
p1.ToMatrix44F() * p2.ToMatrix44F() * new Vector4F(1, 2, 3, 1),
p1 * p2 * new Vector4F(1, 2, 3, 1)));
}
public void Interpolate()
{
Pose p1 = new Pose(new Vector3F(1, 2, 3), QuaternionF.CreateRotationY(0.3f));
Pose p2 = new Pose(new Vector3F(-4, 5, -6), QuaternionF.CreateRotationZ(-0.1f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p1.Position, Pose.Interpolate(p1, p2, 0).Position));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p1.Orientation, Pose.Interpolate(p1, p2, 0).Orientation));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p2.Position, Pose.Interpolate(p1, p2, 1).Position));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p2.Orientation, Pose.Interpolate(p1, p2, 1).Orientation));
Assert.IsTrue(Vector3F.AreNumericallyEqual(InterpolationHelper.Lerp(p1.Position, p2.Position, 0.3f), Pose.Interpolate(p1, p2, 0.3f).Position));
Assert.IsTrue(
QuaternionF.AreNumericallyEqual(
InterpolationHelper.Lerp(QuaternionF.CreateRotation(p1.Orientation), QuaternionF.CreateRotation(p2.Orientation), 0.3f),
QuaternionF.CreateRotation(Pose.Interpolate(p1, p2, 0.3f).Orientation)));
}
/// <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);
}
}
/// <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>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Estimate required segment angle for given accuracy.
// (Easy to derive from simple drawing of a circle segment with a triangle used to represent
// the segment.)
float alpha = (float)Math.Acos((Radius - absoluteDistanceThreshold) / Radius) * 2;
int numberOfSegments = (int)Math.Ceiling(ConstantsF.PiOver2 / alpha) * 4;
// Apply the iteration limit - in case absoluteDistanceThreshold is 0.
// Lets say each iteration doubles the number of segments. This is an arbitrary interpretation
// of the "iteration limit".
numberOfSegments = Math.Min(numberOfSegments, 2 << iterationLimit);
alpha = ConstantsF.TwoPi / numberOfSegments;
TriangleMesh mesh = new TriangleMesh();
// The world space vertices are created by rotating "radius vectors" with this rotations.
QuaternionF rotationY = QuaternionF.CreateRotationY(alpha);
QuaternionF rotationZ = QuaternionF.CreateRotationZ(alpha);
// We use two nested loops: In each loop a "radius vector" is rotated further to get a
// new vertex.
Vector3F rLow = Vector3F.UnitX * Radius; // Radius vector for the lower vertex.
for (int i = 1; i <= numberOfSegments / 4; i++)
{
Vector3F rHigh = rotationZ.Rotate(rLow); // Radius vector for the higher vertex.
// In the inner loop we create lines and triangles between 4 vertices, which are created
// with the radius vectors rLow0, rLow1, rHigh0, rHigh1.
Vector3F rLow0 = rLow;
Vector3F rHigh0 = rHigh;
for (int j = 1; j <= numberOfSegments; j++)
{
Vector3F rLow1 = rotationY.Rotate(rLow0);
Vector3F rHigh1 = rotationY.Rotate(rHigh0);
// Two top hemisphere triangles
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh0,
}, false);
if (i < numberOfSegments / 4) // At the "northpole" only a triangle is needed. No quad.
{
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh0,
}, false);
}
// Two bottom hemisphere triangles
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow0,
Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh0,
Vertex2 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow1,
}, false);
if (i < numberOfSegments / 4) // At the "southpole" only a triangle is needed. No quad.
{
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow1,
Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh0,
Vertex2 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh1,
}, false);
}
// Two side triangles
if (i == 1)
{
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow0,
Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
}, false);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow1,
Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
}, false);
}
rLow0 = rLow1;
rHigh0 = rHigh1;
}
rLow = rHigh;
}
mesh.WeldVertices();
return(mesh);
}
public ConstraintSample3(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// ----- HingeJoint with AngularVelocityMotor
// A board is fixed on a pole like a wind wheel.
// An AngularVelocityMotor creates a rotation with constant angular velocity around the
// hinge axis.
RigidBody box0 = new RigidBody(new BoxShape(0.1f, 2f, 0.1f))
{
Pose = new Pose(new Vector3F(-2, 1, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box0);
RigidBody box1 = new RigidBody(new BoxShape(0.1f, 0.4f, 1f))
{
Pose = new Pose(new Vector3F(-2 + 0.05f, 1.8f, 0))
};
Simulation.RigidBodies.Add(box1);
HingeJoint hingeJoint = new HingeJoint
{
BodyA = box0,
BodyB = box1,
AnchorPoseALocal = new Pose(new Vector3F(0.05f, 0.8f, 0)),
AnchorPoseBLocal = new Pose(new Vector3F(-0.05f, 0, 0)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(hingeJoint);
AngularVelocityMotor angularVelocityMotor = new AngularVelocityMotor
{
BodyA = box0,
// The rotation axis is the local x axis of BodyA.
AxisALocal = Vector3F.UnitX,
BodyB = box1,
TargetVelocity = 10,
// The motor power is limit, so that the rotation can be stopped by other objects blocking
// the movement.
MaxForce = 10000,
// The HingeJoint controls all other axes. So this motor must only act on the hinge
// axis.
UseSingleAxisMode = true,
CollisionEnabled = false,
};
Simulation.Constraints.Add(angularVelocityMotor);
// ----- A PrismaticJoint with a LinearVelocityMotor.
RigidBody box2 = new RigidBody(new BoxShape(0.7f, 0.7f, 0.7f))
{
Pose = new Pose(new Vector3F(0, 2, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box2);
RigidBody box3 = new RigidBody(new BoxShape(0.5f, 1.5f, 0.5f))
{
Pose = new Pose(new Vector3F(0, 1, 0))
};
Simulation.RigidBodies.Add(box3);
_prismaticJoint = new PrismaticJoint
{
BodyA = box2,
BodyB = box3,
AnchorPoseALocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
AnchorPoseBLocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(_prismaticJoint);
_linearVelocityMotor = new LinearVelocityMotor
{
BodyA = box2,
AxisALocal = -Vector3F.UnitY,
BodyB = box3,
TargetVelocity = 1,
CollisionEnabled = false,
MaxForce = 10000,
UseSingleAxisMode = true,
};
Simulation.Constraints.Add(_linearVelocityMotor);
// ----- A BallJoint with a TwistSwingLimit and a QuaternionMotor
// The ball joint connects a cylinder to a static box.
// The twist-swing limits rotational movement.
// The quaternion motor acts like a spring that controls the angle of joint.
RigidBody box4 = new RigidBody(new BoxShape(0.5f, 0.5f, 0.5f))
{
Pose = new Pose(new Vector3F(2, 2, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box4);
RigidBody cylinder0 = new RigidBody(new CylinderShape(0.1f, 0.75f))
{
Pose = new Pose(new Vector3F(2, 2 - 0.75f / 2 - 0.25f, 0))
};
Simulation.RigidBodies.Add(cylinder0);
_ballJoint = new BallJoint
{
BodyA = box4,
BodyB = cylinder0,
AnchorPositionALocal = new Vector3F(0, -0.25f, 0),
AnchorPositionBLocal = new Vector3F(0, 0.75f / 2, 0),
CollisionEnabled = false,
};
Simulation.Constraints.Add(_ballJoint);
_twistSwingLimit = new TwistSwingLimit
{
BodyA = box4,
// The first column is the twist axis (-y). The other two columns are the swing axes.
AnchorOrientationALocal = new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1),
BodyB = cylinder0,
AnchorOrientationBLocal = new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1),
CollisionEnabled = false,
// The twist is limited to +/- 10°. The swing limits are +/- 40° and +/- 60°. This creates
// a deformed cone that limits the swing movements (see visualization).
Minimum = new Vector3F(-MathHelper.ToRadians(10), -MathHelper.ToRadians(40), -MathHelper.ToRadians(60)),
Maximum = new Vector3F(MathHelper.ToRadians(10), MathHelper.ToRadians(40), MathHelper.ToRadians(60)),
};
Simulation.Constraints.Add(_twistSwingLimit);
QuaternionMotor quaternionMotor = new QuaternionMotor
{
BodyA = box4,
AnchorOrientationALocal = Matrix33F.Identity,
BodyB = cylinder0,
AnchorOrientationBLocal = Matrix33F.Identity,
CollisionEnabled = false,
// The QuaternionMotor controls the orientation of the second body relative to the first
// body. Here, we define that the cylinder should swing 30° away from the default
// orientation.
TargetOrientation = QuaternionF.CreateRotationZ(MathHelper.ToRadians(30)),
// Position and orientation motors are similar to damped-springs. We can control
// the stiffness and damping of the spring. (It is also possible to set the SpringConstant
// to 0 if the QuaternionMotor should only act as a rotational damping.)
SpringConstant = 100,
DampingConstant = 20,
};
Simulation.Constraints.Add(quaternionMotor);
}
// Creates a lot of random objects.
private void CreateRandomObjects()
{
var random = new Random();
var isFirstHeightField = true;
int currentShape = 0;
int numberOfObjects = 0;
while (true)
{
numberOfObjects++;
if (numberOfObjects > ObjectsPerType)
{
currentShape++;
numberOfObjects = 0;
}
Shape shape;
switch (currentShape)
{
case 0:
// Box
shape = new BoxShape(ObjectSize, ObjectSize * 2, ObjectSize * 3);
break;
case 1:
// Capsule
shape = new CapsuleShape(0.3f * ObjectSize, 2 * ObjectSize);
break;
case 2:
// Cone
shape = new ConeShape(1 * ObjectSize, 2 * ObjectSize);
break;
case 3:
// Cylinder
shape = new CylinderShape(0.4f * ObjectSize, 2 * ObjectSize);
break;
case 4:
// Sphere
shape = new SphereShape(ObjectSize);
break;
case 5:
// Convex hull of several points.
ConvexHullOfPoints hull = new ConvexHullOfPoints();
hull.Points.Add(new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize));
hull.Points.Add(new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize));
hull.Points.Add(new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize));
shape = hull;
break;
case 6:
// A composite shape: two boxes that form a "T" shape.
var composite = new CompositeShape();
composite.Children.Add(
new GeometricObject(
new BoxShape(ObjectSize, 3 * ObjectSize, ObjectSize),
new Pose(new Vector3F(0, 0, 0))));
composite.Children.Add(
new GeometricObject(
new BoxShape(2 * ObjectSize, ObjectSize, ObjectSize),
new Pose(new Vector3F(0, 2 * ObjectSize, 0))));
shape = composite;
break;
case 7:
shape = new CircleShape(ObjectSize);
break;
case 8:
{
var compBvh = new CompositeShape();
compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), Matrix33F.Identity)));
compBvh.Children.Add(new GeometricObject(new BoxShape(0.8f, 0.5f, 0.5f), new Pose(new Vector3F(0.5f, 0.7f, 0), Matrix33F.CreateRotationZ(-MathHelper.ToRadians(15)))));
compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), Matrix33F.Identity)));
compBvh.Children.Add(new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), Matrix33F.CreateRotationX(0.3f))));
compBvh.Partition = new AabbTree <int>();
shape = compBvh;
break;
}
case 9:
CompositeShape comp = new CompositeShape();
comp.Children.Add(new GeometricObject(new BoxShape(0.5f * ObjectSize, 1 * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3F(0, 0.5f * ObjectSize, 0), QuaternionF.Identity)));
comp.Children.Add(new GeometricObject(new BoxShape(0.8f * ObjectSize, 0.5f * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3F(0.3f * ObjectSize, 0.7f * ObjectSize, 0), QuaternionF.CreateRotationZ(-MathHelper.ToRadians(45)))));
comp.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3F(0, 1.15f * ObjectSize, 0), QuaternionF.Identity)));
shape = comp;
break;
case 10:
shape = new ConvexHullOfPoints(new[]
{
new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
});
break;
case 11:
ConvexHullOfShapes shapeHull = new ConvexHullOfShapes();
shapeHull.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3F(0, 2 * ObjectSize, 0), Matrix33F.Identity)));
shapeHull.Children.Add(new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize), Pose.Identity));
shape = shapeHull;
break;
case 12:
shape = Shape.Empty;
break;
case 13:
var numberOfSamplesX = 10;
var numberOfSamplesZ = 10;
var samples = new float[numberOfSamplesX * numberOfSamplesZ];
for (int z = 0; z < numberOfSamplesZ; z++)
{
for (int x = 0; x < numberOfSamplesX; x++)
{
samples[z * numberOfSamplesX + x] = (float)(Math.Cos(z / 3f) * Math.Sin(x / 2f) * BoxSize / 6);
}
}
HeightField heightField = new HeightField(0, 0, 2 * BoxSize, 2 * BoxSize, samples, numberOfSamplesX, numberOfSamplesZ);
shape = heightField;
break;
//case 14:
//shape = new LineShape(new Vector3F(0.1f, 0.2f, 0.3f), new Vector3F(0.1f, 0.2f, -0.3f).Normalized);
//break;
case 15:
shape = new LineSegmentShape(
new Vector3F(0.1f, 0.2f, 0.3f), new Vector3F(0.1f, 0.2f, 0.3f) + 3 * ObjectSize * new Vector3F(0.1f, 0.2f, -0.3f));
break;
case 16:
shape = new MinkowskiDifferenceShape
{
ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize))
};
break;
case 17:
shape = new MinkowskiSumShape
{
ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize)),
};
break;
case 18:
shape = new OrthographicViewVolume(0, ObjectSize, 0, ObjectSize, ObjectSize / 2, ObjectSize * 2);
break;
case 19:
shape = new PerspectiveViewVolume(MathHelper.ToRadians(60f), 16f / 10, ObjectSize / 2, ObjectSize * 3);
break;
case 20:
shape = new PointShape(0.1f, 0.3f, 0.2f);
break;
case 21:
shape = new RayShape(new Vector3F(0.2f, 0, -0.12f), new Vector3F(1, 2, 3).Normalized, ObjectSize * 2);
break;
case 22:
shape = new RayShape(new Vector3F(0.2f, 0, -0.12f), new Vector3F(1, 2, 3).Normalized, ObjectSize * 2)
{
StopsAtFirstHit = true
};
break;
case 23:
shape = new RectangleShape(ObjectSize, ObjectSize * 2);
break;
case 24:
shape = new TransformedShape(
new GeometricObject(
new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize),
new Pose(new Vector3F(0.1f, 1, -0.2f))));
break;
case 25:
shape = new TriangleShape(
new Vector3F(ObjectSize, 0, 0), new Vector3F(0, ObjectSize, 0), new Vector3F(ObjectSize, ObjectSize, ObjectSize));
break;
//case 26:
// {
// // Create a composite object from which we get the mesh.
// CompositeShape compBvh = new CompositeShape();
// compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), Matrix33F.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3F(0.5f, 0.7f, 0), Matrix33F.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), Matrix33F.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), Matrix33F.CreateRotationX(0.3f))));
// TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
// meshBvhShape.Partition = new AabbTree<int>();
// shape = meshBvhShape;
// break;
// }
//case 27:
// {
// // Create a composite object from which we get the mesh.
// CompositeShape compBvh = new CompositeShape();
// compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), QuaternionF.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3F(0.5f, 0.7f, 0), QuaternionF.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), QuaternionF.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), QuaternionF.CreateRotationX(0.3f))));
// TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
// meshBvhShape.Partition = new AabbTree<int>();
// shape = meshBvhShape;
// break;
// }
case 28:
shape = new ConvexPolyhedron(new[]
{
new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
});
break;
case 29:
return;
default:
currentShape++;
continue;
}
// Create an object with the random shape, pose, color and velocity.
Pose randomPose = new Pose(
random.NextVector3F(-BoxSize + ObjectSize * 2, BoxSize - ObjectSize * 2),
random.NextQuaternionF());
var newObject = new MovingGeometricObject
{
Pose = randomPose,
Shape = shape,
LinearVelocity = random.NextQuaternionF().Rotate(new Vector3F(MaxLinearVelocity, 0, 0)),
AngularVelocity = random.NextQuaternionF().Rotate(Vector3F.Forward)
* RandomHelper.Random.NextFloat(0, MaxAngularVelocity),
};
if (RandomHelper.Random.NextBool())
{
newObject.LinearVelocity = Vector3F.Zero;
}
if (RandomHelper.Random.NextBool())
{
newObject.AngularVelocity = Vector3F.Zero;
}
if (shape is LineShape || shape is HeightField)
{
// Do not move lines or the height field.
newObject.LinearVelocity = Vector3F.Zero;
newObject.AngularVelocity = Vector3F.Zero;
}
// Create only 1 heightField!
if (shape is HeightField)
{
if (isFirstHeightField)
{
isFirstHeightField = true;
newObject.Pose = new Pose(new Vector3F(-BoxSize, -BoxSize, -BoxSize));
}
else
{
currentShape++;
numberOfObjects = 0;
continue;
}
}
// Add collision object to collision domain.
_domain.CollisionObjects.Add(new CollisionObject(newObject));
//co.Type = CollisionObjectType.Trigger;
//co.Name = "Object" + shape.GetType().Name + "_" + i;
}
}
public void Test1()
{
PlaneRayAlgorithm algo = new PlaneRayAlgorithm(new CollisionDetection());
// Plane in xz plane.
CollisionObject a = new CollisionObject(new GeometricObject
{
Shape = new PlaneShape(Vector3F.UnitY, 1),
Pose = new Pose(new Vector3F(0, -1, 0)),
});
// Ray
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new RayShape(new Vector3F(0, 0, 0), new Vector3F(-1, 0, 0), 100),
Pose = new Pose(new Vector3F(0, 1, 0)),
});
// Separated
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(0, algo.GetContacts(a, b).Count);
Assert.AreEqual(-1, algo.GetClosestPoints(a, b)[0].PenetrationDepth);
// Contained
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0, -1, 0));
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(true, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetContacts(a, b).Count);
Assert.AreEqual(0, algo.GetClosestPoints(a, b)[0].PenetrationDepth);
// Touching
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(true, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetContacts(a, b).Count);
Assert.AreEqual(0, algo.GetClosestPoints(a, b)[0].PenetrationDepth);
// Shooting into plane.
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0, 1, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
Assert.AreEqual(1, algo.GetContacts(a, b).Count);
Assert.AreEqual(1, algo.GetContacts(b, a)[0].PenetrationDepth);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-1, 0, 0), algo.GetContacts(b, a)[0].PositionALocal));
Assert.AreEqual(1, algo.GetClosestPoints(a, b)[0].PenetrationDepth);
}
public void TestTouchingContact()
{
PlaneSphereAlgorithm algo = new PlaneSphereAlgorithm(new CollisionDetection());
CollisionObject objectA = new CollisionObject();
((GeometricObject)objectA.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)objectA.GeometricObject).Pose = new Pose(new Vector3F(10, -3, 20));
CollisionObject objectB = new CollisionObject();
((GeometricObject)objectB.GeometricObject).Shape = new PlaneShape(new Vector3F(0, 1, 0).Normalized, 2);
((GeometricObject)objectB.GeometricObject).Pose = new Pose(new Vector3F(1, 0, 0), QuaternionF.CreateRotationZ(-(float)Math.PI));
ContactSet cs = ContactSet.Create(objectA, objectB);
algo.UpdateContacts(cs, 0);
Assert.AreEqual(objectA, cs.ObjectA);
Assert.AreEqual(objectB, cs.ObjectB);
Assert.AreEqual(1, cs.Count);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(10, -2, 20), cs[0].Position));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 1, 0), cs[0].Normal));
Assert.IsTrue(Numeric.AreEqual(0, cs[0].PenetrationDepth));
}
public void ComputeCollision()
{
CollisionObject a = new CollisionObject();
CompositeShape compo = new CompositeShape();
compo.Children.Add(new GeometricObject(new SphereShape(2), Pose.Identity));
compo.Children.Add(new GeometricObject(new SphereShape(1), new Pose(new Vector3F(0, 3, 0))));
a.GeometricObject = new GeometricObject(compo, Pose.Identity);
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new PlaneShape(new Vector3F(0, 1, 0), 1),
Pose = new Pose(new Vector3F(0, -1, 0)),
});
ContactSet set;
CompositeShapeAlgorithm algo = new CompositeShapeAlgorithm(new CollisionDetection());
set = algo.GetClosestPoints(a, b);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(true, algo.HaveContact(b, a));
Assert.AreEqual(1, set.Count);
Assert.AreEqual(2, set[0].PenetrationDepth);
Assert.AreEqual(new Vector3F(0, -2, 0), set[0].PositionAWorld);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 2, 3), a.GeometricObject.Pose.Orientation);
set = set.Swapped;
algo.UpdateContacts(set, 0);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(true, algo.HaveContact(b, a));
Assert.AreEqual(1, set.Count);
Assert.AreEqual(0, set[0].PenetrationDepth);
Assert.AreEqual(new Vector3F(0, 0, 3), set[0].PositionBWorld);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
set = set.Swapped; // new order: (a, b)
algo.UpdateContacts(set, 0);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(2, set.Count);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-2, 0, 0), set[0].PositionALocal));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-1, 3, 0), set[1].PositionALocal));
Assert.AreEqual(new Vector3F(0, -1, 0), set[0].Normal);
Assert.AreEqual(new Vector3F(0, -1, 0), set[1].Normal);
Assert.AreEqual(2, set[0].PenetrationDepth);
Assert.IsTrue(Numeric.AreEqual(1, set[1].PenetrationDepth));
algo.UpdateClosestPoints(set, 0);
Assert.AreEqual(1, set.Count);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 2.1f, 0), a.GeometricObject.Pose.Orientation);
algo.UpdateContacts(set, 0);
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(0, set.Count);
algo.UpdateClosestPoints(set, 0);
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-2, 0, 0), set[0].PositionALocal));
Assert.AreEqual(new Vector3F(0, -1, 0), set[0].Normal);
}
public void ComputeCollisionLineLine()
{
CollisionObject ray = new CollisionObject(new GeometricObject
{
Shape = new RayShape(new Vector3F(0, 0, -1), new Vector3F(1, 0, 0), 10),
Pose = new Pose(new Vector3F(0, 0, 1)),
});
CollisionObject sphere = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1),
Pose = new Pose(RandomHelper.Random.NextQuaternionF()),
});
RaySphereAlgorithm algo = new RaySphereAlgorithm(new CollisionDetection());
ContactSet set;
set = algo.GetClosestPoints(ray, sphere);
Assert.AreEqual(0, set[0].PenetrationDepth);
Assert.AreEqual(Vector3F.UnitY, set[0].Normal);
Assert.AreEqual(true, algo.HaveContact(ray, sphere));
set = set.Swapped;
algo.UpdateContacts(set, 0);
Assert.AreEqual(0, set[0].PenetrationDepth);
Assert.AreEqual(-Vector3F.UnitY, set[0].Normal);
((GeometricObject)sphere.GeometricObject).Pose = new Pose(new Vector3F(10, 0, 0), sphere.GeometricObject.Pose.Orientation);
set = algo.GetClosestPoints(sphere, ray);
Assert.IsTrue(Numeric.AreEqual(9, set[0].PenetrationDepth));
Assert.IsTrue(Vector3F.AreNumericallyEqual(-Vector3F.UnitX, set[0].Normal));
Assert.AreEqual(true, algo.HaveContact(ray, sphere));
((GeometricObject)ray.GeometricObject).Pose = new Pose(new Vector3F(0, -2, 1), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
set = algo.GetClosestPoints(sphere, ray);
Assert.IsTrue(Numeric.AreEqual(-9, set[0].PenetrationDepth));
Assert.IsTrue(Vector3F.AreNumericallyEqual(-Vector3F.UnitX, set[0].Normal));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(4.5f, 0, 0), set[0].Position));
Assert.AreEqual(false, algo.HaveContact(ray, sphere));
algo.UpdateContacts(set, 0);
Assert.AreEqual(0, set.Count);
// Degenerate case: sphere radius is 0.
((GeometricObject)sphere.GeometricObject).Shape = new SphereShape(0);
((GeometricObject)ray.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 1));
algo.UpdateClosestPoints(set, 0);
Assert.AreEqual(0, set[0].PenetrationDepth);
Assert.AreEqual(-Vector3F.UnitY, set[0].Normal);
Assert.AreEqual(true, algo.HaveContact(ray, sphere));
}
public void RayCastStopsAtFirstHit()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
CollisionObject ray = new CollisionObject();
((GeometricObject)ray.GeometricObject).Shape = new RayShape(new Vector3F(), new Vector3F(1, 0, 0), 100)
{
StopsAtFirstHit = true,
};
//ray.Name = "Ray";
CollisionObject b = new CollisionObject();
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(-10, 0, 0f));
//b.Name = "b";
CollisionObject c = new CollisionObject();
((GeometricObject)c.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0f));
//c.Name = "c";
CollisionObject d = new CollisionObject();
((GeometricObject)d.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)d.GeometricObject).Pose = new Pose(new Vector3F(10, 0, 0f));
//d.Name = "d";
CollisionObject e = new CollisionObject();
((GeometricObject)e.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)e.GeometricObject).Pose = new Pose(new Vector3F(20, 0, 0f));
//e.Name = "e";
CollisionObject f = new CollisionObject();
((GeometricObject)f.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)f.GeometricObject).Pose = new Pose(new Vector3F(110, 0, 0f));
//f.Name = "f";
// Positions: b=-10, c=0, d=10, e=20, f=110
domain.CollisionObjects.Add(ray);
domain.CollisionObjects.Add(b);
domain.CollisionObjects.Add(d);
domain.CollisionObjects.Add(c);
domain.CollisionObjects.Add(e);
domain.CollisionObjects.Add(f);
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, c));
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3F(30));
// Positions: b=-10, d=10, e=20, c=30, f=110
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, d));
((GeometricObject)d.GeometricObject).Pose = new Pose(new Vector3F(40));
// Positions: b=-10, e=20, c=30, d=40, f=110
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, e));
((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
domain.Update(0.01f);
Assert.AreEqual(0, domain.GetContacts(ray).Count());
((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, QuaternionF.CreateRotationZ(ConstantsF.Pi));
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, b));
((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, QuaternionF.Identity);
domain.Update(0.01f);
// Positions: b=-10, e=20, c=30, d=40, f=110
CollisionObject gNotInDomain = new CollisionObject();
((GeometricObject)gNotInDomain.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)gNotInDomain.GeometricObject).Pose = new Pose(new Vector3F(10, 0, 0f));
Assert.AreEqual(true, domain.HaveContact(ray, gNotInDomain));
Assert.AreEqual(1, domain.GetContacts(gNotInDomain).Count());
Assert.AreEqual(1, domain.GetContacts(ray, gNotInDomain).Count);
Assert.AreEqual(true, domain.HasContact(gNotInDomain));
((GeometricObject)gNotInDomain.GeometricObject).Pose = new Pose(new Vector3F(25, 0, 0f)); // behind e
Assert.AreEqual(false, domain.HaveContact(ray, gNotInDomain));
Assert.AreEqual(false, domain.HaveContact(gNotInDomain, ray));
Assert.AreEqual(false, domain.HasContact(gNotInDomain));
Assert.AreEqual(0, domain.GetContacts(gNotInDomain).Count());
Assert.IsNull(domain.GetContacts(ray, gNotInDomain));
Assert.IsNull(domain.GetContacts(gNotInDomain, ray));
// Remove ray from domain.
domain.CollisionObjects.Remove(ray);
domain.Update(0.01f);
Assert.AreEqual(0, domain.ContactSets.Count);
// Positions: b=-10, e=20, g=25, c=30, d=40, f=110
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, e));
Assert.AreEqual(false, domain.HaveContact(ray, c));
Assert.AreEqual(false, domain.HaveContact(ray, gNotInDomain));
Assert.IsNull(domain.GetContacts(ray, gNotInDomain));
}
public void ComputeCollisionLineLine()
{
CollisionObject line0 = new CollisionObject(new GeometricObject
{
Shape = new LineShape(new Vector3F(1, 2, 3), new Vector3F(1, 0, 0)),
});
//line0.Name = "line0";
CollisionObject line1 = new CollisionObject(new GeometricObject
{
Shape = new LineShape(new Vector3F(0, 0, 1), new Vector3F(1, 0, 0)),
});
//line1.Name = "line1";
LineAlgorithm algo = new LineAlgorithm(new CollisionDetection());
ContactSet set;
((GeometricObject)line1.GeometricObject).Pose = new Pose(new Vector3F(1, 2, 2), line1.GeometricObject.Pose.Orientation);
set = algo.GetClosestPoints(line0, line1);
Assert.AreEqual(0, set[0].PenetrationDepth);
Assert.AreEqual(Vector3F.UnitY, set[0].Normal);
Assert.AreEqual(true, algo.HaveContact(line0, line1));
((GeometricObject)line1.GeometricObject).Pose = new Pose(line1.GeometricObject.Pose.Position, QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
set = algo.GetClosestPoints(line0, line1);
Assert.IsTrue(Numeric.AreEqual(0, set[0].PenetrationDepth));
Assert.AreEqual(new Vector3F(1, 2, 3), set[0].Position);
Assert.IsTrue(Vector3F.AreNumericallyEqual(Vector3F.UnitZ, set[0].Normal));
Assert.AreEqual(true, algo.HaveContact(line0, line1));
((GeometricObject)line1.GeometricObject).Pose = new Pose(new Vector3F(1, 2, 4), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
set = algo.GetClosestPoints(line1, line0);
Assert.IsTrue(Numeric.AreEqual(-2, set[0].PenetrationDepth));
Assert.AreEqual(new Vector3F(1, 2, 4), set[0].Position);
Assert.AreEqual(-Vector3F.UnitZ, set[0].Normal);
Assert.AreEqual(false, algo.HaveContact(line0, line1));
algo.UpdateContacts(set, 0.01f);
Assert.AreEqual(0, set.Count);
}
