Beispiel #1
0
        public void CapsuleTest()
        {
            var s = new CapsuleShape(1, 2);
              var v0 = s.GetVolume(0.001f, 10);

              var m = s.GetMesh(0.001f, 10);
              var v1 = m.GetVolume();

              Assert.IsTrue(Numeric.AreEqual(v0, v1, 0.01f * (1 + v0)));  // 1% error is allowed.
        }
Beispiel #2
0
    public LockRotationSample(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);

      // Next, we add boxes and capsules in random positions and orientations.
      // For all bodies the flags LockRotationX/Y/Z are set. This will prevent all
      // rotation that would be caused by forces. (It is still allowed to manually 
      // change the rotation or to set an angular velocity.)

      BoxShape boxShape = new BoxShape(0.5f, 0.8f, 1.2f);
      for (int i = 0; i < 10; i++)
      {
        Vector3F position = RandomHelper.Random.NextVector3F(-10, 10);
        position.Y = 5;
        QuaternionF orientation = RandomHelper.Random.NextQuaternionF();

        RigidBody body = new RigidBody(boxShape)
        {
          Pose = new Pose(position, orientation),
          LockRotationX = true,
          LockRotationY = true,
          LockRotationZ = true,
        };
        Simulation.RigidBodies.Add(body);
      }

      CapsuleShape capsuleShape = new CapsuleShape(0.3f, 1.2f);
      for (int i = 0; i < 10; i++)
      {
        Vector3F randomPosition = RandomHelper.Random.NextVector3F(-10, 10);
        randomPosition.Y = 5;
        QuaternionF randomOrientation = RandomHelper.Random.NextQuaternionF();

        RigidBody body = new RigidBody(capsuleShape)
        {
          Pose = new Pose(randomPosition, randomOrientation),
          LockRotationX = true,
          LockRotationY = true,
          LockRotationZ = true,
        };
        Simulation.RigidBodies.Add(body);
      }
    }
Beispiel #3
0
    public ResponseFilterSample(Microsoft.Xna.Framework.Game game)
      : base(game)
    {
      // Add basic force effects.
      Simulation.ForceEffects.Add(new Gravity());
      Simulation.ForceEffects.Add(new Damping());

      // Only disable collision response if you need collision detection info but no collision
      // response. If you can disable collision detection too, use 
      //   Simulation.CollisionDomain.CollisionDetection.CollisionFilter 
      // instead - this is more efficient!
      // (In this sample, a custom filter implementation is used. DigitalRune.Physics provides
      // a standard filter implementation: DigitalRune.Physics.CollisionResponseFilter.)
      Simulation.ResponseFilter = new MyCollisionResponseFilter();

      // 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 boxes at random poses.
      BoxShape boxShape = new BoxShape(0.5f, 0.8f, 1.2f);
      for (int i = 0; i < 20; i++)
      {
        Vector3F position = RandomHelper.Random.NextVector3F(-3, 3);
        position.Y = 5;
        QuaternionF orientation = RandomHelper.Random.NextQuaternionF();

        RigidBody body = new RigidBody(boxShape)
        {
          Pose = new Pose(position, orientation),
        };
        Simulation.RigidBodies.Add(body);
      }

      // ----- Add capsules at random poses.
      CapsuleShape capsuleShape = new CapsuleShape(0.3f, 1.2f);
      for (int i = 0; i < 20; i++)
      {
        Vector3F position = RandomHelper.Random.NextVector3F(-3, 3);
        position.Y = 5;
        QuaternionF orientation = RandomHelper.Random.NextQuaternionF();

        RigidBody body = new RigidBody(capsuleShape)
        {
          Pose = new Pose(position, orientation),
        };
        Simulation.RigidBodies.Add(body);
      }
    }
