Ejemplo n.º 1
0
        /*
         * void MyContactCallback(object sender, ContactAddedEventArgs e)
         * {
         *  if (e.CollisionObject0Wrapper.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
         *  {
         *      CompoundShape compound = e.CollisionObject0Wrapper.CollisionObject.CollisionShape as CompoundShape;
         *      CollisionShape childShape = compound.GetChildShape(e.Index0);
         *  }
         *
         *  if (e.CollisionObject1Wrapper.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
         *  {
         *      CompoundShape compound = e.CollisionObject1Wrapper.CollisionObject.CollisionShape as CompoundShape;
         *      CollisionShape childShape = compound.GetChildShape(e.Index1);
         *  }
         *
         *  e.IsContactModified = true;
         * }
         */

        public void SetupEmptyDynamicsWorld()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher    = new CollisionDispatcher(CollisionConf);

            CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;

            convexDecompositionObjectOffset = new Vector3(10, 0, 0);

            Broadphase = new AxisSweep3(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000));
            //Broadphase = new SimpleBroadphase();

            Solver = new SequentialImpulseConstraintSolver();
            World  = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);

            // create the ground
            CollisionShape groundShape = new BoxShape(30, 2, 30);

            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -4.5f, 0), groundShape);

            ground.UserObject = "Ground";

            // create a few dynamic rigidbodies
            float mass = 1.0f;

            CollisionShape colShape = new BoxShape(1);

            CollisionShapes.Add(colShape);
            Vector3 localInertia = colShape.CalculateLocalInertia(mass);
        }
Ejemplo n.º 2
0
        public Physics()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher    = new CollisionDispatcher(CollisionConf);

            Broadphase = new DbvtBroadphase();

            World         = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
            World.Gravity = new Vector3(0, -10, 0);

            // create the ground
            CollisionShape groundShape = new BoxShape(50, 1, 50);

            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);

            ground.UserObject = "Ground";

            // create a few dynamic rigidbodies
            float mass = 1.0f;

            CollisionShape colShape = new BoxShape(1);

            CollisionShapes.Add(colShape);
            Vector3 localInertia = colShape.CalculateLocalInertia(mass);

            float start_x = StartPosX - ArraySizeX / 2;
            float start_y = StartPosY;
            float start_z = StartPosZ - ArraySizeZ / 2;

            int k, i, j;

            for (k = 0; k < ArraySizeY; k++)
            {
                for (i = 0; i < ArraySizeX; i++)
                {
                    for (j = 0; j < ArraySizeZ; j++)
                    {
                        Matrix startTransform = Matrix.Translation(
                            2 * i + start_x,
                            2 * k + start_y,
                            2 * j + start_z
                            );

                        // using motionstate is recommended, it provides interpolation capabilities
                        // and only synchronizes 'active' objects
                        DefaultMotionState        myMotionState = new DefaultMotionState(startTransform);
                        RigidBodyConstructionInfo rbInfo        =
                            new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
                        RigidBody body = new RigidBody(rbInfo);

                        // make it drop from a height
                        body.Translate(new Vector3(0, 20, 0));

                        World.AddRigidBody(body);
                    }
                }
            }
        }
Ejemplo n.º 3
0
        public Physics()
        {
            CLStuff.InitCL();

            cloths = new Cloth[numFlags];
            for (int flagIndex = 0; flagIndex < numFlags; ++flagIndex)
            {
                cloths[flagIndex] = new Cloth();
                cloths[flagIndex].CreateBuffers(clothWidth, clothHeight);
            }


            gSolver        = new OpenCLSoftBodySolver(CLStuff.commandQueue, CLStuff.cxMainContext);
            softBodyOutput = new SoftBodySolverOutputCLToCpu();

            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new SoftBodyRigidBodyCollisionConfiguration();
            Dispatcher    = new CollisionDispatcher(CollisionConf);

            Broadphase = new DbvtBroadphase();
            Solver     = new SequentialImpulseConstraintSolver();

            World         = new SoftRigidDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf, gSolver);
            World.Gravity = new Vector3(0, -10, 0);

            // create the ground
            CollisionShape groundShape = new BoxShape(50, 50, 50);

            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -60, 0), groundShape);

            ground.UserObject = "Ground";


            SoftWorld.WorldInfo.AirDensity   = 1.2f;
            SoftWorld.WorldInfo.WaterDensity = 0;
            SoftWorld.WorldInfo.WaterOffset  = 0;
            SoftWorld.WorldInfo.WaterNormal  = Vector3.Zero;
            SoftWorld.WorldInfo.Gravity      = new Vector3(0, -10, 0);


            CreateFlag(clothWidth, clothHeight, out flags);

            // Create output buffer descriptions for ecah flag
            // These describe where the simulation should send output data to
            for (int flagIndex = 0; flagIndex < flags.Count; ++flagIndex)
            {
                // flags[flagIndex].WindVelocity = new Vector3(0, 0, 15.0f);

                // In this case we have a DX11 output buffer with a vertex at index 0, 8, 16 and so on as well as a normal at 3, 11, 19 etc.
                // Copies will be performed GPU-side directly into the output buffer

                CpuVertexBufferDescriptor vertexBufferDescriptor = new CpuVertexBufferDescriptor(cloths[flagIndex].CpuBuffer, 0, 8, 3, 8);
                cloths[flagIndex].VertexBufferDescriptor = vertexBufferDescriptor;
            }

            gSolver.Optimize(SoftWorld.SoftBodyArray);

            World.StepSimulation(1.0f / 60.0f, 0);
        }
