internal DigitalRuneSphericalJoint(SphericalJointDescriptor descriptor)
{
WrappedSphericalJoint = new BallJoint();
#region set RigidBodies
if (!(descriptor.RigidBodyA is RigidBody))
throw new ArgumentException(String.Format("The type of the property 'RigidBodyA' must be '{0}'.", typeof(RigidBody)));
WrappedSphericalJoint.BodyA = ((RigidBody)descriptor.RigidBodyA).WrappedRigidBody;
_rigidBodyA = descriptor.RigidBodyA;
if (!(descriptor.RigidBodyB is RigidBody))
throw new ArgumentException("The type of the property 'RigidBodyB' must be 'System.Physics.DigitalRune.RigidBody'.");
WrappedSphericalJoint.BodyB = ((RigidBody)descriptor.RigidBodyB).WrappedRigidBody;
_rigidBodyB = descriptor.RigidBodyB;
#endregion
WrappedSphericalJoint.AnchorPositionALocal = descriptor.AnchorPositionALocal.ToDigitalRune();
WrappedSphericalJoint.AnchorPositionBLocal = descriptor.AnchorPositionBLocal.ToDigitalRune();
Descriptor = descriptor;
}
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);
}
public ConstraintSample1(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);
// Tip: It is best to initialize bodies in a position where the constraints are
// satisfied - but even if they are initialized in other poses, the simulation
// will try to move them to the correct positions.
// ----- FixedJoint
// Create two boxes and connect them with a FixedJoint.
RigidBody box0 = new RigidBody(new BoxShape(1, 1, 1))
{
Pose = new Pose(new Vector3F(-5, 3, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box0);
RigidBody box1 = new RigidBody(new BoxShape(1.2f, 1.2f, 1.2f))
{
Pose = new Pose(new Vector3F(-5, 3 - 1.2f, 0))
};
Simulation.RigidBodies.Add(box1);
FixedJoint fixedJoint = new FixedJoint
{
BodyA = box0,
// The attachment point on the first box is at the bottom of the box.
AnchorPoseALocal = new Pose(new Vector3F(0, -0.5f, 0)),
BodyB = box1,
// The attachment point on the second box is at the top of the box.
AnchorPoseBLocal = new Pose(new Vector3F(0, 0.6f, 0)),
// Disable collision between the connected bodies.
CollisionEnabled = false,
};
Simulation.Constraints.Add(fixedJoint);
// ----- BallJoint
// Create two boxes and connect a corner of the second box with a ball-and-socked joint
// to the first box.
RigidBody box2 = new RigidBody(new BoxShape(1, 1, 1))
{
Pose = new Pose(new Vector3F(-3, 3, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box2);
RigidBody box3 = new RigidBody(new BoxShape(1.2f, 1.2f, 1.2f))
{
Pose = new Pose(new Vector3F(-3, 3, 0))
};
Simulation.RigidBodies.Add(box3);
BallJoint ballJoint = new BallJoint
{
BodyA = box2,
// The attachment point on the first box is at the box bottom.
AnchorPositionALocal = new Vector3F(0, -0.5f, 0),
BodyB = box3,
// The attachment point on the second box is a corner of the box.
AnchorPositionBLocal = new Vector3F(0.6f, 0.6f, 0.6f),
};
Simulation.Constraints.Add(ballJoint);
// ----- PrismaticJoint
// A prismatic joint is like a slider (without rotation around the slider axis).
RigidBody box4 = new RigidBody(new BoxShape(1, 1, 1))
{
Pose = new Pose(new Vector3F(-1, 3, 0)),
};
Simulation.RigidBodies.Add(box4);
RigidBody box5 = new RigidBody(new BoxShape(0.5f, 1.5f, 0.5f))
{
Pose = new Pose(new Vector3F(-1, 3 - 0.5f, 0))
};
Simulation.RigidBodies.Add(box5);
PrismaticJoint prismaticJoint = new PrismaticJoint
{
BodyA = box4,
// The attachment point on the first box is in the center of the box.
// The slider joint allows linear movement along the first constraint axis.
// --> To define the constraint anchor orientation:
// The columns are the axes. We set the local -y axis in the first column. This is the
// slider axis. The other two columns are two orthonormal axes.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseALocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
BodyB = box5,
// The attachment point on the second box is at the top of the box.
AnchorPoseBLocal = new Pose(new Vector3F(0, 0.75f, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
CollisionEnabled = false,
// The slider axis is -y. We limit the up movement, so that the anchor point on the second
// body can slide up to the anchor point on the first body, but not higher.
Minimum = 0,
// The second body can slide down until the anchor points have a max. distance of 0.5.
Maximum = 0.5f,
};
Simulation.Constraints.Add(prismaticJoint);
// ----- CylindricalJoint
// A cylindrical joint is a slider that allows rotation around the slider axis.
RigidBody box6 = new RigidBody(new BoxShape(1, 1, 1))
{
Pose = new Pose(new Vector3F(1, 3, 0)),
};
Simulation.RigidBodies.Add(box6);
RigidBody box7 = new RigidBody(new BoxShape(0.5f, 1.5f, 0.5f))
{
Pose = new Pose(new Vector3F(1, 3 - 0.5f, 0))
};
Simulation.RigidBodies.Add(box7);
CylindricalJoint cylindricalJoint = new CylindricalJoint
{
BodyA = box6,
AnchorPoseALocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
BodyB = box7,
AnchorPoseBLocal = new Pose(new Vector3F(0, 0.75f, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
CollisionEnabled = false,
// The linear movement limits on the slider axis.
LinearMinimum = 0,
LinearMaximum = 0.5f,
// The rotation limits around the slider axis (in radians). Here, we allow free
// rotations.
AngularMinimum = float.NegativeInfinity,
AngularMaximum = float.PositiveInfinity,
};
Simulation.Constraints.Add(cylindricalJoint);
// ----- HingeJoint
// Hinge joints allow rotations around one axis. They can be used to model swinging doors
// or rotating wheels.
RigidBody cylinder0 = new RigidBody(new CylinderShape(0.1f, 2f))
{
Pose = new Pose(new Vector3F(3, 1, 0)),
MotionType = MotionType.Static
};
Simulation.RigidBodies.Add(cylinder0);
RigidBody box8 = new RigidBody(new BoxShape(1f, 1.8f, 0.1f))
{
Pose = new Pose(new Vector3F(3 + 0.5f, 1, 0))
};
Simulation.RigidBodies.Add(box8);
HingeJoint hingeJoint = new HingeJoint
{
BodyA = cylinder0,
AnchorPoseALocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
BodyB = box8,
AnchorPoseBLocal = new Pose(new Vector3F(-0.5f, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
CollisionEnabled = false,
// The rotation limits around the hinge axis (in radians).
Minimum = -ConstantsF.PiOver2,
Maximum = ConstantsF.PiOver2,
};
Simulation.Constraints.Add(hingeJoint);
}
private void InitializeMarionetteConstraints()
{
// Create constraints that pull important body parts to Kinect joint positions.
// The Update() method below will update the BallJoint.AnchorPositionALocal vectors.
// Limit the maximal forces that these joints can apply. We do not want these joints to be
// so strong that they can violate the ragdoll joint and limit constraints. Increasing
// this force makes the ragdoll more responsive but can also violate ragdoll constraints
// (e.g. by stretching the limbs).
const float maxForce = 1000;
var ragdollSkeleton = _meshNode.SkeletonPose.Skeleton;
_headSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("Head")],
MaxForce = maxForce,
};
Simulation.Constraints.Add(_headSpring);
_elbowLeftSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("L_Forearm")],
MaxForce = maxForce / 2, // Elbow springs are weaker because the correct
// hand position is more important and the hand
// constraint should therefore be stronger.
};
// This constraint should be attached at the elbow position and not at the center of the forearm:
var elbowLeftJointPosition = _meshNode.SkeletonPose.GetBonePoseAbsolute(ragdollSkeleton.GetIndex("L_Forearm")).Translation;
_elbowLeftSpring.AnchorPositionBLocal = _elbowLeftSpring.BodyB.Pose.ToLocalPosition(elbowLeftJointPosition);
Simulation.Constraints.Add(_elbowLeftSpring);
_handLeftSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("L_Hand")],
MaxForce = maxForce,
};
Simulation.Constraints.Add(_handLeftSpring);
_elbowRightSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("R_Forearm")],
MaxForce = maxForce / 2,
};
// This constraint should be attached at the elbow position and not at the center of the forearm:
var elbowRightJointPosition = _meshNode.SkeletonPose.GetBonePoseAbsolute(ragdollSkeleton.GetIndex("R_Forearm")).Translation;
_elbowRightSpring.AnchorPositionBLocal = _elbowRightSpring.BodyB.Pose.ToLocalPosition(elbowRightJointPosition);
Simulation.Constraints.Add(_elbowRightSpring);
_handRightSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("R_Hand")],
MaxForce = maxForce,
};
Simulation.Constraints.Add(_handRightSpring);
_kneeLeftSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("L_Knee2")],
MaxForce = maxForce,
};
// This constraint should be attached at the knee position and not at the center of the lower leg:
var kneeLeftJointPosition = _meshNode.SkeletonPose.GetBonePoseAbsolute(ragdollSkeleton.GetIndex("L_Knee2")).Translation;
_kneeLeftSpring.AnchorPositionBLocal = _kneeLeftSpring.BodyB.Pose.ToLocalPosition(kneeLeftJointPosition);
Simulation.Constraints.Add(_kneeLeftSpring);
_ankleLeftSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("L_Ankle1")],
MaxForce = maxForce,
};
Simulation.Constraints.Add(_ankleLeftSpring);
_kneeRightSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("R_Knee")],
MaxForce = maxForce,
};
// This constraint should be attached at the knee position and not at the center of the lower leg:
var kneeRightJointPosition = _meshNode.SkeletonPose.GetBonePoseAbsolute(ragdollSkeleton.GetIndex("R_Knee")).Translation;
_kneeRightSpring.AnchorPositionBLocal = _kneeRightSpring.BodyB.Pose.ToLocalPosition(kneeRightJointPosition);
Simulation.Constraints.Add(_kneeRightSpring);
_ankleRightSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("R_Ankle")],
MaxForce = maxForce,
};
Simulation.Constraints.Add(_ankleRightSpring);
}
/// <summary>
/// Adds a BallJoint between the specified bones.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="ragdoll">The ragdoll.</param>
/// <param name="parent">The parent.</param>
/// <param name="child">The child.</param>
private static void AddJoint(SkeletonPose skeletonPose, Ragdoll ragdoll, int parent, int child)
{
// Get bodies and offsets for the bones.
var skeleton = skeletonPose.Skeleton;
var childBody = ragdoll.Bodies[child];
var childOffset = ragdoll.BodyOffsets[child];
var parentBody = ragdoll.Bodies[parent];
var parentOffset = ragdoll.BodyOffsets[parent];
// Get bind poses of the bones in model space.
var parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(parent).Inverse;
var childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(child).Inverse;
// The child pose relative to the parent bone.
var bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
// Add BallJoint that connects the two bones. The position of the joint is the
// origin of the child bone.
BallJoint joint = new BallJoint
{
BodyA = parentBody,
BodyB = childBody,
CollisionEnabled = false,
AnchorPositionALocal = (parentOffset.Inverse * bindPoseRelative).Position,
AnchorPositionBLocal = childOffset.Inverse.Position,
ErrorReduction = 0.2f,
Softness = 0.0001f,
};
ragdoll.Joints.Add(joint);
}
/// <summary>
/// Removes all dynamic rigid bodies from the simulation.
/// </summary>
private void ClearScene()
{
// Remove all rigid bodies, except static bodies.
var staticBodies = Simulation.RigidBodies
.Where(body => body.MotionType == MotionType.Static)
.ToArray();
Simulation.RigidBodies.Clear();
Simulation.RigidBodies.AddRange(staticBodies);
Simulation.Constraints.Clear();
_spring = null;
}
/// <summary>
/// Allows the user to drag a rigid body by using touch.
/// </summary>
private void DragBodies()
{
// Here is how it works:
// We first make a hit-test using a ray to check whether the user touches a rigid body.
// If there is a hit we create a spring (using a ball-socket joint) and connect the rigid
// body to the touch location. Every time the user moves her finger we update the position
// of the spring and the spring pulls the rigid body towards the finger.
// We use raw touch points to select and drag a rigid body.
TouchCollection touches = InputService.TouchCollection;
if (touches.Count == 0)
{
// No touches detected.
if (_spring != null)
{
// There is an active spring, so the user is currently dragging a rigid body.
// Release the body by removing the spring.
_spring.Simulation.Constraints.Remove(_spring);
_spring = null;
}
}
else
{
// Touch detected.
TouchLocation touchLocation = touches[0];
// Convert the touch location from screen coordinates to world coordinates.
var cameraNode = GraphicsScreen.CameraNode;
Vector3 pScreen = new Vector3(touchLocation.Position.X, touchLocation.Position.Y, 0);
Vector3 pWorld = GraphicsService.GraphicsDevice.Viewport.Unproject(
pScreen,
cameraNode.Camera.Projection,
(Matrix)cameraNode.View,
Matrix.Identity);
// pWorld is point on the near clip plane of the camera.
// Set the origin and direction of the ray for hit-testing.
Vector3F rayOrigin = _cameraPosition;
Vector3F rayDirection = ((Vector3F)pWorld - _cameraPosition).Normalized;
if (touchLocation.State == TouchLocationState.Pressed)
{
// Let's create a ray and see if we hit a rigid body.
// (Create the ray shape and the required collision object only once.)
if (_rayShape == null)
{
_rayShape = new RayShape { StopsAtFirstHit = true };
_rayCollisionObject = new CollisionObject(new GeometricObject(_rayShape));
}
// Set the origin and direction of the ray.
_rayShape.Origin = rayOrigin;
_rayShape.Direction = rayDirection.Normalized;
// Make a hit test using the collision detection and get the first contact found.
var contactSet = Simulation.CollisionDomain
.GetContacts(_rayCollisionObject)
.FirstOrDefault();
if (contactSet != null && contactSet.Count > 0)
{
// Get the point where the ray hits the rigid body.
Contact contact = contactSet[0];
// The contact sets contains two objects ("ObjectA" and "ObjectB").
// One is the ray the other is the object that was hit by the ray.
var hitCollisionObject = (contactSet.ObjectA == _rayCollisionObject)
? contactSet.ObjectB
: contactSet.ObjectA;
// Check whether the object is a dynamic rigid body.
var hitBody = hitCollisionObject.GeometricObject as RigidBody;
if (hitBody != null && hitBody.MotionType == MotionType.Dynamic)
{
// Remove the old joint, in case a rigid body is already grabbed.
if (_spring != null && _spring.Simulation != null)
_spring.Simulation.Constraints.Remove(_spring);
// The penetration depth tells us the distance from the ray origin to the rigid body
// in view direction.
_springAnchorDistanceFromCamera = contact.PenetrationDepth;
// Get the position where the ray hits the other object.
// (The position is defined in the local space of the object.)
Vector3F hitPositionLocal = (contactSet.ObjectA == _rayCollisionObject)
? contact.PositionBLocal
: contact.PositionALocal;
// Attach the rigid body at the touch location using a ball-socket joint.
// (Note: We could also use a FixedJoint, if we don't want any rotations.)
_spring = new BallJoint
{
BodyA = hitBody,
AnchorPositionALocal = hitPositionLocal,
// We need to attach the grabbed object to a second body. In this case we just want to
// anchor the object at a specific point in the world. To achieve this we can use the
// special rigid body "World", which is defined in the simulation.
BodyB = Simulation.World,
AnchorPositionBLocal = rayOrigin + rayDirection * _springAnchorDistanceFromCamera,
// Some constraint adjustments.
ErrorReduction = 0.3f,
// We set a softness > 0. This makes the joint "soft" and it will act like
// damped spring.
Softness = 0.00001f,
// We limit the maximal force. This reduces the ability of this joint to violate
// other constraints.
MaxForce = 1e6f
};
// Add the spring to the simulation.
Simulation.Constraints.Add(_spring);
}
}
}
else if (touchLocation.State == TouchLocationState.Moved)
{
if (_spring != null)
{
// User has grabbed something.
// Update the position of the object by updating the anchor position of the ball-socket
// joint.
_spring.AnchorPositionBLocal = rayOrigin + rayDirection * _springAnchorDistanceFromCamera;
// Reduce the angular velocity by a certain factor. (This acts like a damping because we
// do not want the object to rotate like crazy.)
_spring.BodyA.AngularVelocity *= 0.9f;
}
}
}
}
private static void DrawAngularLimit(BallJoint joint, AngularLimit limit)
{
Vector3F jointPosition = joint.BodyA.Pose.ToWorldPosition(joint.AnchorPositionALocal);
// A transformation that converts from constraint anchor space to world space.
Pose constraintToWorld = limit.BodyA.Pose * new Pose(limit.AnchorOrientationALocal);
// Draw an arc for each rotation axis.
DrawArc(constraintToWorld, jointPosition, Vector3F.UnitX, Vector3F.UnitY, limit.Minimum.X, limit.Maximum.X, Color.Red);
DrawArc(constraintToWorld, jointPosition, Vector3F.UnitY, Vector3F.UnitX, limit.Minimum.Y, limit.Maximum.Y, Color.Green);
DrawArc(constraintToWorld, jointPosition, Vector3F.UnitZ, Vector3F.UnitX, limit.Minimum.Z, limit.Maximum.Z, Color.Blue);
}
private static void DrawTwistSwingLimit(BallJoint joint, TwistSwingLimit limit)
{
// ----- Draw swing cone.
// The tip of the swing cone:
Vector3F coneTip = joint.BodyA.Pose.ToWorldPosition(joint.AnchorPositionALocal);
// The first point on the swing cone:
Vector3 previousConePoint = (Vector3)limit.GetPointOnCone(0, coneTip, _scale);
// Draw swing cone.
const int numberOfSegments = 24;
const float segmentAngle = ConstantsF.TwoPi / numberOfSegments;
Color color = Color.Violet;
for (int i = 0; i < numberOfSegments; i++)
{
Vector3 conePoint = (Vector3)limit.GetPointOnCone((i + 1) * segmentAngle, coneTip, _scale);
// Line from cone tip to cone base.
_points[_pointCount] = new VertexPositionColor((Vector3)coneTip, color);
IncrementPointCount();
_points[_pointCount] = new VertexPositionColor(conePoint, color);
IncrementPointCount();
// Line on the cone base.
_points[_pointCount] = new VertexPositionColor(previousConePoint, color);
IncrementPointCount();
_points[_pointCount] = new VertexPositionColor(conePoint, color);
IncrementPointCount();
previousConePoint = conePoint;
}
// ----- Draw twist axis.
// The x-axis is the twist direction.
Vector3F twistAxis = Vector3F.UnitX;
// The twist axis relative to body B.
Vector3F twistAxisDirectionBLocal = limit.AnchorOrientationBLocal * twistAxis;
// The twist axis relative to world space.
Vector3F twistAxisDirection = limit.BodyB.Pose.ToWorldDirection(twistAxisDirectionBLocal);
// (A similar computation is used in DrawArc() below.)
// Line in twist direction.
_points[_pointCount] = new VertexPositionColor((Vector3)coneTip, Color.Red);
IncrementPointCount();
_points[_pointCount] = new VertexPositionColor((Vector3)(coneTip + twistAxisDirection * _scale), Color.Red);
IncrementPointCount();
// A transformation that converts from constraint anchor space to world space.
Pose constraintToWorld = limit.BodyA.Pose * new Pose(limit.AnchorOrientationALocal);
// Draw an arc that visualizes the twist limits.
DrawArc(constraintToWorld, coneTip, Vector3F.UnitX, Vector3F.UnitY, limit.Minimum.X, limit.Maximum.X, Color.Red);
}
private static void AddJoint(Ragdoll ragdoll, Skeleton skeleton, AvatarBone parentBone, AvatarBone childBone, float errorReduction, float softness)
{
int parentIndex = (int)parentBone;
int childIndex = (int)childBone;
// To define AnchorPositionALocal/AnchorPositionBLocal:
// To get the AnchorPositionALocal we apply jointPosesAbsolute[indexA].Inverse to
// convert the joint pose from model space into the joints space of parentBone. Then we apply
// ragdoll.BodyOffsets[boneAIndex].Inverse to convert from joint space to body space. The result is
// the joint position of B in body space of A.
// To get AnchorPositionBLocal, we only have to apply the inverse offset.
BallJoint joint = new BallJoint
{
BodyA = ragdoll.Bodies[parentIndex],
BodyB = ragdoll.Bodies[childIndex],
CollisionEnabled = false,
AnchorPositionALocal = (ragdoll.BodyOffsets[parentIndex].Inverse * skeleton.GetBindPoseAbsoluteInverse(parentIndex) * skeleton.GetBindPoseAbsoluteInverse(childIndex).Inverse).Translation,
AnchorPositionBLocal = ragdoll.BodyOffsets[childIndex].Inverse.Position,
ErrorReduction = errorReduction,
Softness = softness,
};
ragdoll.Joints[childIndex] = joint;
}
public override void Update(GameTime gameTime)
{
if (_spring != null && !_inputService.IsDown(MouseButtons.Left) && !_inputService.IsDown(Buttons.LeftTrigger, PlayerIndex.One))
{
// The user has released the object.
_spring.Simulation.Constraints.Remove(_spring);
_spring = null;
}
if (_inputService.IsPressed(MouseButtons.Left, false) || _inputService.IsPressed(Buttons.LeftTrigger, false, PlayerIndex.One))
{
// The user has pressed the grab button.
// Remove the old joint, in case anything is grabbed.
if (_spring != null && _spring.Simulation != null)
_spring.Simulation.Constraints.Remove(_spring);
// The spring is attached at the position that is targeted with the cross-hair.
// We can perform a ray hit-test to find the position. The ray starts at the camera
// position and shoots forward (-z direction).
Camera camera = Game.Components.OfType<Camera>().First();
Vector3F cameraPosition = camera.Pose.Position;
Vector3F cameraDirection = camera.Pose.ToWorldDirection(-Vector3F.UnitZ);
// Create a ray for picking.
RayShape ray = new RayShape(cameraPosition, cameraDirection, 1000);
// The ray should stop at the first hit. We only want the first object.
ray.StopsAtFirstHit = true;
// The collision detection requires a CollisionObject.
CollisionObject rayCollisionObject = new CollisionObject(new GeometricObject(ray, Pose.Identity));
// Assign the collision object to collision group 2. (In PhysicsGame.cs a collision filter
// based on collision groups was set.)
rayCollisionObject.CollisionGroup = 2;
// Get the first object that has contact with the ray.
ContactSet contactSet = _simulation.CollisionDomain.GetContacts(rayCollisionObject).FirstOrDefault();
if (contactSet != null && contactSet.Count > 0)
{
// The ray has hit something.
// The contact set contains all detected contacts between the ray and the rigid body.
// Get the first contact in the contact set. (A ray hit usually contains exactly 1 contact.)
Contact contact = contactSet[0];
// The contact set contains the object pair of the collision. One object is the ray.
// The other is the object we want to grab.
CollisionObject hitCollisionObject = (contactSet.ObjectA == rayCollisionObject) ? contactSet.ObjectB : contactSet.ObjectA;
// Check whether a dynamic rigid body was hit.
RigidBody hitBody = hitCollisionObject.GeometricObject as RigidBody;
if (hitBody != null && hitBody.MotionType == MotionType.Dynamic)
{
// Attach the rigid body at the cursor position using a ball-socket joint.
// (Note: We could also use a FixedJoint, if we don't want any rotations.)
// The penetration depth tells us the distance from the ray origin to the rigid body.
_springAttachmentDistanceFromObserver = contact.PenetrationDepth;
// Get the position where the ray hits the other object.
// (The position is defined in the local space of the object.)
Vector3F hitPositionLocal = (contactSet.ObjectA == rayCollisionObject) ? contact.PositionBLocal : contact.PositionALocal;
_spring = new BallJoint
{
BodyA = hitBody,
AnchorPositionALocal = hitPositionLocal,
// We need to attach the grabbed object to a second body. In this case we just want to
// anchor the object at a specific point in the world. To achieve this we can use the
// special rigid body "World", which is defined in the simulation.
BodyB = _simulation.World,
// AnchorPositionBLocal is set below.
// Some constraint adjustments.
ErrorReduction = 0.3f,
// We set a softness > 0. This makes the joint "soft" and it will act like
// damped spring.
Softness = 0.00001f,
// We limit the maximal force. This reduces the ability of this joint to violate
// other constraints.
MaxForce = 1e6f
};
// Add the spring to the simulation.
_simulation.Constraints.Add(_spring);
}
}
}
if (_spring != null)
{
// User has grabbed something.
// Update the position of the object by updating the anchor position of the ball-socket
// joint.
Camera camera = Game.Components.OfType<Camera>().First();
Vector3F cameraPosition = camera.Pose.Position;
Vector3F cameraDirection = camera.Pose.ToWorldDirection(-Vector3F.UnitZ);
_spring.AnchorPositionBLocal = cameraPosition + cameraDirection * _springAttachmentDistanceFromObserver;
// Reduce the angular velocity by a certain factor. (This acts like a damping because we
// do not want the object to rotate like crazy.)
_spring.BodyA.AngularVelocity *= 0.9f;
}
base.Update(gameTime);
}
// Using a softness value > 0 is important to give the ragdoll a more natural movement and to
// avoid jittering.
public static void AddRagdoll(Simulation simulation, float scale, Vector3F ragdollPosition, float softness, bool addDamping)
{
// Ragdolls are usually used in games to create realistic death animations of
// characters. The character is usually rendered using a skinned triangle mesh.
// But in the physics simulation the body parts of the character are represented
// using simple shapes, such as spheres, capsules, boxes, or convex polyhedra,
// which are connected with joints.
// The physics simulations computes how these parts collide and fall. The positions
// and orientations are then read back each frame to update the animation of the
// triangle mesh.
// In this example the ragdoll is built from spheres, capsules and boxes. The
// rigid bodies are created in code. In practice, ragdolls should be built using
// external tools, such as a 3D modeler or a game editor.
#region ----- Create rigid bodies for the most relevant body parts -----
// The density used for all bodies.
const float density = 1000;
BoxShape pelvisShape = new BoxShape(0.3f * scale, 0.22f * scale, 0.20f * scale);
MassFrame pelvisMass = MassFrame.FromShapeAndDensity(pelvisShape, Vector3F.One, density, 0.01f, 3);
RigidBody pelvis = new RigidBody(pelvisShape, pelvisMass, null)
{
Pose = new Pose(new Vector3F(0f, 0.01f * scale, -0.03f * scale) + ragdollPosition),
};
simulation.RigidBodies.Add(pelvis);
BoxShape torsoShape = new BoxShape(0.35f * scale, 0.22f * scale, 0.44f * scale);
MassFrame torsoMass = MassFrame.FromShapeAndDensity(torsoShape, Vector3F.One, density, 0.01f, 3);
RigidBody torso = new RigidBody(torsoShape, torsoMass, null)
{
Pose = new Pose(new Vector3F(0f, 0.01f * scale, -0.4f * scale) + ragdollPosition),
};
simulation.RigidBodies.Add(torso);
SphereShape headShape = new SphereShape(0.13f * scale);
MassFrame headMass = MassFrame.FromShapeAndDensity(headShape, Vector3F.One, density, 0.01f, 3);
RigidBody head = new RigidBody(headShape, headMass, null)
{
Pose = new Pose(new Vector3F(0f * scale, 0f, -0.776f * scale) + ragdollPosition),
};
simulation.RigidBodies.Add(head);
CapsuleShape upperArmShape = new CapsuleShape(0.08f * scale, 0.3f * scale);
MassFrame upperArmMass = MassFrame.FromShapeAndDensity(upperArmShape, Vector3F.One, density, 0.01f, 3);
RigidBody leftUpperArm = new RigidBody(upperArmShape, upperArmMass, null)
{
Pose = new Pose(new Vector3F(-0.32f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(leftUpperArm);
RigidBody rightUpperArm = new RigidBody(upperArmShape, upperArmMass, null)
{
Pose = new Pose(new Vector3F(0.32f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(rightUpperArm);
CapsuleShape lowerArmShape = new CapsuleShape(0.08f * scale, 0.4f * scale);
MassFrame lowerArmMass = MassFrame.FromShapeAndDensity(lowerArmShape, Vector3F.One, density, 0.01f, 3);
RigidBody leftLowerArm = new RigidBody(lowerArmShape, lowerArmMass, null)
{
Pose = new Pose(new Vector3F(-0.62f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(leftLowerArm);
RigidBody rightLowerArm = new RigidBody(lowerArmShape, lowerArmMass, null)
{
Pose = new Pose(new Vector3F(0.62f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(rightLowerArm);
CapsuleShape upperLegShape = new CapsuleShape(0.09f * scale, 0.5f * scale);
MassFrame upperLegMass = MassFrame.FromShapeAndDensity(upperLegShape, Vector3F.One, density, 0.01f, 3);
RigidBody leftUpperLeg = new RigidBody(upperLegShape, upperLegMass, null)
{
Pose = new Pose(new Vector3F(-0.10f * scale, 0.01f * scale, 0.233f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(leftUpperLeg);
RigidBody rightUpperLeg = new RigidBody(upperLegShape, upperLegMass, null)
{
Pose = new Pose(new Vector3F(0.10f * scale, 0.01f * scale, 0.233f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(rightUpperLeg);
CapsuleShape lowerLegShape = new CapsuleShape(0.08f * scale, 0.4f * scale);
MassFrame lowerLegMass = MassFrame.FromShapeAndDensity(pelvisShape, Vector3F.One, density, 0.01f, 3);
RigidBody leftLowerLeg = new RigidBody(lowerLegShape, lowerLegMass, null)
{
Pose = new Pose(new Vector3F(-0.11f * scale, 0.01f * scale, 0.7f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(leftLowerLeg);
RigidBody rightLowerLeg = new RigidBody(lowerLegShape, lowerLegMass, null)
{
Pose = new Pose(new Vector3F(0.11f * scale, 0.01f * scale, 0.7f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(rightLowerLeg);
BoxShape footShape = new BoxShape(0.12f * scale, 0.28f * scale, 0.07f * scale);
MassFrame footMass = MassFrame.FromShapeAndDensity(footShape, Vector3F.One, density, 0.01f, 3);
RigidBody leftFoot = new RigidBody(footShape, footMass, null)
{
Pose = new Pose(new Vector3F(-0.11f * scale, -0.06f * scale, 0.94f * scale) + ragdollPosition),
};
simulation.RigidBodies.Add(leftFoot);
RigidBody rightFoot = new RigidBody(footShape, footMass, null)
{
Pose = new Pose(new Vector3F(0.11f * scale, -0.06f * scale, 0.94f * scale) + ragdollPosition),
};
simulation.RigidBodies.Add(rightFoot);
#endregion
#region ----- Add joints between body parts -----
float errorReduction = 0.3f;
float maxForce = float.PositiveInfinity;
Vector3F pelvisJointPosition = new Vector3F(0f, 0.026f * scale, -0.115f * scale) + ragdollPosition;
HingeJoint pelvisJoint = new HingeJoint
{
BodyA = torso,
BodyB = pelvis,
AnchorPoseALocal = new Pose(torso.Pose.ToLocalPosition(pelvisJointPosition)),
AnchorPoseBLocal = new Pose(pelvis.Pose.ToLocalPosition(pelvisJointPosition)),
Minimum = -0.5f,
Maximum = 1.1f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(pelvisJoint);
Vector3F neckJointPosition = new Vector3F(0f, 0.026f * scale, -0.690f * scale) + ragdollPosition;
HingeJoint neckJoint = new HingeJoint
{
BodyA = head,
BodyB = torso,
AnchorPoseALocal = new Pose(head.Pose.ToLocalPosition(neckJointPosition)),
AnchorPoseBLocal = new Pose(torso.Pose.ToLocalPosition(neckJointPosition)),
Minimum = -1f,
Maximum = 1f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(neckJoint);
Vector3F leftShoulderJointPosition = new Vector3F(-0.193f * scale, 0.056f * scale, -0.528f * scale) + ragdollPosition;
Vector3F leftShoulderJointAxis = new Vector3F(0, -1, -1).Normalized;
Matrix33F leftShoulderJointOrientation = new Matrix33F();
leftShoulderJointOrientation.SetColumn(0, leftShoulderJointAxis);
leftShoulderJointOrientation.SetColumn(1, leftShoulderJointAxis.Orthonormal1);
leftShoulderJointOrientation.SetColumn(2, leftShoulderJointAxis.Orthonormal2);
BallJoint leftShoulderJoint = new BallJoint
{
BodyA = leftUpperArm,
BodyB = torso,
AnchorPositionALocal = leftUpperArm.Pose.ToLocalPosition(leftShoulderJointPosition),
AnchorPositionBLocal = torso.Pose.ToLocalPosition(leftShoulderJointPosition),
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(leftShoulderJoint);
Vector3F rightShoulderJointPosition = new Vector3F(0.193f * scale, 0.056f * scale, -0.528f * scale) + ragdollPosition;
Vector3F rightShoulderJointAxis = new Vector3F(0, 1, 1).Normalized;
Matrix33F rightShoulderJointOrientation = new Matrix33F();
rightShoulderJointOrientation.SetColumn(0, rightShoulderJointAxis);
rightShoulderJointOrientation.SetColumn(1, rightShoulderJointAxis.Orthonormal1);
rightShoulderJointOrientation.SetColumn(2, rightShoulderJointAxis.Orthonormal2);
BallJoint rightShoulderJoint = new BallJoint
{
BodyA = rightUpperArm,
BodyB = torso,
AnchorPositionALocal = rightUpperArm.Pose.ToLocalPosition(rightShoulderJointPosition),
AnchorPositionBLocal = torso.Pose.ToLocalPosition(rightShoulderJointPosition),
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(rightShoulderJoint);
Vector3F leftElbowJointPosition = new Vector3F(-0.451f * scale, 0.071f * scale, -0.538f * scale) + ragdollPosition;
Matrix33F elbowAxisOrientation = new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0);
HingeJoint leftElbowJoint = new HingeJoint
{
BodyA = leftLowerArm,
BodyB = leftUpperArm,
AnchorPoseALocal = new Pose(leftLowerArm.Pose.ToLocalPosition(leftElbowJointPosition), leftLowerArm.Pose.Orientation.Inverse * elbowAxisOrientation),
AnchorPoseBLocal = new Pose(leftUpperArm.Pose.ToLocalPosition(leftElbowJointPosition), leftUpperArm.Pose.Orientation.Inverse * elbowAxisOrientation),
Minimum = -2,
Maximum = 0,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(leftElbowJoint);
Vector3F rightElbowJointPosition = new Vector3F(0.451f * scale, 0.071f * scale, -0.538f * scale) + ragdollPosition;
HingeJoint rightElbowJoint = new HingeJoint
{
BodyA = rightLowerArm,
BodyB = rightUpperArm,
AnchorPoseALocal = new Pose(rightLowerArm.Pose.ToLocalPosition(rightElbowJointPosition), rightLowerArm.Pose.Orientation.Inverse * elbowAxisOrientation),
AnchorPoseBLocal = new Pose(rightUpperArm.Pose.ToLocalPosition(rightElbowJointPosition), rightUpperArm.Pose.Orientation.Inverse * elbowAxisOrientation),
Minimum = 0,
Maximum = 2,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(rightElbowJoint);
Vector3F leftHipJointPosition = new Vector3F(-0.107f * scale, 0.049f * scale, 0.026f * scale) + ragdollPosition;
HingeJoint leftHipJoint = new HingeJoint
{
BodyA = pelvis,
BodyB = leftUpperLeg,
AnchorPoseALocal = new Pose(pelvis.Pose.ToLocalPosition(leftHipJointPosition)),
AnchorPoseBLocal = new Pose(leftUpperLeg.Pose.ToLocalPosition(leftHipJointPosition), leftUpperLeg.Pose.Orientation.Inverse),
Minimum = -0.1f,
Maximum = 1.2f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(leftHipJoint);
Vector3F rightHipJointPosition = new Vector3F(0.107f * scale, 0.049f * scale, 0.026f * scale) + ragdollPosition;
HingeJoint rightHipJoint = new HingeJoint
{
BodyA = pelvis,
BodyB = rightUpperLeg,
AnchorPoseALocal = new Pose(pelvis.Pose.ToLocalPosition(rightHipJointPosition)),
AnchorPoseBLocal = new Pose(rightUpperLeg.Pose.ToLocalPosition(rightHipJointPosition), rightUpperLeg.Pose.Orientation.Inverse),
Minimum = -0.1f,
Maximum = 1.2f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(rightHipJoint);
Vector3F leftKneeJointPosition = new Vector3F(-0.118f * scale, -0.012f * scale, 0.439f * scale) + ragdollPosition;
HingeJoint leftKneeJoint = new HingeJoint
{
BodyA = leftLowerLeg,
BodyB = leftUpperLeg,
AnchorPoseALocal = new Pose(leftLowerLeg.Pose.ToLocalPosition(leftKneeJointPosition)),
AnchorPoseBLocal = new Pose(leftUpperLeg.Pose.ToLocalPosition(leftKneeJointPosition)),
Minimum = 0,
Maximum = 1.7f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(leftKneeJoint);
Vector3F rightKneeJointPosition = new Vector3F(0.118f * scale, -0.012f * scale, 0.439f * scale) + ragdollPosition;
HingeJoint rightKneeJoint = new HingeJoint
{
BodyA = rightLowerLeg,
BodyB = rightUpperLeg,
AnchorPoseALocal = new Pose(rightLowerLeg.Pose.ToLocalPosition(rightKneeJointPosition)),
AnchorPoseBLocal = new Pose(rightUpperLeg.Pose.ToLocalPosition(rightKneeJointPosition)),
Minimum = 0,
Maximum = 1.7f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(rightKneeJoint);
Vector3F leftAnkleJointPosition = new Vector3F(-0.118f * scale, -0.016f * scale, 0.861f * scale) + ragdollPosition;
HingeJoint leftAnkleJoint = new HingeJoint
{
BodyA = leftFoot,
BodyB = leftLowerLeg,
AnchorPoseALocal = new Pose(leftFoot.Pose.ToLocalPosition(leftAnkleJointPosition)),
AnchorPoseBLocal = new Pose(leftLowerLeg.Pose.ToLocalPosition(leftAnkleJointPosition), leftLowerLeg.Pose.Orientation.Inverse),
Minimum = -0.4f,
Maximum = 0.9f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(leftAnkleJoint);
Vector3F rightAnkleJointPosition = new Vector3F(0.118f * scale, -0.016f * scale, 0.861f * scale) + ragdollPosition;
HingeJoint rightAnkleJoint = new HingeJoint
{
BodyA = rightFoot,
BodyB = rightLowerLeg,
AnchorPoseALocal = new Pose(rightFoot.Pose.ToLocalPosition(rightAnkleJointPosition)),
AnchorPoseBLocal = new Pose(rightLowerLeg.Pose.ToLocalPosition(rightAnkleJointPosition), rightLowerLeg.Pose.Orientation.Inverse),
Minimum = -0.4f,
Maximum = 0.9f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(rightAnkleJoint);
#endregion
#region ----- Add damping to improve stability -----
if (addDamping)
{
// Damping removes jiggling and improves stability.
softness = 0.05f;
maxForce = float.PositiveInfinity;
AddDamping(simulation, pelvis, torso, softness, maxForce);
AddDamping(simulation, torso, head, softness, maxForce);
AddDamping(simulation, torso, leftUpperArm, softness, maxForce);
AddDamping(simulation, leftUpperArm, leftLowerArm, softness, maxForce);
AddDamping(simulation, torso, rightUpperArm, softness, maxForce);
AddDamping(simulation, rightUpperArm, rightLowerArm, softness, maxForce);
AddDamping(simulation, pelvis, leftUpperLeg, softness, maxForce);
AddDamping(simulation, pelvis, rightUpperLeg, softness, maxForce);
AddDamping(simulation, leftUpperLeg, leftLowerLeg, softness, maxForce);
AddDamping(simulation, rightUpperLeg, rightLowerLeg, softness, maxForce);
AddDamping(simulation, leftLowerLeg, leftFoot, softness, maxForce);
AddDamping(simulation, rightLowerLeg, rightFoot, softness, maxForce);
}
#endregion
}
public IKPhysicsSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
GraphicsScreen.DrawReticle = true;
// Add game objects which allows to grab rigid bodies.
_grabObject = new GrabObject(Services);
GameObjectService.Objects.Add(_grabObject);
// Add Dude model.
var modelNode = ContentManager.Load<ModelNode>("Dude/Dude");
_meshNode = modelNode.GetSubtree().OfType<MeshNode>().First().Clone();
_meshNode.PoseLocal = new Pose(new Vector3F(0, 0, 0));
SampleHelper.EnablePerPixelLighting(_meshNode);
GraphicsScreen.Scene.Children.Add(_meshNode);
// Create a ragdoll for the Dude model.
_ragdoll = new Ragdoll();
DudeRagdollCreator.Create(_meshNode.SkeletonPose, _ragdoll, Simulation, 0.571f);
// Set the initial world space pose of the whole ragdoll. And copy the bone poses of the
// current skeleton pose.
_ragdoll.Pose = _meshNode.PoseWorld;
_ragdoll.UpdateBodiesFromSkeleton(_meshNode.SkeletonPose);
// Enable constraints (joints and limits, no motors)
_ragdoll.EnableJoints();
_ragdoll.EnableLimits();
_ragdoll.DisableMotors();
foreach (var body in _ragdoll.Bodies)
{
if (body != null)
{
// Disable rigid body sleeping. (If we leave it enabled, the simulation might
// disable slow bodies before they reach their IK goal.)
body.CanSleep = false;
// Disable collisions response.
body.CollisionResponseEnabled = false;
}
}
// Add rigid bodies and the constraints of the ragdoll to the simulation.
_ragdoll.AddToSimulation(Simulation);
// Disable all force effects (default gravity and damping).
Simulation.ForceEffects.Clear();
// Create constraints which hold selected bodies at their current position
// relative to the world.
// To constrain the position + orientation, we use a FixedJoint.
foreach (var boneName in new[] { "Pelvis" })
{
var ragdollBody = _ragdoll.Bodies[_meshNode.SkeletonPose.Skeleton.GetIndex(boneName)];
var ikJoint = new FixedJoint
{
AnchorPoseALocal = ragdollBody.Pose,
BodyA = Simulation.World,
AnchorPoseBLocal = Pose.Identity,
BodyB = ragdollBody,
CollisionEnabled = false,
MaxForce = 1000,
};
_ikJoints.Add(ikJoint);
Simulation.Constraints.Add(ikJoint);
}
// To constrain only the position, we use a BallJoint.
foreach(var boneName in new[] { "L_Hand", "R_Hand", "L_Ankle1", "R_Ankle" })
{
var ragdollBody = _ragdoll.Bodies[_meshNode.SkeletonPose.Skeleton.GetIndex(boneName)];
var ikJoint = new BallJoint
{
AnchorPositionALocal = ragdollBody.Pose.Position,
BodyA = Simulation.World,
AnchorPositionBLocal = Vector3F.Zero,
BodyB = ragdollBody,
CollisionEnabled = false,
MaxForce = 1000,
};
_ikJoints.Add(ikJoint);
Simulation.Constraints.Add(ikJoint);
}
}
// OnUnload() is called when the GameObject is removed from the IGameObjectService.
protected override void OnUnload()
{
_simulation.Constraints.Remove(_spring);
_spring = null;
}