Beispiel #4
0
        /// <summary>
        /// Computes a minimum bounding shape that contains all given points.
        /// </summary>
        /// <param name="points">The points.</param>
        /// <returns>A minimum bounding shape that contains all given points.</returns>
        /// <remarks>
        /// The returned shape will be a <see cref="SphereShape"/>, a <see cref="CapsuleShape"/>,
        /// a <see cref="BoxShape"/>, or a <see cref="TransformedShape"/> (containing a sphere, capsule,
        /// or a box). The bounding shape is not guaranteed to be optimal, it is only guaranteed that
        /// the bounding shape includes all given points.
        /// </remarks>
        /// <exception cref="ArgumentNullException">
        /// <paramref name="points"/> is <see langword="null"/>.
        /// </exception>
        /// <exception cref="ArgumentException">
        /// <paramref name="points"/> is empty.
        /// </exception>
        public static Shape CreateBoundingShape(IList<Vector3F> points)
        {
            if (points == null)
            throw new ArgumentNullException("points");
              if (points.Count == 0)
            throw new ArgumentException("The list of 'points' is empty.");

              // Compute minimal sphere.
              Vector3F center;
              float radius;
              ComputeBoundingSphere(points, out radius, out center);
              SphereShape sphere = new SphereShape(radius);
              float sphereVolume = sphere.GetVolume();

              // Compute minimal capsule.
              float height;
              Pose capsulePose;
              ComputeBoundingCapsule(points, out radius, out height, out capsulePose);
              CapsuleShape capsule = new CapsuleShape(radius, height);
              float capsuleVolume = capsule.GetVolume();

              // Compute minimal box.
              Vector3F boxExtent;
              Pose boxPose;
              ComputeBoundingBox(points, out boxExtent, out boxPose);
              var box = new BoxShape(boxExtent);
              float boxVolume = box.GetVolume();

              // Return the object with the smallest volume.
              // A TransformedShape is used if the shape needs to be translated or rotated.
              if (sphereVolume < boxVolume && sphereVolume < capsuleVolume)
              {
            if (center.IsNumericallyZero)
              return sphere;

            return new TransformedShape(new GeometricObject(sphere, new Pose(center)));
              }
              else if (capsuleVolume < boxVolume)
              {
            if (!capsulePose.HasTranslation && !capsulePose.HasRotation)
              return capsule;

            return new TransformedShape(new GeometricObject(capsule, capsulePose));
              }
              else
              {
            if (!boxPose.HasTranslation && !boxPose.HasRotation)
              return box;

            return new TransformedShape(new GeometricObject(box, boxPose));
              }
        }