Ejemplo n.º 4
0
        // Create a door using a hinge constraint attached to the world
        private void CreateDoor()
        {
            const float mass = 1.0f;

            var doorShape = new BoxShape(2.0f, 5.0f, 0.2f);

            CollisionShapes.Add(doorShape);
            RigidBody doorBody = LocalCreateRigidBody(mass, Matrix.Translation(-5.0f, -2.0f, 0.0f), doorShape);

            doorBody.ActivationState = ActivationState.DisableDeactivation;

            var pivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
            var axisA  = Vector3.UnitY;                          // pointing upwards, aka Y-axis

            var hinge = new HingeConstraint(doorBody, pivotA, axisA);

            hinge.DebugDrawSize = 5;

            //hinge.SetLimit(0.0f, (float)Math.PI / 2);
            // test problem values
            //hinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
            //hinge.SetLimit(1, -1);
            //hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
            //hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
            //hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
            hinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
            //hinge.SetLimit(0, 0);
            World.AddConstraint(hinge);
        }
Ejemplo n.º 5
0
        private void CreateBoxes()
        {
            const float mass     = 1.0f;
            var         colShape = new BoxShape(1);

            CollisionShapes.Add(colShape);
            Vector3 localInertia = colShape.CalculateLocalInertia(mass);

            var rbInfo = new RigidBodyConstructionInfo(mass, null, colShape, localInertia);

            for (int y = 0; y < ArraySizeY; y++)
            {
                for (int x = 0; x < ArraySizeX; x++)
                {
                    for (int z = 0; z < ArraySizeZ; z++)
                    {
                        Vector3 position = startPosition + 2 * new Vector3(x, y, z);

                        // make it drop from a height
                        position += new Vector3(0, 10, 0);

                        // using MotionState is recommended, it provides interpolation capabilities
                        // and only synchronizes 'active' objects
                        rbInfo.MotionState = new DefaultMotionState(Matrix.Translation(position));
                        var body = new RigidBody(rbInfo);

                        World.AddRigidBody(body);
                    }
                }
            }

            rbInfo.Dispose();
        }
Ejemplo n.º 6
0
        protected override void OnInitializePhysics()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher    = new CollisionDispatcher(CollisionConf);

            Broadphase = new AxisSweep3(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000));
            Solver     = new SequentialImpulseConstraintSolver();

            World         = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
            World.Gravity = new Vector3(0, -10, 0);

            //World.DispatchInfo.UseConvexConservativeDistanceUtil = true;
            //World.DispatchInfo.ConvexConservativeDistanceThreshold = 0.01f;

            // Setup a big ground box
            CollisionShape groundShape = new BoxShape(100, 10, 100);

            CollisionShapes.Add(groundShape);
            Matrix groundTransform = Matrix.Translation(0, -10, 0);

            RigidBody ground = LocalCreateRigidBody(0, groundTransform, groundShape);

            ground.UserObject = "Ground";

            // Spawn one ragdoll
            SpawnRagdoll(new Vector3(1, 0.5f, 0));
            SpawnRagdoll(new Vector3(-1, 0.5f, 0));
        }
Ejemplo n.º 7
0
        protected override void OnInitializePhysics()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher    = new CollisionDispatcher(CollisionConf);

            Vector3 worldAabbMin = new Vector3(-10000, -10000, -10000);
            Vector3 worldAabbMax = new Vector3(10000, 10000, 10000);

            Broadphase = new AxisSweep3(worldAabbMin, worldAabbMax);

            Solver = new SequentialImpulseConstraintSolver();

            World         = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
            World.Gravity = new Vector3(0, -10, 0);
            World.SetInternalTickCallback(MotorPreTickCallback, this, true);

            // create the ground
            CollisionShape groundShape = new BoxShape(200, 10, 200);

            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -10, 0), groundShape);

            ground.UserObject = "Ground";

            fCyclePeriod    = 2000.0f;
            fMuscleStrength = 0.5f;
            m_Time          = 0;

            SpawnTestRig(new Vector3(1, 0.5f, 0), false);
            SpawnTestRig(new Vector3(-2, 0.5f, 0), true);
        }
Ejemplo n.º 8
0
        protected override void Initialize()
        {
            base.Initialize();

            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher    = new CollisionDispatcher(CollisionConf);

            Broadphase = new DbvtBroadphase();

            World             = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
            World.Gravity     = new Vector3(0, -10, 0);
            World.DebugDrawer = DebugDrawer;

            // create the ground
            _groundShape = new BoxShape(50, 1, 50);
            LoadModel("ground", _groundShape);
            CollisionShapes.Add(_groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, _groundShape);

            // create a few dynamic rigidbodies
            const float mass = 1.0f;

            _boxShape = new BoxShape(1);
            LoadModel("cube", _boxShape);
            CollisionShapes.Add(_boxShape);

            var rbInfo = new RigidBodyConstructionInfo(mass, null, _boxShape);

            rbInfo.LocalInertia = _boxShape.CalculateLocalInertia(mass);

            for (int k = 0; k < ArraySizeY; k++)
            {
                for (int i = 0; i < ArraySizeX; i++)
                {
                    for (int j = 0; j < ArraySizeZ; j++)
                    {
                        Matrix startTransform = Matrix.CreateTranslation(
                            _start + new Vector3(2 * i, 2 * k, 2 * j));

                        // using motionstate is recommended, it provides interpolation capabilities
                        // and only synchronizes 'active' objects
                        rbInfo.MotionState = new DefaultMotionState(startTransform);

                        var body = new RigidBody(rbInfo);

                        // make it drop from a height
                        body.Translate(new Vector3(0, 20, 0));

                        World.AddRigidBody(body);
                    }
                }
            }

            rbInfo.Dispose();

            FreeLook.SetEyeTarget(ref _eye, ref _target);
        }
