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;
        }
Esempio n. 2
0
    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);
    }
Esempio n. 3
0
    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);
    }
Esempio n. 4
0
    // 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
    }
Esempio n. 5
0
    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);
      }
    }
Esempio n. 6
0
    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);
      }
    }