Beispiel #5
0
    public ShapesSample(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 sphere.
      Shape sphere = new SphereShape(0.5f);
      Simulation.RigidBodies.Add(new RigidBody(sphere));

      // ----- Add a box.
      BoxShape box = new BoxShape(0.5f, 0.9f, 0.7f);
      Simulation.RigidBodies.Add(new RigidBody(box));

      // ----- Add a capsule.
      CapsuleShape capsule = new CapsuleShape(0.4f, 1.2f);
      Simulation.RigidBodies.Add(new RigidBody(capsule));

      // ----- Add a cone.
      ConeShape cone = new ConeShape(0.5f, 1f);
      Simulation.RigidBodies.Add(new RigidBody(cone));

      // ----- Add a cylinder.
      CylinderShape cylinder = new CylinderShape(0.3f, 1f);
      Simulation.RigidBodies.Add(new RigidBody(cylinder));

      // ----- Add a convex hull of random points.
      ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints();
      for (int i = 0; i < 20; i++)
        convexHullOfPoints.Points.Add(RandomHelper.Random.NextVector3F(-0.5f, 0.5f));
      Simulation.RigidBodies.Add(new RigidBody(convexHullOfPoints));

      // ----- Add a convex polyhedron. 
      // (A ConvexPolyhedron is similar to the ConvexHullOfPoints. The difference is that 
      // the points in a ConvexHullOfPoints can be changed at runtime. A ConvexPolyhedron 
      // cannot be changed at runtime, but it is faster.)
      List<Vector3F> points = new List<Vector3F>();
      for (int i = 0; i < 20; i++)
        points.Add(RandomHelper.Random.NextVector3F(-0.7f, 0.7f));
      ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(points);
      Simulation.RigidBodies.Add(new RigidBody(convexPolyhedron));

      // ----- Add a composite shape (a table that consists of 5 boxes).
      CompositeShape composite = new CompositeShape();
      composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, -0.5f))));
      composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, -0.5f))));
      composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, 0.5f))));
      composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, 0.5f))));
      composite.Children.Add(new GeometricObject(new BoxShape(1.8f, 0.1f, 1.1f), new Pose(new Vector3F(0, 0.8f, 0))));
      Simulation.RigidBodies.Add(new RigidBody(composite));

      // ----- Add a convex hull of multiple shapes.
      ConvexHullOfShapes convexHullOfShapes = new ConvexHullOfShapes();
      convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(-0.4f, 0, 0))));
      convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(+0.4f, 0, 0))));
      Simulation.RigidBodies.Add(new RigidBody(convexHullOfShapes));

      // ----- Add the Minkowski sum of two shapes. 
      // (The Minkowski sum is a mathematical operation that combines two shapes. 
      // Here a circle is combined with a sphere. The result is a wheel.)
      MinkowskiSumShape minkowskiSum = new MinkowskiSumShape();
      minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.2f), Pose.Identity);
      minkowskiSum.ObjectB = new GeometricObject(new CircleShape(0.5f), Pose.Identity);
      Simulation.RigidBodies.Add(new RigidBody(minkowskiSum));

      // Create another Minkowski sum. (Here a small sphere is added to a box to create a 
      // box with rounded corners.)
      minkowskiSum = new MinkowskiSumShape();
      minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.1f), Pose.Identity);
      minkowskiSum.ObjectB = new GeometricObject(new BoxShape(0.2f, 0.5f, 0.8f), Pose.Identity);
      Simulation.RigidBodies.Add(new RigidBody(minkowskiSum));

      // ----- Add a triangle mesh. 
      // A triangle mesh could be loaded from a file or built from an XNA model. 
      // Here we first create a composite shape and convert the shape into a triangle 
      // mesh. (Any Shape in DigitalRune.Geometry can be converted to a triangle mesh.)
      CompositeShape dumbbell = new CompositeShape();
      dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(0.6f, 0.0f, 0.0f))));
      dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(-0.6f, 0.0f, 0.0f))));
      dumbbell.Children.Add(new GeometricObject(new CylinderShape(0.1f, 0.6f), new Pose(Matrix33F.CreateRotationZ(ConstantsF.PiOver2))));

      TriangleMeshShape triangleMeshShape = new TriangleMeshShape(dumbbell.GetMesh(0.01f, 2));

      // Optional: We can enable "contact welding". When this flag is enabled, the triangle shape
      // precomputes additional internal information for the mesh. The collision detection will 
      // be able to compute better contacts (e.g. better normal vectors at triangle edges).
      // Pro: Collision detection can compute better contact information.
      // Con: Contact welding information needs a bit of memory. And the collision detection is
      // a bit slower.
      triangleMeshShape.EnableContactWelding = true;

      // Optional: Assign a spatial partitioning scheme to the triangle mesh. (A spatial partition
      // adds an additional memory overhead, but it improves collision detection speed tremendously!)
      triangleMeshShape.Partition = new AabbTree<int>()
      {
        // The tree is automatically built using a mixed top-down/bottom-up approach. Bottom-up
        // building is slower but produces better trees. If the tree building takes too long,
        // we can lower the BottomUpBuildThreshold (default is 128).
        BottomUpBuildThreshold = 0,
      };

      Simulation.RigidBodies.Add(new RigidBody(triangleMeshShape));

      // ----- Set random positions/orientations.
      // (Start with the second object. The first object is the ground plane which should
      // not be changed.)
      for (int i = 1; i < Simulation.RigidBodies.Count; i++)
      {
        RigidBody body = Simulation.RigidBodies[i];

        Vector3F position = RandomHelper.Random.NextVector3F(-3, 3);
        position.Y = 3;   // Position the objects 3m above ground.
        QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
        body.Pose = new Pose(position, orientation);
      }
    }