Ejemplo n.º 9
0
        private void CreateGround()
        {
            var groundShape = new BoxShape(30, 2, 30);

            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -4.5f, 0), groundShape);

            ground.UserObject = "Ground";
        }
Ejemplo n.º 10
0
        private void CreateGround()
        {
            var groundShape = new BoxShape(250, 50, 250);

            CollisionShapes.Add(groundShape);
            var ground = base.LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape);

            ground.UserObject = "Ground";
        }
Ejemplo n.º 11
0
        private void CreateGround()
        {
            var groundShape = new BoxShape(150, 7, 150);

            CollisionShapes.Add(groundShape);
            var ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);

            ground.UserObject = "Ground";
        }
Ejemplo n.º 12
0
        private void CreateGround()
        {
            var groundShape = new BoxShape(50, 1, 50);

            //var groundShape = new StaticPlaneShape(Vector3.UnitY, 1);
            CollisionShapes.Add(groundShape);
            RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);

            body.UserObject = "Ground";
        }
Ejemplo n.º 13
0
        private void CreateGround()
        {
            BoxShape ground = new BoxShape(200, 1, 200);

            ground.InitializePolyhedralFeatures();
            CollisionShapes.Add(ground);
            RigidBody body = LocalCreateRigidBody(0, Matrix.Identity, ground);

            body.Friction = 0.5f;
            //body.RollingFriction = 0.3f;
            body.UserObject = "Ground";
        }
Ejemplo n.º 14
0
        private void CreateGround()
        {
            var groundShape = new BoxShape(50, 1, 50);

            //groundShape.InitializePolyhedralFeatures();
            //var groundShape = new StaticPlaneShape(Vector3.UnitY, 1);

            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);

            ground.UserObject = "Ground";
        }
Ejemplo n.º 15
0
        private void CreateBoxes()
        {
            var colShape = new BoxShape(1);

            CollisionShapes.Add(colShape);

            for (int i = 0; i < 10; i++)
            {
                //CollisionShape colShape = new CapsuleShape(0.5f,2.0f);//boxShape = new SphereShape(1.0f);
                Matrix startTransform = Matrix.Translation(2 * i, 10, 1);
                LocalCreateRigidBody(1.0f, startTransform, colShape);
            }
        }
Ejemplo n.º 16
0
        private void CreateBoxes()
        {
            var colShape = new BoxShape(1);

            //var colShape = new CapsuleShape(0.5f, 2.0f);//boxShape = new SphereShape(1.0f);
            CollisionShapes.Add(colShape);

            for (int j = 0; j < NumDynamicBoxesX; j++)
            {
                for (int i = 0; i < NumDynamicBoxesY; i++)
                {
                    Matrix startTransform = Matrix.Translation(5 * (i - NumDynamicBoxesX / 2), 10, 5 * (j - NumDynamicBoxesY / 2));
                    LocalCreateRigidBody(1.0f, startTransform, colShape);
                }
            }
        }
        private void CreateGround()
        {
            const int totalVerts          = NumVertsX * NumVertsY;
            const int totalTriangles      = 2 * (NumVertsX - 1) * (NumVertsY - 1);
            const int triangleIndexStride = 3 * sizeof(int);
            const int vertexStride        = Vector3.SizeInBytes;

            var mesh = new IndexedMesh();

            mesh.Allocate(totalTriangles, totalVerts, triangleIndexStride, vertexStride);

            var indicesStream = mesh.GetTriangleStream();

            using (var indices = new BinaryWriter(indicesStream))
            {
                for (int x = 0; x < NumVertsX - 1; x++)
                {
                    for (int y = 0; y < NumVertsY - 1; y++)
                    {
                        int row1Index = x * NumVertsX + y;
                        int row2Index = row1Index + NumVertsX;
                        indices.Write(row1Index);
                        indices.Write(row1Index + 1);
                        indices.Write(row2Index + 1);

                        indices.Write(row1Index);
                        indices.Write(row2Index + 1);
                        indices.Write(row2Index);
                    }
                }
            }

            indexVertexArrays = new TriangleIndexVertexArray();
            indexVertexArrays.AddIndexedMesh(mesh);

            SetVertexPositions(waveHeight, 0.0f);

            const bool useQuantizedAabbCompression = true;

            groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);
            CollisionShapes.Add(groundShape);

            staticBody = LocalCreateRigidBody(0.0f, Matrix.Identity, groundShape);
            staticBody.CollisionFlags |= CollisionFlags.StaticObject;
            staticBody.UserObject      = "Ground";
        }
