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