Beispiel #6
0
    // Creates a lot of random objects.
    private void CreateRandomObjects()
    {
      var random = new Random();

      var isFirstHeightField = true;

      int currentShape = 0;
      int numberOfObjects = 0;
      while (true)
      {
        numberOfObjects++;
        if (numberOfObjects > ObjectsPerType)
        {
          currentShape++;
          numberOfObjects = 0;
        }

        Shape shape;
        switch (currentShape)
        {
          case 0:
            // Box
            shape = new BoxShape(ObjectSize, ObjectSize * 2, ObjectSize * 3);
            break;
          case 1:
            // Capsule
            shape = new CapsuleShape(0.3f * ObjectSize, 2 * ObjectSize);
            break;
          case 2:
            // Cone
            shape = new ConeShape(1 * ObjectSize, 2 * ObjectSize);
            break;
          case 3:
            // Cylinder
            shape = new CylinderShape(0.4f * ObjectSize, 2 * ObjectSize);
            break;
          case 4:
            // Sphere
            shape = new SphereShape(ObjectSize);
            break;
          case 5:
            // Convex hull of several points.
            ConvexHullOfPoints hull = new ConvexHullOfPoints();
            hull.Points.Add(new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize));
            hull.Points.Add(new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize));
            hull.Points.Add(new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
            hull.Points.Add(new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
            hull.Points.Add(new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize));
            shape = hull;
            break;
          case 6:
            // A composite shape: two boxes that form a "T" shape.
            var composite = new CompositeShape();
            composite.Children.Add(
              new GeometricObject(
                new BoxShape(ObjectSize, 3 * ObjectSize, ObjectSize),
                new Pose(new Vector3F(0, 0, 0))));
            composite.Children.Add(
              new GeometricObject(
                new BoxShape(2 * ObjectSize, ObjectSize, ObjectSize),
                new Pose(new Vector3F(0, 2 * ObjectSize, 0))));
            shape = composite;
            break;
          case 7:
            shape = new CircleShape(ObjectSize);
            break;
          case 8:
            {
              var compBvh = new CompositeShape();
              compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), Matrix33F.Identity)));
              compBvh.Children.Add(new GeometricObject(new BoxShape(0.8f, 0.5f, 0.5f), new Pose(new Vector3F(0.5f, 0.7f, 0), Matrix33F.CreateRotationZ(-MathHelper.ToRadians(15)))));
              compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), Matrix33F.Identity)));
              compBvh.Children.Add(new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), Matrix33F.CreateRotationX(0.3f))));
              compBvh.Partition = new AabbTree<int>();
              shape = compBvh;
              break;
            }
          case 9:
            CompositeShape comp = new CompositeShape();
            comp.Children.Add(new GeometricObject(new BoxShape(0.5f * ObjectSize, 1 * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3F(0, 0.5f * ObjectSize, 0), QuaternionF.Identity)));
            comp.Children.Add(new GeometricObject(new BoxShape(0.8f * ObjectSize, 0.5f * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3F(0.3f * ObjectSize, 0.7f * ObjectSize, 0), QuaternionF.CreateRotationZ(-MathHelper.ToRadians(45)))));
            comp.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3F(0, 1.15f * ObjectSize, 0), QuaternionF.Identity)));
            shape = comp;
            break;
          case 10:
            shape = new ConvexHullOfPoints(new[]
            {
              new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
              new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
              new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
              new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
              new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
            });
            break;
          case 11:
            ConvexHullOfShapes shapeHull = new ConvexHullOfShapes();
            shapeHull.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3F(0, 2 * ObjectSize, 0), Matrix33F.Identity)));
            shapeHull.Children.Add(new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize), Pose.Identity));
            shape = shapeHull;
            break;
          case 12:
            shape = Shape.Empty;
            break;
          case 13:
            var numberOfSamplesX = 10;
            var numberOfSamplesZ = 10;
            var samples = new float[numberOfSamplesX * numberOfSamplesZ];
            for (int z = 0; z < numberOfSamplesZ; z++)
              for (int x = 0; x < numberOfSamplesX; x++)
                samples[z * numberOfSamplesX + x] = (float)(Math.Cos(z / 3f) * Math.Sin(x / 2f) * BoxSize / 6);
            HeightField heightField = new HeightField(0, 0, 2 * BoxSize, 2 * BoxSize, samples, numberOfSamplesX, numberOfSamplesZ);
            shape = heightField;
            break;
          //case 14:
          //shape = new LineShape(new Vector3F(0.1f, 0.2f, 0.3f), new Vector3F(0.1f, 0.2f, -0.3f).Normalized);
          //break;            
          case 15:
            shape = new LineSegmentShape(
              new Vector3F(0.1f, 0.2f, 0.3f), new Vector3F(0.1f, 0.2f, 0.3f) + 3 * ObjectSize * new Vector3F(0.1f, 0.2f, -0.3f));
            break;
          case 16:
            shape = new MinkowskiDifferenceShape
            {
              ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
              ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize))
            };
            break;
          case 17:
            shape = new MinkowskiSumShape
            {
              ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
              ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize)),
            };
            break;
          case 18:
            shape = new OrthographicViewVolume(0, ObjectSize, 0, ObjectSize, ObjectSize / 2, ObjectSize * 2);
            break;
          case 19:
            shape = new PerspectiveViewVolume(MathHelper.ToRadians(60f), 16f / 10, ObjectSize / 2, ObjectSize * 3);
            break;
          case 20:
            shape = new PointShape(0.1f, 0.3f, 0.2f);
            break;
          case 21:
            shape = new RayShape(new Vector3F(0.2f, 0, -0.12f), new Vector3F(1, 2, 3).Normalized, ObjectSize * 2);
            break;
          case 22:
            shape = new RayShape(new Vector3F(0.2f, 0, -0.12f), new Vector3F(1, 2, 3).Normalized, ObjectSize * 2)
            {
              StopsAtFirstHit = true
            };
            break;
          case 23:
            shape = new RectangleShape(ObjectSize, ObjectSize * 2);
            break;
          case 24:
            shape = new TransformedShape(
              new GeometricObject(
                new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize),
                new Pose(new Vector3F(0.1f, 1, -0.2f))));
            break;
          case 25:
            shape = new TriangleShape(
              new Vector3F(ObjectSize, 0, 0), new Vector3F(0, ObjectSize, 0), new Vector3F(ObjectSize, ObjectSize, ObjectSize));
            break;
          //case 26:
          //  {
          //    // Create a composite object from which we get the mesh.
          //    CompositeShape compBvh = new CompositeShape();
          //    compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), Matrix33F.Identity)));
          //    compBvh.Children.Add(
          //      new GeometricObject(
          //        new BoxShape(0.8f, 0.5f, 0.5f),
          //        new Pose(new Vector3F(0.5f, 0.7f, 0), Matrix33F.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
          //    compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), Matrix33F.Identity)));
          //    compBvh.Children.Add(
          //      new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), Matrix33F.CreateRotationX(0.3f))));

          //    TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
          //    meshBvhShape.Partition = new AabbTree<int>();
          //    shape = meshBvhShape;
          //    break;
          //  }
          //case 27:
          //  {
          //    // Create a composite object from which we get the mesh.
          //    CompositeShape compBvh = new CompositeShape();
          //    compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), QuaternionF.Identity)));
          //    compBvh.Children.Add(
          //      new GeometricObject(
          //        new BoxShape(0.8f, 0.5f, 0.5f),
          //        new Pose(new Vector3F(0.5f, 0.7f, 0), QuaternionF.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
          //    compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), QuaternionF.Identity)));
          //    compBvh.Children.Add(
          //      new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), QuaternionF.CreateRotationX(0.3f))));

          //    TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
          //    meshBvhShape.Partition = new AabbTree<int>();
          //    shape = meshBvhShape;
          //    break;
          //  }
          case 28:
            shape = new ConvexPolyhedron(new[]
            {
              new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
              new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
              new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
              new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
              new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
            });
            break;
          case 29:
            return;
          default:
            currentShape++;
            continue;
        }

        // Create an object with the random shape, pose, color and velocity.
        Pose randomPose = new Pose(
          random.NextVector3F(-BoxSize + ObjectSize * 2, BoxSize - ObjectSize * 2),
          random.NextQuaternionF());
        var newObject = new MovingGeometricObject
        {
          Pose = randomPose,
          Shape = shape,
          LinearVelocity = random.NextQuaternionF().Rotate(new Vector3F(MaxLinearVelocity, 0, 0)),
          AngularVelocity = random.NextQuaternionF().Rotate(Vector3F.Forward)
                            * RandomHelper.Random.NextFloat(0, MaxAngularVelocity),
        };

        if (RandomHelper.Random.NextBool())
          newObject.LinearVelocity = Vector3F.Zero;
        if (RandomHelper.Random.NextBool())
          newObject.AngularVelocity = Vector3F.Zero;

        if (shape is LineShape || shape is HeightField)
        {
          // Do not move lines or the height field.
          newObject.LinearVelocity = Vector3F.Zero;
          newObject.AngularVelocity = Vector3F.Zero;
        }

        // Create only 1 heightField!
        if (shape is HeightField)
        {
          if (isFirstHeightField)
          {
            isFirstHeightField = true;
            newObject.Pose = new Pose(new Vector3F(-BoxSize, -BoxSize, -BoxSize));
          }
          else
          {
            currentShape++;
            numberOfObjects = 0;
            continue;
          }
        }

        // Add collision object to collision domain.
        _domain.CollisionObjects.Add(new CollisionObject(newObject));

        //co.Type = CollisionObjectType.Trigger;
        //co.Name = "Object" + shape.GetType().Name + "_" + i;
      }
    }
        private static void GetMass(CapsuleShape capsule, Vector3F scale, float densityOrMass, bool isDensity, out float mass, out Matrix33F inertia)
        {
            scale = Vector3F.Absolute(scale);
              Vector3F radius = capsule.Radius * scale;
              Vector3F radius2 = radius * radius;
              float height = capsule.Height * scale.Y - 2 * radius.Y; // Height of cylinder part.
              float height2 = height * height;
              mass = (isDensity) ? ConstantsF.Pi * radius.X * radius.Z * (4.0f / 3.0f * radius.Y + height) * densityOrMass : densityOrMass;

              inertia = Matrix33F.Zero;
              float denom = 4 * radius.Y + 3 * height;
              inertia.M00 = 1.0f / denom * mass * (2 * radius.Y * (2.0f / 5.0f * (radius2.Y + radius2.Z) + 3.0f / 4.0f * height * radius.Y + 1.0f / 2.0f * height2) + 3.0f * height * (1.0f / 4.0f * radius2.Z + 1.0f / 12.0f * height2));
              inertia.M11 = 1.0f / denom * mass * (2 * radius.Y * 2.0f / 5.0f * (radius2.X + radius2.Z) + 3.0f * height * 1.0f / 4.0f * (radius2.X + radius2.Z));
              inertia.M22 = 1.0f / denom * mass * (2 * radius.Y * (2.0f / 5.0f * (radius2.X + radius2.Y) + 3.0f / 4.0f * height * radius.Y + 1.0f / 2.0f * height2) + 3.0f * height * (1.0f / 4.0f * radius2.X + 1.0f / 12.0f * height2));
        }