Ejemplo n.º 18
0
        /*
         * void MyContactCallback(object sender, ContactAddedEventArgs e)
         * {
         *  if (e.CollisionObject0Wrapper.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
         *  {
         *      CompoundShape compound = e.CollisionObject0Wrapper.CollisionObject.CollisionShape as CompoundShape;
         *      CollisionShape childShape = compound.GetChildShape(e.Index0);
         *  }
         *
         *  if (e.CollisionObject1Wrapper.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
         *  {
         *      CompoundShape compound = e.CollisionObject1Wrapper.CollisionObject.CollisionShape as CompoundShape;
         *      CollisionShape childShape = compound.GetChildShape(e.Index1);
         *  }
         *
         *  e.IsContactModified = true;
         * }
         */

        public void SetupEmptyDynamicsWorld()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher    = new CollisionDispatcher(CollisionConf);
            Broadphase    = new AxisSweep3(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000));
            Solver        = new SequentialImpulseConstraintSolver();
            World         = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);

            // create the ground
            CollisionShape groundShape = new BoxShape(30, 2, 30);

            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -4.5f, 0), groundShape);

            ground.UserObject = "Ground";
        }
Ejemplo n.º 19
0
        protected override void OnInitializePhysics()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher    = new CollisionDispatcher(CollisionConf);

            Broadphase = new AxisSweep3(
                new Vector3(-1000, -1000, -1000),
                new Vector3(1000, 1000, 1000));

            World         = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
            World.Gravity = new Vector3(0, -10, 0);

            renderCallback = new DrawingResult(World);

            var boxA = new BoxShape(1.0f)
            {
                Margin = 0
            };
            var boxB = new BoxShape(0.5f)
            {
                Margin = 0
            };

            CollisionShapes.Add(boxA);
            CollisionShapes.Add(boxB);

            Quaternion rotA = new Quaternion(0.739f, -0.204f, 0.587f, 0.257f);

            rotA.Normalize();

            objectA = new CollisionObject
            {
                CollisionShape = boxA,
                WorldTransform = Matrix.RotationQuaternion(rotA) * Matrix.Translation(0, 3, 0)
            };
            objectB = new CollisionObject
            {
                CollisionShape = boxB,
                WorldTransform = Matrix.Translation(0, 4.248f, 0)
            };

            //World.AddCollisionObject(objectA);
            World.AddCollisionObject(objectB);
        }
Ejemplo n.º 20
0
        protected override void OnInitializePhysics()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher    = new CollisionDispatcher(CollisionConf);

            Vector3 worldMin = new Vector3(-1000, -1000, -1000);
            Vector3 worldMax = new Vector3(1000, 1000, 1000);

            Broadphase = new AxisSweep3(worldMin, worldMax);
            Solver     = new SequentialImpulseConstraintSolver();

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
            World.SolverInfo.SplitImpulse = 1;
            World.Gravity      = new Vector3(0, -10, 0);
            IsDebugDrawEnabled = true;


            CollisionShape colShape = new BoxShape(1);

            CollisionShapes.Add(colShape);

            for (int j = 0; j < NumDynamicBoxesX; j++)
            {
                for (int i = 0; i < NumDynamicBoxesY; i++)
                {
                    //CollisionShape colShape = new CapsuleShape(0.5f,2.0f);//boxShape = new SphereShape(1.0f);
                    Matrix startTransform = Matrix.Translation(5 * (i - NumDynamicBoxesX / 2), 10, 5 * (j - NumDynamicBoxesY / 2));
                    LocalCreateRigidBody(1.0f, startTransform, colShape);
                }
            }

            SetVertexPositions(WaveHeight, 0.0f);

            const bool useQuantizedAabbCompression = true;

            groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);
            CollisionShapes.Add(groundShape);

            staticBody = LocalCreateRigidBody(0.0f, Matrix.Identity, groundShape);
            staticBody.CollisionFlags |= CollisionFlags.StaticObject;
            staticBody.UserObject      = "Ground";
        }
Ejemplo n.º 21
0
        private RigidBody CreateGear(float radius, Matrix transform)
        {
            const float mass = 6.28f;

            var shape = new CompoundShape();
            var axle  = new CylinderShape(0.2f, 0.25f, 0.2f);
            var wheel = new CylinderShape(radius, 0.025f, radius);

            shape.AddChildShape(Matrix.Identity, axle);
            shape.AddChildShape(Matrix.Identity, wheel);
            CollisionShapes.Add(shape);
            CollisionShapes.Add(axle);
            CollisionShapes.Add(wheel);

            RigidBody body = LocalCreateRigidBody(mass, transform, shape);

            body.LinearFactor = Vector3.Zero;
            return(body);
        }
Ejemplo n.º 22
0
        protected override void OnInitializePhysics()
        {
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher    = new CollisionDispatcher(CollisionConf);

            Broadphase = new DbvtBroadphase();

            World              = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
            World.Gravity      = new Vector3(0, -10, 0);
            IsDebugDrawEnabled = true;
            Vector3 a = Vector3.Zero;

            World.DebugDrawWorld();

            // ground
            CollisionShape groundShape = new BoxShape(50, 1, 50);

            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);

            ground.UserObject = "Ground";

            // Objects
            //colShape = new BoxShape(1);
            Vector3[] points0 = new Vector3[] {
                new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1)
            };
            Vector3[] points1 = new Vector3[] {
                new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1), new Vector3(0, 0, -1), new Vector3(-1, -1, 0)
            };
            colShape0 = new ConvexHullShape(points0);
            colShape1 = new ConvexHullShape(points1);
            CollisionShapes.Add(colShape0);
            CollisionShapes.Add(colShape1);

            body2 = LocalCreateRigidBody(0, body2Position, colShape1);

            rotBody = LocalCreateRigidBody(0, rotBodyPosition, colShape0);
            rotBody.CollisionFlags |= CollisionFlags.KinematicObject;
            rotBody.ActivationState = ActivationState.DisableDeactivation;
        }
