internal DigitalRuneHingeJoint(HingeJointDescriptor descriptor) { WrappedHingeJoint = new HingeJoint(); #region set RigidBodies if (!(descriptor.RigidBodyA is RigidBody)) throw new ArgumentException(String.Format("The type of the property 'RigidBodyA' must be '{0}'.", typeof(RigidBody))); WrappedHingeJoint.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'."); WrappedHingeJoint.BodyB = ((RigidBody)descriptor.RigidBodyB).WrappedRigidBody; _rigidBodyB = descriptor.RigidBodyB; #endregion WrappedHingeJoint.AnchorPoseALocal = descriptor.AnchorPoseALocal.ToDigitalRune(); WrappedHingeJoint.AnchorPoseBLocal = descriptor.AnchorPoseBLocal.ToDigitalRune(); WrappedHingeJoint.Maximum = descriptor.MaximumAngle; WrappedHingeJoint.Minimum = descriptor.MinimumAngle; 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); }
// 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 ConstraintCarSample(Microsoft.Xna.Framework.Game game) : base(game) { // Add basic force effects. Simulation.ForceEffects.Add(new Gravity()); Simulation.ForceEffects.Add(new Damping()); // Create a material with high friction - this will be used for the ground and the wheels // to give the wheels some traction. UniformMaterial roughMaterial = new UniformMaterial { DynamicFriction = 1, StaticFriction = 1, }; // Add a ground plane. RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0), null, roughMaterial) { MotionType = MotionType.Static, }; Simulation.RigidBodies.Add(groundPlane); // Now, we build a car out of one box for the chassis and 4 cylindric wheels. // Front wheels are fixed with Hinge2Joints and motorized with AngularVelocityMotors. // Back wheels are fixed with HingeJoints and not motorized. - This creates a sloppy // car configuration. Please note that cars for racing games are not normally built with // simple constraints - nevertheless, it is funny to do and play with it. // Check out the "Vehicle Sample" (not included in this project)! The "Vehicle Sample" // provides a robust ray-car implementation.) // ----- Chassis BoxShape chassisShape = new BoxShape(1.7f, 1, 4f); MassFrame chassisMass = MassFrame.FromShapeAndDensity(chassisShape, Vector3F.One, 200, 0.01f, 3); // Here is a trick: The car topples over very easily. By lowering the center of mass we // make it more stable. chassisMass.Pose = new Pose(new Vector3F(0, -1, 0)); RigidBody chassis = new RigidBody(chassisShape, chassisMass, null) { Pose = new Pose(new Vector3F(0, 1, 0)), }; Simulation.RigidBodies.Add(chassis); // ------ Wheels CylinderShape cylinderShape = new CylinderShape(0.4f, 0.3f); MassFrame wheelMass = MassFrame.FromShapeAndDensity(cylinderShape, Vector3F.One, 500, 0.01f, 3); RigidBody wheelFrontLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial) { Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)), }; Simulation.RigidBodies.Add(wheelFrontLeft); RigidBody wheelFrontRight = new RigidBody(cylinderShape, wheelMass, roughMaterial) { Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)), }; Simulation.RigidBodies.Add(wheelFrontRight); RigidBody wheelBackLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial) { Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)), }; Simulation.RigidBodies.Add(wheelBackLeft); RigidBody wheelBackRight = new RigidBody(cylinderShape, wheelMass, roughMaterial) { Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)), }; Simulation.RigidBodies.Add(wheelBackRight); // ----- Hinge2Joints for the front wheels. // A Hinge2Joint allows a limited rotation on the first constraint axis - the steering // axis. The second constraint axis is locked and the third constraint axis is the // rotation axis of the wheels. _frontLeftHinge = new Hinge2Joint { BodyA = chassis, // --> To define the constraint anchor orientation for the chassis: // The columns are the axes. We set the local y axis in the first column. This is // steering axis. In the last column we set the -x axis. This is the wheel rotation axis. // In the middle column is a vector that is normal to the first and last axis. // (All three columns are orthonormal and form a valid rotation matrix.) AnchorPoseALocal = new Pose(new Vector3F(-0.9f, -0.4f, -1.4f), new Matrix33F(0, 0, -1, 1, 0, 0, 0, -1, 0)), BodyB = wheelFrontLeft, // --> To define the constraint anchor orientation for the chassis: // The columns are the axes. We set the local x axis in the first column. This is // steering axis. In the last column we set the y axis. This is the wheel rotation axis. // (In local space of a cylinder the cylinder axis is the +y axis.) // In the middle column is a vector that is normal to the first and last axis. // (All three columns are orthonormal and form a valid rotation matrix.) AnchorPoseBLocal = new Pose(new Matrix33F(1, 0, 0, 0, 0, 1, 0, -1, 0)), CollisionEnabled = false, Minimum = new Vector2F(-0.7f, float.NegativeInfinity), Maximum = new Vector2F(0.7f, float.PositiveInfinity), }; Simulation.Constraints.Add(_frontLeftHinge); _frontRightHinge = new Hinge2Joint { BodyA = chassis, BodyB = wheelFrontRight, AnchorPoseALocal = new Pose(new Vector3F(0.9f, -0.4f, -1.4f), new Matrix33F(0, 0, -1, 1, 0, 0, 0, -1, 0)), AnchorPoseBLocal = new Pose(new Matrix33F(1, 0, 0, 0, 0, 1, 0, -1, 0)), CollisionEnabled = false, Minimum = new Vector2F(-0.7f, float.NegativeInfinity), Maximum = new Vector2F(0.7f, float.PositiveInfinity), }; Simulation.Constraints.Add(_frontRightHinge); // ----- HingeJoints for the back wheels. // Hinges allow free rotation on the first constraint axis. HingeJoint backLeftHinge = new HingeJoint { BodyA = chassis, AnchorPoseALocal = new Pose(new Vector3F(-0.9f, -0.4f, 1.4f)), BodyB = wheelBackLeft, // --> To define the constraint anchor orientation: // The columns are the axes. We set the local y axis in the first column. This is // cylinder axis and should be the hinge axis. In the other two columns we set two // orthonormal vectors. // (All three columns are orthonormal and form a valid rotation matrix.) AnchorPoseBLocal = new Pose(new Matrix33F(0, 0, 1, 1, 0, 0, 0, 1, 0)), CollisionEnabled = false, }; Simulation.Constraints.Add(backLeftHinge); HingeJoint backRightHinge = new HingeJoint { BodyA = chassis, AnchorPoseALocal = new Pose(new Vector3F(0.9f, -0.4f, 1.4f)), BodyB = wheelBackRight, AnchorPoseBLocal = new Pose(new Matrix33F(0, 0, 1, 1, 0, 0, 0, 1, 0)), CollisionEnabled = false, }; Simulation.Constraints.Add(backRightHinge); // ----- Motors for the front wheels. // (Motor axes and target velocities are set in Update() below.) _frontLeftMotor = new AngularVelocityMotor { BodyA = chassis, BodyB = wheelFrontLeft, CollisionEnabled = false, // We use "single axis mode", which means the motor drives only on axis and does not // block motion orthogonal to this axis. - Rotation about the orthogonal axes is already // controlled by the Hinge2Joint. UseSingleAxisMode = true, // The motor has only limited power: MaxForce = 50000, }; Simulation.Constraints.Add(_frontLeftMotor); _frontRightMotor = new AngularVelocityMotor { BodyA = chassis, BodyB = wheelFrontRight, CollisionEnabled = false, UseSingleAxisMode = true, MaxForce = 50000, }; Simulation.Constraints.Add(_frontRightMotor); // ----- Drop a few boxes to create obstacles. BoxShape boxShape = new BoxShape(1, 1, 1); MassFrame boxMass = MassFrame.FromShapeAndDensity(boxShape, Vector3F.One, 100, 0.01f, 3); for (int i = 0; i < 20; i++) { Vector3F position = RandomHelper.Random.NextVector3F(-20, 20); position.Y = 5; QuaternionF orientation = RandomHelper.Random.NextQuaternionF(); RigidBody body = new RigidBody(boxShape, boxMass, null) { Pose = new Pose(position, orientation), }; Simulation.RigidBodies.Add(body); } }
public BridgeSample(Microsoft.Xna.Framework.Game game) : base(game) { // Add basic force effects. Simulation.ForceEffects.Add(new Gravity()); Simulation.ForceEffects.Add(new Damping()); // Add a ground plane. RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0)) { Name = "GroundPlane", // Names are not required but helpful for debugging. MotionType = MotionType.Static, }; Simulation.RigidBodies.Add(groundPlane); // We add another damping effect that acts only on the suspension bridge parts. // This damping uses higher damping factors than the standard damping. It makes the // bridge movement smoother and more stable. // We use a ListAreaOfEffect. So the additional damping acts only on bodies in this list. ListAreaOfEffect boardList = new ListAreaOfEffect(new List<RigidBody>()); Damping damping = new Damping { AreaOfEffect = boardList, AngularDamping = 1f, LinearDamping = 0.5f }; Simulation.ForceEffects.Add(damping); const int numberOfBoards = 20; BoxShape boardShape = new BoxShape(0.8f, 0.1f, 1.5f); RigidBody lastBoard = null; for (int i = 0; i < numberOfBoards; i++) { // A single plank of the bridge. RigidBody body = new RigidBody(boardShape) { Pose = new Pose(new Vector3F(-10 + boardShape.WidthX * i, 4, 0)) }; Simulation.RigidBodies.Add(body); // Add the body to the list of the additional damping force effect. boardList.RigidBodies.Add(body); if (lastBoard != null) { // Connect the last body with current body using a hinge. HingeJoint hinge = new HingeJoint { BodyA = lastBoard, // The attachment point is at the right side of the board. // --> To define the constraint anchor orientation: // The columns are the axes. We set the local z axis in the first column. This is // the hinge axis. In the other two columns we set two orthonormal vectors. // (All three columns are orthonormal and form a valid rotation matrix.) AnchorPoseALocal = new Pose(new Vector3F(boardShape.WidthX / 2, 0, 0), new Matrix33F(0, 0, -1, 0, 1, 0, 1, 0, 0)), BodyB = body, // The attachment point is at the left side of the board. // The anchor orientation is defined as above. AnchorPoseBLocal = new Pose(new Vector3F(-boardShape.WidthX / 2, 0, 0), new Matrix33F(0, 0, -1, 0, 1, 0, 1, 0, 0)), CollisionEnabled = false, // ErrorReduction and Softness are tweaked to get a stable and smooth bridge // movement. ErrorReduction = 0.3f, Softness = 0.00005f, }; Simulation.Constraints.Add(hinge); } else if (i == 0) { // To attach the bridge somewhere, connect the the first board to a fixed position in the // world. HingeJoint hinge = new HingeJoint { BodyA = Simulation.World, AnchorPoseALocal = new Pose(new Vector3F(-9, 3, 0), new Matrix33F(0, 0, -1, 0, 1, 0, 1, 0, 0)), BodyB = body, AnchorPoseBLocal = new Pose(new Vector3F(-boardShape.WidthX / 2, 0, 0), new Matrix33F(0, 0, -1, 0, 1, 0, 1, 0, 0)), }; Simulation.Constraints.Add(hinge); } if (i == numberOfBoards - 1) { // To attach the bridge somewhere, connect the the last board to a fixed position in the // world. HingeJoint hinge = new HingeJoint { BodyA = Simulation.World, AnchorPoseALocal = new Pose(new Vector3F(9, 3, 0), new Matrix33F(0, 0, -1, 0, 1, 0, 1, 0, 0)), BodyB = body, AnchorPoseBLocal = new Pose(new Vector3F(boardShape.WidthX / 2, 0, 0), new Matrix33F(0, 0, -1, 0, 1, 0, 1, 0, 0)), }; Simulation.Constraints.Add(hinge); } lastBoard = body; } // The bridge is ready. // Now, add some ramps so that the character controller can walk up to the bridge. BoxShape rampShape = new BoxShape(10, 10, 2); RigidBody ramp0 = new RigidBody(rampShape) { Pose = new Pose(new Vector3F(-12.5f, -3f, 0), Matrix33F.CreateRotationZ(0.3f)), MotionType = MotionType.Static, }; Simulation.RigidBodies.Add(ramp0); RigidBody ramp1 = new RigidBody(rampShape) { Pose = new Pose(new Vector3F(12.5f, -3f, 0), Matrix33F.CreateRotationZ(-0.3f)), MotionType = MotionType.Static, }; Simulation.RigidBodies.Add(ramp1); // Drop a few light boxes onto the bridge. BoxShape boxShape = new BoxShape(1, 1, 1); MassFrame boxMass = MassFrame.FromShapeAndDensity(boxShape, Vector3F.One, 100, 0.01f, 3); for (int i = 0; i < 10; i++) { Vector3F randomPosition = new Vector3F(RandomHelper.Random.NextFloat(-10, 10), 5, 0); QuaternionF randomOrientation = RandomHelper.Random.NextQuaternionF(); RigidBody body = new RigidBody(boxShape, boxMass, null) { Pose = new Pose(randomPosition, randomOrientation), }; Simulation.RigidBodies.Add(body); } }