Beispiel #8
0
        public void CapsuleMass()
        {
            var s = new CapsuleShape(1, 3);
              float m0;
              Vector3F com0;
              Matrix33F i0;
              MassHelper.GetMass(s, new Vector3F(1, -2, -3), 1, true, 0.001f, 10, out m0, out com0, out i0);

              var m = s.GetMesh(0.001f, 10);
              m.Transform(Matrix44F.CreateScale(1, -2, -3));
              float m1;
              Vector3F com1;
              Matrix33F i1;
              MassHelper.GetMass(m, 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)));

              // Try other density.
              float m2;
              Vector3F com2;
              Matrix33F i2;
              MassHelper.GetMass(s, new Vector3F(1, -2, -3), 0.7f, true, 0.001f, 10, out m2, out com2, out i2);
              Assert.IsTrue(Numeric.AreEqual(m0 * 0.7f, m2, e * (1 + m0)));
              Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com2, e * (1 + com0.Length)));
              Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0 * 0.7f, i2, e * (1 + i0.Trace)));

              // Try with target mass.
              float m3;
              Vector3F com3;
              Matrix33F i3;
              MassHelper.GetMass(s, new Vector3F(1, -2, -3), 23, false, 0.001f, 10, out m3, out com3, out i3);
              Assert.IsTrue(Numeric.AreEqual(23, m3, e * (1 + m0)));
              Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com3, e * (1 + com0.Length)));
              Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0 * 23 / m0, i3, e * (1 + i0.Trace)));
        }