Ejemplo n.º 23
0
        public void BeginContact(Contact contact)
        {
            if (!contact.AreTouching)
            {
                return;
            }
            CollisionData temp = new CollisionData();

            temp.BodyA = contact.FixtureA.Body;
            temp.BodyB = contact.FixtureB.Body;

            WorldManifold manifold;

            contact.GetWorldManifold(out manifold);
            for (int i = 0; i < contact.Manifold.PointCount; i++)
            {
                temp.Points.Add(manifold.Points[i].ToAsdVector());
            }

            CollisionShapes.Add(temp);
        }
Ejemplo n.º 24
0
        void AddBoxes()
        {
            // create a few dynamic rigidbodies
            const float mass = 1.0f;

            BoxShape colShape = new BoxShape(1);

            CollisionShapes.Add(colShape);
            Vector3 localInertia = colShape.CalculateLocalInertia(mass);

            const float startX = StartPosX - ArraySizeX / 2;
            const float startY = StartPosY;
            const float startZ = StartPosZ - ArraySizeZ / 2;

            int k, i, j;

            for (k = 0; k < ArraySizeY; k++)
            {
                for (i = 0; i < ArraySizeX; i++)
                {
                    for (j = 0; j < ArraySizeZ; j++)
                    {
                        Matrix startTransform = Matrix.Translation(
                            3 * i + startX,
                            3 * k + startY,
                            3 * j + startZ
                            );

                        // using motionstate is recommended, it provides interpolation capabilities
                        // and only synchronizes 'active' objects
                        DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
                        using (var rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia))
                        {
                            var body = new RigidBody(rbInfo);
                            World.AddRigidBody(body);
                        }
                    }
                }
            }
        }
Ejemplo n.º 25
0
        void InitializeDemo()
        {
            motorControl.Goal      = 0;
            motorControl.MaxTorque = 0;

            CollisionShape groundShape = new BoxShape(50, 50, 50);

            CollisionShapes.Add(groundShape);
            RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -62, 0), groundShape);

            body.UserObject = "Ground";

            softBodyWorldInfo.SparseSdf.Reset();

            softBodyWorldInfo.AirDensity   = 1.2f;
            softBodyWorldInfo.WaterDensity = 0;
            softBodyWorldInfo.WaterOffset  = 0;
            softBodyWorldInfo.WaterNormal  = Vector3.Zero;
            softBodyWorldInfo.Gravity      = new Vector3(0, -10, 0);

            demos[demo]();
        }
Ejemplo n.º 26
0
        private void CreateBoxStack()
        {
            //var shape = new CylinderShape(CubeHalfExtents, CubeHalfExtents, CubeHalfExtents);
            var shape = new BoxShape(CubeHalfExtents);

            CollisionShapes.Add(shape);

            const int numObjects = 120;

            for (int i = 0; i < numObjects; i++)
            {
                //stack them
                const int colsize = 10;
                int       row     = (int)((i * CubeHalfExtents * 2) / (colsize * 2 * CubeHalfExtents));
                int       row2    = row;
                int       col     = (i) % (colsize) - colsize / 2;

                if (col > 3)
                {
                    col   = 11;
                    row2 |= 1;
                }

                Matrix trans = Matrix.Translation(col * 2 * CubeHalfExtents + (row2 % 2) * CubeHalfExtents,
                                                  row * 2 * CubeHalfExtents + CubeHalfExtents + ExtraHeight, 0);

                RigidBody body = LocalCreateRigidBody(1, trans, shape);
                body.SetAnisotropicFriction(shape.AnisotropicRollingFrictionDirection, AnisotropicFrictionFlags.AnisotropicRollingFriction);
                body.Friction = 0.5f;
                //body.RollingFriction = 0.3f;

                if (ccdMode)
                {
                    body.CcdMotionThreshold   = 1e-7f;
                    body.CcdSweptSphereRadius = 0.9f * CubeHalfExtents;
                }
            }
        }
Ejemplo n.º 27
0
        protected override void OnInitializePhysics()
        {
            SetupDynamicsWorld();

            CreateGround();

            cubeShape = new BoxShape(CubeHalfExtent);
            CollisionShapes.Add(cubeShape);

            CreateGears();
            CreateHingedBoxes();
            CreateMotorHinge();
            CreateMotorHinge2();
            CreateSlider();
            CreateD6Slider();
            CreateDoor();
            CreateGeneric6DofConstraint();
            CreateConeTwist();
            CreateUniversalConstraint();
            CreateGeneric6DofSpringConstraint();
            CreateHinge();
            CreateHinge2();
        }
