public override void ShootBox(Vector3 camPos, Vector3 destination)
        {
            if (World != null)
            {
                const float mass = 1.0f;

                if (shootBoxShape == null)
                {
                    shootBoxShape = new BoxShape(1.0f);
                    shootBoxShape.InitializePolyhedralFeatures();
                }

                RigidBody body = LocalCreateRigidBody(mass, Matrix.Translation(camPos), shootBoxShape);
                body.LinearFactor = new Vector3(1, 1, 1);
                //body->setRestitution(1);

                Vector3 linVel = destination - camPos;
                linVel.Normalize();
                body.LinearVelocity = linVel * shootBoxInitialSpeed;
                body.AngularVelocity = Vector3.Zero;
                body.ContactProcessingThreshold = 1e30f;

                // when using m_ccdMode, disable regular CCD
                if (ccdMode)
                {
                    body.CcdMotionThreshold = 0.0001f;
                    body.CcdSweptSphereRadius = 0.4f;
                }
            }
        }

 public ConvexcastBatch(float ray_length, float z, float min_y = -1000, float max_y = 10)
 {
     boxShapeHalfExtents = new Vector3(1.0f, 1.0f, 1.0f);
     boxShape = new BoxShape(boxShapeHalfExtents);
     frame_counter = 0;
     ms = 0;
     max_ms = 0;
     min_ms = 9999;
     sum_ms_samples = 0;
     sum_ms = 0;
     dx = 10.0f;
     min_x = -40;
     max_x = 20;
     this.min_y = min_y;
     this.max_y = max_y;
     sign = 1.0f;
     const float dalpha = 4 * (float)Math.PI / NUMRAYS_IN_BAR;
     for (int i = 0; i < NUMRAYS_IN_BAR; i++)
     {
         float alpha = dalpha * i;
         // rotate around by alpha degrees y
         Matrix tr = Matrix.RotationQuaternion(Quaternion.RotationAxis(new Vector3(0.0f, 1.0f, 0.0f), alpha));
         direction[i] = new Vector3(1.0f, 0.0f, 0.0f);
         direction[i] = Vector3.TransformCoordinate(direction[i], tr);
         source[i] = new Vector3(min_x, max_y, z);
         dest[i] = source[i] + direction[i] * ray_length;
         dest[i][1] = min_y;
         normal[i] = new Vector3(1.0f, 0.0f, 0.0f);
     }
 }

 Mesh CreateBoxShape(BoxShape shape)
 {
     Vector3 size = shape.HalfExtentsWithMargin;
     Mesh mesh = Mesh.CreateBox(device, size.X * 2, size.Y * 2, size.Z * 2);
     shapes.Add(shape, mesh);
     return mesh;
 }

 public override CollisionShape GetCollisionShape()
 {
     if (collisionShapePtr == null) {
         collisionShapePtr = new BoxShape(extents.ToBullet());
     }
     return collisionShapePtr;
 }

        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);

            CollisionShapes = new List<CollisionShape>();

            // 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.CreateTranslation(
                            new Vector3(
                                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);
                    }
                }
            }
        }

        public static Vector3[] CreateBox(BoxShape shape)
        {
            Vector3 size = shape.HalfExtentsWithMargin;
            Vector3[] vertices = new Vector3[36 * 2];
            Vector3 normal;
            int v = 0;

            for (int j = 0; j < 3; j++)
            {
                for (int i = 1; i != -3; i -= 2)
                {
                    normal = GetVectorByAxis(0, i, 0, j);
                    vertices[v++] = GetVectorByAxis(i, i, i, j) * size;
                    vertices[v++] = normal;
                    vertices[v++] = GetVectorByAxis(1, i, -1, j) * size;
                    vertices[v++] = normal;
                    vertices[v++] = GetVectorByAxis(-1, i, 1, j) * size;
                    vertices[v++] = normal;
                    vertices[v++] = GetVectorByAxis(-i, i, -i, j) * size;
                    vertices[v++] = normal;
                    vertices[v++] = GetVectorByAxis(-1, i, 1, j) * size;
                    vertices[v++] = normal;
                    vertices[v++] = GetVectorByAxis(1, i, -1, j) * size;
                    vertices[v++] = normal;
                }
            }

            return vertices;
        }

        /// <summary>
        /// Constructs a new plane primitive, with the specified size.
        /// </summary>
        public PlanePrimitive(Renderer Renderer, float width, float length, int tessellationU, int tessellationV)
        {
            this.Renderer = Renderer;
            Vector3 normal = new Vector3(0, 1, 0);
            CollisionShape = new BoxShape(new Vector3(width / 2, 0.001f, length / 2));
            for (int v = 0; v < tessellationV+1; v++)
            {
                for (int u = 0; u < tessellationU+1; u++)
                {
                    GeometryData.AddVertex(new Vector3((((float)u / (float)tessellationU) * width) - (width / 2), 0, (((float)v / (float)tessellationV) * length) - (length / 2)), normal, new Vector2(((float)width / (float)tessellationU) * u, ((float)length / (float)tessellationV) * v));
                }
            }
            for (int v = 0; v < tessellationV - 0; v++)
            {
                for (int u = 0; u < tessellationU - 0; u++)
                {
                    GeometryData.AddIndex(u + 0 + ((tessellationU + 1) * v));
                    GeometryData.AddIndex(u + 1 + ((tessellationU + 1) * v));
                    GeometryData.AddIndex(u + 0 + tessellationU + 1 + ((tessellationU + 1) * v));

                    GeometryData.AddIndex(u + 0 + tessellationU + 1 + ((tessellationU + 1) * v));
                    GeometryData.AddIndex(u + 1 + ((tessellationU + 1) * v));
                    GeometryData.AddIndex(u + 1 + tessellationU + 1 + ((tessellationU + 1) * v));
                }
            }
            InitializePrimitive();
        }

        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);

            // 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(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1)
            };
            Vector3[] points1 = {
                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;
        }

 /// <summary>
 /// Constructs a new box primitive, with the specified dimensions.
 /// </summary>
 public BoxPrimitive(Renderer Renderer, Vector3 dimensions)
 {
     this.Renderer = Renderer;
     this.Dimensions = dimensions;
     CollisionShape = new BoxShape(new Vector3(dimensions.X / 2, dimensions.Y / 2, dimensions.Z / 2));
     GeometryData = GenerateGeometry();
 }

        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));
        }

        /*
        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);
        }

        protected override void OnInitializePhysics()
        {
            BoxShape boxA = new BoxShape(new Vector3(1, 1, 1));
            boxA.Margin = 0;

            BoxShape boxB = new BoxShape(new Vector3(0.5f, 0.5f, 0.5f));
            boxB.Margin = 0;

            objects[0] = new CollisionObject();
            objects[1] = new CollisionObject();

            objects[0].CollisionShape = boxA;
            objects[1].CollisionShape = boxB;

            // 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);
            IsDebugDrawEnabled = true;

            //World.AddCollisionObject(objects[0]);
            World.AddCollisionObject(objects[1]);

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

            objects[0].WorldTransform = Matrix.RotationQuaternion(rotA) * Matrix.Translation(0, 3, 0);
            objects[1].WorldTransform = Matrix.Translation(0, 4.248f, 0);
        }

        private void AddVertexGraph(VertexGraph graph)
        {
            Individual = graph.Vertices.Select(vertex =>
            {
                var mass = 10000;
                var motionState = new DefaultMotionState();
                var collisionShape = new BoxShape(1);

                var info = new RigidBodyConstructionInfo(mass, motionState, collisionShape);
                var rigidBody = new VertexBoundRigidBody(vertex, info);

                return rigidBody;
            }).ToList();

            foreach (var body in Individual)
            {
                // Select the 3 nearest vertices, excluding this one
                var nearest = Individual.OrderBy(a => a.Binding.DistanceTo(body.Binding))
                                         .Where(a => a != body)
                                         .Take(3);
                foreach (var other in nearest)
                {
                    // TODO: What are these matrices supposed to be?
                    var frameInA = body.MotionState.WorldTransform;
                    var frameInB = other.MotionState.WorldTransform;

                    // TODO: How do you specify the spring's springiness?
                    var constraint = new Generic6DofSpringConstraint(body, other, frameInA, frameInB, true);

                    // TODO: Now how do I apply this to the bodies?
                    body.AddConstraintRef(constraint);
                    other.AddConstraintRef(constraint);
                }
            }
        }

        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);
        }

 public override CollisionShape GetCollisionShape()
 {
     if (collisionShapePtr == null) {
         collisionShapePtr = new BoxShape(extents.ToBullet());
         ((BoxShape)collisionShapePtr).LocalScaling = m_localScaling.ToBullet();
     }
     return collisionShapePtr;
 }

        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);
        }

 static void TestAlignment()
 {
     const float mass = 1.0f;
     Vector3WriteTest vTest = new Vector3WriteTest();
     vTest.Value1 = 2.0f;
     vTest.Value2 = 3.0f;
     using (BoxShape shape = new BoxShape(1))
     {
         shape.CalculateLocalInertia(mass, out vTest.Vector);
     }
     if (vTest.Value1 != 2.0f || vTest.Value2 != 3.0f)
     {
         Console.WriteLine("Vector3 value was overwritten with padding!");
     }
 }

        public static TriggerRegion CreateBoxTriggerRegion(string name, TriggerReportEvent trh, Vector3 dimensions, Vector3 position, Quaternion orientation)
        {
            var csm = LKernel.GetG<CollisionShapeManager>();
            CollisionShape shape;

            if (!csm.TryGetShape(name, out shape)) {
                shape = new BoxShape(dimensions);
                csm.RegisterShape(name, shape);
            }

            var tr = new TriggerRegion(name, position, orientation, shape);
            tr.OnTrigger += trh;
            AddToDispose(tr, trh);
            return tr;
        }

        /*
        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";
        }

        public Physics()
        {
            CollisionConfiguration collisionConf = new DefaultCollisionConfiguration();
            CollisionDispatcher dispatcher = new CollisionDispatcher(collisionConf);

            World = new DiscreteDynamicsWorld(dispatcher, new DbvtBroadphase(), null, collisionConf);
            World.Gravity = new Vector3(0, -10, 0);

            // create the ground
            CollisionShape groundShape = new BoxShape(50, 1, 50);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
            ground.UserObject = "Ground";

            // create a box
            CollisionShape boxShape = new BoxShape(1);
            LocalCreateRigidBody(1.0f, Matrix.Translation(0, 20, 0), boxShape);
        }