Beispiel #9
0
    // Creates a lot of random objects.
    private void CreateRandomObjects()
    {
      var random = new Random(12345);

      for (int i = 0; i < NumberOfObjects; i++)
      {
        // Randomly choose a shape.
        Shape randomShape;
        switch (random.Next(0, 7))
        {
          case 0:
            // Box
            randomShape = new BoxShape(ObjectSize, ObjectSize * 2, ObjectSize * 3);
            break;
          case 1:
            // Capsule
            randomShape = new CapsuleShape(0.3f * ObjectSize, 2 * ObjectSize);
            break;
          case 2:
            // Cone
            randomShape = new ConeShape(1 * ObjectSize, 2 * ObjectSize);
            break;
          case 3:
            // Cylinder
            randomShape = new CylinderShape(0.4f * ObjectSize, 2 * ObjectSize);
            break;
          case 4:
            // Sphere
            randomShape = new SphereShape(ObjectSize);
            break;
          case 5:
            // Convex hull of several points.
            ConvexHullOfPoints hull = new ConvexHullOfPoints();
            hull.Points.Add(new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize));
            hull.Points.Add(new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize));
            hull.Points.Add(new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
            hull.Points.Add(new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
            hull.Points.Add(new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize));
            randomShape = hull;
            break;
          case 6:
            // A composite shape: two boxes that form a "T" shape.
            var composite = new CompositeShape();
            composite.Children.Add(
              new GeometricObject(
                new BoxShape(ObjectSize, 3 * ObjectSize, ObjectSize),
                new Pose(new Vector3F(0, 0, 0))));
            composite.Children.Add(
              new GeometricObject(
                new BoxShape(2 * ObjectSize, ObjectSize, ObjectSize),
                new Pose(new Vector3F(0, 2 * ObjectSize, 0))));
            randomShape = composite;
            break;
          default:
#if WINDOWS
            Trace.Fail("Ups, we shouldn't land here :-(");
#endif
            randomShape = new SphereShape();
            break;
        }

        // Create an object with the random shape, pose, color and velocity.
        Pose randomPose = new Pose(
          random.NextVector3F(-BoxSize + ObjectSize * 2, BoxSize - ObjectSize * 2),
          random.NextQuaternionF());

        var newObject = new MovingGeometricObject
        {
          Pose = randomPose,
          Shape = randomShape,
          LinearVelocity = random.NextQuaternionF().Rotate(new Vector3F(MaxLinearVelocity, 0, 0)),
          AngularVelocity = random.NextQuaternionF().Rotate(Vector3F.Forward)
                            * RandomHelper.Random.NextFloat(0, MaxAngularVelocity),
        };

        // Add collision object to collision domain.
        _domain.CollisionObjects.Add(new CollisionObject(newObject));

        // We will collect a few statistics for debugging.
        Profiler.SetFormat("NumObjects", 1, "The total number of objects.");
        Profiler.SetFormat("NumObjectPairs", 1, "The number of objects pairs, which have to be checked.");
        Profiler.SetFormat("BroadPhasePairs", 1, "The number of overlaps reported by the broad phase.");
        Profiler.SetFormat("ContactSetCount", 1, "The number of actual collisions.");
      }
    }