Ejemplo n.º 28
0
        protected override void OnInitializePhysics()
        {
            int i;

            shootBoxInitialSpeed = 4000;

            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();

            Dispatcher = new CollisionDispatcher(CollisionConf);
            //Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.BoxShape, BroadphaseNativeType.BoxShape,
            //    CollisionConf.GetCollisionAlgorithmCreateFunc(BroadphaseNativeType.ConvexShape, BroadphaseNativeType.ConvexShape));

            Broadphase = new DbvtBroadphase();


            // the default constraint solver.
            Solver = new SequentialImpulseConstraintSolver();

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
            World.SolverInfo.SolverMode |= SolverModes.Use2FrictionDirections | SolverModes.RandomizeOrder;
            //World.SolverInfo.SplitImpulse = 0;
            World.SolverInfo.NumIterations = 20;

            World.DispatchInfo.UseContinuous = ccdMode;

            World.Gravity = new Vector3(0, -10, 0);

            BoxShape ground = new BoxShape(200, 1, 200);

            ground.InitializePolyhedralFeatures();
            CollisionShapes.Add(ground);
            RigidBody body = LocalCreateRigidBody(0, Matrix.Identity, ground);

            body.Friction = 0.5f;
            //body.RollingFriction = 0.3f;
            body.UserObject = "Ground";

            //CollisionShape shape = new CylinderShape(CubeHalfExtents, CubeHalfExtents, CubeHalfExtents);
            CollisionShape shape = new BoxShape(CubeHalfExtents, CubeHalfExtents, CubeHalfExtents);

            CollisionShapes.Add(shape);

            const int numObjects = 120;

            for (i = 0; i < numObjects; i++)
            {
                //stack them
                const int colsize = 10;
                int       row     = (int)((i * CubeHalfExtents * 2) / (colsize * 2 * CubeHalfExtents));
                int       row2    = row;
                int       col     = (i) % (colsize) - colsize / 2;

                if (col > 3)
                {
                    col   = 11;
                    row2 |= 1;
                }

                Matrix trans = Matrix.Translation(col * 2 * CubeHalfExtents + (row2 % 2) * CubeHalfExtents,
                                                  row * 2 * CubeHalfExtents + CubeHalfExtents + ExtraHeight, 0);

                body = LocalCreateRigidBody(1, trans, shape);
                body.SetAnisotropicFriction(shape.AnisotropicRollingFrictionDirection, AnisotropicFrictionFlags.AnisotropicRollingFriction);
                body.Friction = 0.5f;
                //body.RollingFriction = 0.3f;

                if (ccdMode)
                {
                    body.CcdMotionThreshold   = 1e-7f;
                    body.CcdSweptSphereRadius = 0.9f * CubeHalfExtents;
                }
            }
        }
Ejemplo n.º 29
0
        private void Create2dBodies()
        {
            // Re-using the same collision is better for memory usage and performance
            float u = 0.96f;

            Vector3[] points      = { new Vector3(0, u, 0), new Vector3(-u, -u, 0), new Vector3(u, -u, 0) };
            var       childShape0 = new BoxShape(1, 1, Depth);
            var       colShape    = new Convex2DShape(childShape0);
            var       childShape1 = new ConvexHullShape(points);
            var       colShape2   = new Convex2DShape(childShape1);
            var       childShape2 = new CylinderShapeZ(1, 1, Depth);
            var       colShape3   = new Convex2DShape(childShape2);

            CollisionShapes.Add(colShape);
            CollisionShapes.Add(colShape2);
            CollisionShapes.Add(colShape3);

            CollisionShapes.Add(childShape0);
            CollisionShapes.Add(childShape1);
            CollisionShapes.Add(childShape2);

            colShape.Margin = 0.03f;

            float   mass         = 1.0f;
            Vector3 localInertia = colShape.CalculateLocalInertia(mass);

            var rbInfo = new RigidBodyConstructionInfo(mass, null, colShape, localInertia);

            Vector3 x      = new Vector3(-ArraySizeX, 8, -20);
            Vector3 y      = Vector3.Zero;
            Vector3 deltaX = new Vector3(1, 2, 0);
            Vector3 deltaY = new Vector3(2, 0, 0);

            for (int i = 0; i < ArraySizeY; i++)
            {
                y = x;
                for (int j = 0; j < ArraySizeX; j++)
                {
                    Matrix startTransform = Matrix.Translation(y - new Vector3(-10, 0, 0));

                    //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
                    rbInfo.MotionState = new DefaultMotionState(startTransform);

                    switch (j % 3)
                    {
                    case 0:
                        rbInfo.CollisionShape = colShape;
                        break;

                    case 1:
                        rbInfo.CollisionShape = colShape3;
                        break;

                    default:
                        rbInfo.CollisionShape = colShape2;
                        break;
                    }
                    var body = new RigidBody(rbInfo)
                    {
                        //ActivationState = ActivationState.IslandSleeping,
                        LinearFactor  = new Vector3(1, 1, 0),
                        AngularFactor = new Vector3(0, 0, 1)
                    };

                    World.AddRigidBody(body);

                    y += deltaY;
                }
                x += deltaX;
            }

            rbInfo.Dispose();
        }
Ejemplo n.º 30
0
        protected override void OnInitializePhysics()
        {
            SetupEmptyDynamicsWorld();

            CollisionShape groundShape = new BoxShape(50, 1, 50);

            //CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
            CollisionShapes.Add(groundShape);
            RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);

            body.UserObject = "Ground";

            CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));

            CollisionShapes.Add(shape);


            const float THETA = (float)Math.PI / 4.0f;
            float       L_1   = 2 - (float)Math.Tan(THETA);
            float       L_2   = 1 / (float)Math.Cos(THETA);
            float       RATIO = L_2 / L_1;

            RigidBody bodyA;
            RigidBody bodyB;

            CollisionShape cylA = new CylinderShape(0.2f, 0.25f, 0.2f);
            CollisionShape cylB = new CylinderShape(L_1, 0.025f, L_1);
            CompoundShape  cyl0 = new CompoundShape();

            cyl0.AddChildShape(Matrix.Identity, cylA);
            cyl0.AddChildShape(Matrix.Identity, cylB);

            float   mass = 6.28f;
            Vector3 localInertia;

            cyl0.CalculateLocalInertia(mass, out localInertia);
            RigidBodyConstructionInfo ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);

            ci.StartWorldTransform = Matrix.Translation(-8, 1, -8);

            body = new RigidBody(ci); //1,0,cyl0,localInertia);
            World.AddRigidBody(body);
            body.LinearFactor  = Vector3.Zero;
            body.AngularFactor = new Vector3(0, 1, 0);
            bodyA = body;

            cylA = new CylinderShape(0.2f, 0.26f, 0.2f);
            cylB = new CylinderShape(L_2, 0.025f, L_2);
            cyl0 = new CompoundShape();
            cyl0.AddChildShape(Matrix.Identity, cylA);
            cyl0.AddChildShape(Matrix.Identity, cylB);

            mass = 6.28f;
            cyl0.CalculateLocalInertia(mass, out localInertia);
            ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
            Quaternion orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);

            ci.StartWorldTransform = Matrix.RotationQuaternion(orn) * Matrix.Translation(-10, 2, -8);

            body = new RigidBody(ci);//1,0,cyl0,localInertia);
            body.LinearFactor = Vector3.Zero;
            HingeConstraint hinge = new HingeConstraint(body, Vector3.Zero, new Vector3(0, 1, 0), true);

            World.AddConstraint(hinge);
            bodyB = body;
            body.AngularVelocity = new Vector3(0, 3, 0);

            World.AddRigidBody(body);

            Vector3 axisA = new Vector3(0, 1, 0);
            Vector3 axisB = new Vector3(0, 1, 0);

            orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
            Matrix mat = Matrix.RotationQuaternion(orn);

            axisB = new Vector3(mat.M21, mat.M22, mat.M23);

            GearConstraint gear = new GearConstraint(bodyA, bodyB, axisA, axisB, RATIO);

            World.AddConstraint(gear, true);


            mass = 1.0f;

            RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);

            RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
            //RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
            //body1.ActivationState = ActivationState.DisableDeactivation;
            //body1.SetDamping(0.3f, 0.3f);

            Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
            Vector3 axisInA  = new Vector3(0, 0, 1);

            Vector3 pivotInB;

            if (body1 != null)
            {
                Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
                pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
            }
            else
            {
                pivotInB = pivotInA;
            }

            Vector3 axisInB;

            if (body1 != null)
            {
                Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
                axisInB = Vector3.TransformCoordinate(axisInA, transform);
            }
            else
            {
                axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
            }

#if P2P
            {
                TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
                //TypedConstraint p2p = new Point2PointConstraint(body0, body1, pivotInA, pivotInB);
                //TypedConstraint hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB);
                World.AddConstraint(p2p);
                p2p.DebugDrawSize = 5;
            }
#else
            {
                hinge = new HingeConstraint(body0, pivotInA, axisInA);

                //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
                //float	targetVelocity = 0.f;
                //float	maxMotorImpulse = 0.01;
                const float targetVelocity  = 1.0f;
                const float maxMotorImpulse = 1.0f;
                hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
                World.AddConstraint(hinge);
                hinge.DebugDrawSize = 5;
            }