Beispiel #10
0
    private void StandUp()
    {
      // Similar to crouch - only we make the character capsule taller again. 
      // Before we change the height of the capsule we need to check if there is enough 
      // room to stand up. To check this we position a smaller capsule in this area and
      // test for collisions.
      CapsuleShape testCapsule = new CapsuleShape(0.38f, 1.6f);
      GeometricObject testObject = new GeometricObject(testCapsule, new Pose(CharacterController.Position + 1.0f * Vector3F.UnitY));
      CollisionObject testCollisionObject = new CollisionObject(testObject)
      {
        CollisionGroup = 4,                 // Should not collide with the character.
        Type = CollisionObjectType.Trigger, // Use a trigger because we do not need to compute detailed
      };                                    // collision information.

      // Check whether the test capsule touches anything.
      if (!_simulation.CollisionDomain.HasContact(testCollisionObject))
      {
        // No contact, enough room to stand up.
        CharacterController.Height = 1.8f;

        // The drawing data in the UserData (see RigidBodyRenderer) must be invalidated.
        CharacterController.Body.UserData = null;
      }
    }
Beispiel #11
0
    public RopeSample(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);

      const float capsuleHeight = 0.6f;
      CapsuleShape shape = new CapsuleShape(0.15f, capsuleHeight);
      for (int i = 0; i < 20; i++)
      {
        // A segment of the rope:
        RigidBody body = new RigidBody(shape)
        {
          Pose = new Pose(new Vector3F(0, 1 + i * capsuleHeight, 0)),
        };
        Simulation.RigidBodies.Add(body);

        if (i > 0)
        {
          // Connect the last body with the current body using a UniversalJoint that
          // allows rotations on two axes (no twist).
          RigidBody lastBody = Simulation.RigidBodies[i];
          UniversalJoint ballJoint = new UniversalJoint
          {
            BodyA = lastBody,
            // This attachment point is a the top of the first capsule.
            // The universal joint allows rotations around the first and the second axis.
            // The last axis is the twist axis where no rotation is allowed.
            // --> To define the constraint anchor orientation:
            // The columns are the axes. We set the local x axis in the first column and the
            // -z axis in the second column. The third column is y axis about which no rotation
            // is allowed. 
            // (All three columns are orthonormal and form a valid rotation matrix.)
            AnchorPoseALocal = new Pose(new Vector3F(0, capsuleHeight / 2, 0),
                                        new Matrix33F(1, 0, 0,
                                                      0, 0, 1,
                                                      0, -1, 0)),

            BodyB = body,
            // This attachment point is at the bottom of the second capsule.
            // The anchor orientation is defined as above.
            AnchorPoseBLocal = new Pose(new Vector3F(0, -capsuleHeight / 2, 0),
                                        new Matrix33F(1, 0, 0,
                                                      0, 0, 1,
                                                      0, -1, 0)),

            // Disable collision between body A and B.
            CollisionEnabled = false,

            // ErrorReduction and Softness are tweaked to create an appropriate amount
            // of springiness and damping.
            ErrorReduction = 0.1f,
            Softness = 0.0001f,

            // We allow a +/- 60° rotation around the first and the second constraint axis.
            Minimum = -new Vector2F(MathHelper.ToRadians(60), MathHelper.ToRadians(60)),
            Maximum = new Vector2F(MathHelper.ToRadians(60), MathHelper.ToRadians(60))
          };
          Simulation.Constraints.Add(ballJoint);
        }
        else
        {
          // This is the first body. There is no former body to link too. 
          // We set a velocity for this body to give the rope an initial movement.
          body.LinearVelocity = new Vector3F(1, 0, 0);
        }
      }
    }
        //--------------------------------------------------------------
        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();
        }
Beispiel #13
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
    }
        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;
        }