#endif

            RigidBody pRbA1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
            //RigidBody pRbA1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
            pRbA1.ActivationState = ActivationState.DisableDeactivation;

            // add dynamic rigid body B1
            RigidBody pRbB1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
            //RigidBody pRbB1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
            pRbB1.ActivationState = ActivationState.DisableDeactivation;

            // create slider constraint between A1 and B1 and add it to world
            SliderConstraint spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, true);
            //spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, false);
            spSlider1.LowerLinearLimit = -15.0f;
            spSlider1.UpperLinearLimit = -5.0f;
            spSlider1.LowerLinearLimit = 5.0f;
            spSlider1.UpperLinearLimit = 15.0f;
            spSlider1.LowerLinearLimit = -10.0f;
            spSlider1.UpperLinearLimit = -10.0f;

            spSlider1.LowerAngularLimit = -(float)Math.PI / 3.0f;
            spSlider1.UpperAngularLimit = (float)Math.PI / 3.0f;

            World.AddConstraint(spSlider1, true);
            spSlider1.DebugDrawSize = 5.0f;


            //create a slider, using the generic D6 constraint
            Vector3     sliderWorldPos = new Vector3(0, 10, 0);
            Vector3     sliderAxis     = Vector3.UnitX;
            const float angle          = 0; //SIMD_RADS_PER_DEG * 10.f;
            Matrix      trans          = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
            d6body0 = LocalCreateRigidBody(mass, trans, shape);
            d6body0.ActivationState = ActivationState.DisableDeactivation;

            RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
            World.AddRigidBody(fixedBody1);

            Matrix frameInA = Matrix.Translation(0, 5, 0);
            Matrix frameInB = Matrix.Translation(0, 5, 0);

            //bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
            const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
            spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA)
            {
                LinearLowerLimit = lowerSliderLimit,
                LinearUpperLimit = hiSliderLimit,

                //range should be small, otherwise singularities will 'explode' the constraint
                //AngularLowerLimit = new Vector3(-1.5f,0,0),
                //AngularUpperLimit = new Vector3(1.5f,0,0),
                //AngularLowerLimit = new Vector3(0,0,0),
                //AngularUpperLimit = new Vector3(0,0,0),
                AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0),
                AngularUpperLimit = new Vector3(1.5f, 0, 0)
            };

            //spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
            spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
            spSlider6Dof.TranslationalLimitMotor.MaxMotorForce  = new Vector3(0.1f, 0, 0);

            World.AddConstraint(spSlider6Dof);
            spSlider6Dof.DebugDrawSize = 5;



            // create a door using hinge constraint attached to the world

            CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
            CollisionShapes.Add(pDoorShape);
            RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
            pDoorBody.ActivationState = ActivationState.DisableDeactivation;
            Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
            Vector3 btAxisA  = Vector3.UnitY;                          // pointing upwards, aka Y-axis

            spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);

            //spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
            // test problem values
            //spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);

            //spDoorHinge.SetLimit(1, -1);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
            spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
            //spDoorHinge.SetLimit(0, 0);
            World.AddConstraint(spDoorHinge);
            spDoorHinge.DebugDrawSize = 5;

            RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);



            // create a generic 6DOF constraint

            //RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
            RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
            //RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
            //RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;

            frameInA = Matrix.Translation(-5, 0, 0);
            frameInB = Matrix.Translation(5, 0, 0);

            Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
            //Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
            pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
            pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
            //pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
            //pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
            //pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
            //pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);

            //pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
            //pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
            //pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);

            //pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);

            pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
            pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);

            //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
            //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);



            // create a ConeTwist constraint

            pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
            //pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
            //pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);

            frameInA  = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
            frameInA *= Matrix.Translation(0, -5, 0);
            frameInB  = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
            frameInB *= Matrix.Translation(0, 5, 0);

            coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
            //coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
            //coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
            coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
            World.AddConstraint(coneTwist, true);
            coneTwist.DebugDrawSize = 5;



            // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)

            RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
            pBody.ActivationState = ActivationState.DisableDeactivation;
            Vector3 pivotA = new Vector3(10.0f, 0.0f, 0.0f);
            btAxisA = new Vector3(0.0f, 0.0f, 1.0f);

            HingeConstraint pHinge = new HingeConstraint(pBody, pivotA, btAxisA);
            //pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
            pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
            World.AddConstraint(pHinge);
            pHinge.DebugDrawSize = 5;



            // create a universal joint using generic 6DOF constraint
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB (child) below it :
            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some (arbitrary) data to build constraint frames
            Vector3 parentAxis = new Vector3(1, 0, 0);
            Vector3 childAxis  = new Vector3(0, 0, 1);
            Vector3 anchor     = new Vector3(20, 2, 0);

            UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
            pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
            pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(pUniv, true);
            // draw constraint frames and limits for debugging
            pUniv.DebugDrawSize = 5;

            World.AddConstraint(pGen6DOF, true);
            pGen6DOF.DebugDrawSize = 5;



            // create a generic 6DOF constraint with springs

            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;

            frameInA = Matrix.Translation(10, 0, 0);
            frameInB = Matrix.Identity;

            Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true)
            {
                LinearUpperLimit  = new Vector3(5, 0, 0),
                LinearLowerLimit  = new Vector3(-5, 0, 0),
                AngularLowerLimit = new Vector3(0, 0, -1.5f),
                AngularUpperLimit = new Vector3(0, 0, 1.5f),
                DebugDrawSize     = 5
            };
            World.AddConstraint(pGen6DOFSpring, true);

            pGen6DOFSpring.EnableSpring(0, true);
            pGen6DOFSpring.SetStiffness(0, 39.478f);
            pGen6DOFSpring.SetDamping(0, 0.5f);
            pGen6DOFSpring.EnableSpring(5, true);
            pGen6DOFSpring.SetStiffness(5, 39.478f);
            pGen6DOFSpring.SetDamping(0, 0.3f);
            pGen6DOFSpring.SetEquilibriumPoint();



            // create a Hinge2 joint
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB (child) below it :
            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some data to build constraint frames
            parentAxis = new Vector3(0, 1, 0);
            childAxis  = new Vector3(1, 0, 0);
            anchor     = new Vector3(-20, 0, 0);
            Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
            pHinge2.SetLowerLimit(-(float)Math.PI / 4);
            pHinge2.SetUpperLimit((float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(pHinge2, true);
            // draw constraint frames and limits for debugging
            pHinge2.DebugDrawSize = 5;



            // create a Hinge joint between two dynamic bodies
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB:
            pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some data to build constraint frames
            axisA = new Vector3(0, 1, 0);
            axisB = new Vector3(0, 1, 0);
            Vector3 pivotA2 = new Vector3(-5, 0, 0);
            Vector3 pivotB  = new Vector3(5, 0, 0);
            spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA2, pivotB, axisA, axisB);
            spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(spHingeDynAB, true);
            // draw constraint frames and limits for debugging
            spHingeDynAB.DebugDrawSize = 5;
        }