public RigidBodyConstructionInfo(float mass, MotionState motionState, CollisionShape collisionShape)
 {
     _native = btRigidBody_btRigidBodyConstructionInfo_new(mass, (motionState != null) ? motionState._native : IntPtr.Zero,
         (collisionShape != null) ? collisionShape._native : IntPtr.Zero);
     _collisionShape = collisionShape;
     _motionState = motionState;
 }

		public unsafe static void AddChildShape(this CompoundShape obj, ref OpenTK.Matrix4 localTransform, CollisionShape shape)
		{
			fixed (OpenTK.Matrix4* localTransformPtr = &localTransform)
			{
				obj.AddChildShape(ref *(BulletSharp.Math.Matrix*)localTransformPtr, shape);
			}
		}

 public PhysicalBody(RigidBody rigidBody, CollisionShape shape, TransformationManager manager)
 {
     Body = rigidBody;
     Shape = shape;
     Transformation = manager;
     Enabled = false;
 }

 public override void RemoveShape(CollisionShape shape)
 {
     if (shapes.ContainsKey(shape))
     {
         shapes[shape].Dispose();
         shapes.Remove(shape);
     }
 }

        public BulletPhysicObject(CollisionShape CollisionShape, Vector3 translation, Matrix rotation, Vector3 Scale, float mass, CollisionFilterGroups CollisionFilterGroup = CollisionFilterGroups.DefaultFilter,CollisionFilterGroups collisionFilterMask = CollisionFilterGroups.DefaultFilter)
        {
            shape  = CollisionShape;
            this.scale = Scale;
            this.mass = mass;
            shape.LocalScaling = Scale;
            obj = LocalCreateRigidBody(mass, Matrix.CreateTranslation(translation) * rotation, CollisionShape);            

        }

        Mesh CreateShape(CollisionShape shape)
        {
            uint[] indices;
            BulletSharp.Math.Vector3[] vertices = CreateShape(shape, out indices);

            int vertexCount = vertices.Length / 2;
            int indexCount = (indices != null) ? indices.Length : vertexCount;
            bool index32 = indexCount > 65536;

            Mesh mesh = new Mesh(device, indexCount / 3, vertexCount,
                MeshFlags.SystemMemory | (index32 ? MeshFlags.Use32Bit : 0), VertexFormat.Position | VertexFormat.Normal);

            DataStream vertexBuffer = mesh.LockVertexBuffer(LockFlags.Discard);
            vertexBuffer.WriteRange(vertices);
            mesh.UnlockVertexBuffer();

            DataStream indexBuffer = mesh.LockIndexBuffer(LockFlags.Discard);
            if (index32)
            {
                if (indices == null)
                {
                    indices = new uint[indexCount];
                    uint i = 0;
                    while (i < indexCount)
                    {
                        indices[i] = i;
                        i++;
                    }
                }
                indexBuffer.WriteRange(indices);
            }
            else
            {
                ushort[] indices_s;
                if (indices == null)
                {
                    indices_s = new ushort[indexCount];
                    ushort i = 0;
                    while (i < indexCount)
                    {
                        indices_s[i] = i;
                        i++;
                    }
                }
                else
                {
                    indices_s = CompactIndexBuffer(indices);
                }
                indexBuffer.WriteRange(indices_s);
            }
            mesh.UnlockIndexBuffer();

            shapes.Add(shape, mesh);
            return mesh;
        }

        public override RigidBody CreateRigidBody(bool isDynamic, float mass, Matrix startTransform, CollisionShape shape, string bodyName)
        {
            RigidBody body = base.CreateRigidBody(isDynamic, mass, startTransform, shape, bodyName);

            if (bodyName != null && bodyName.Equals("GroundName"))
                body.UserObject = "Ground";

            if (shape.ShapeType == BroadphaseNativeType.StaticPlaneShape)
                body.UserObject = "Ground";

            return body;
        }

        public Prop(CollisionShape shape, float mass, Matrix4 start)
        {
            Physics physics = Game.Instance.Physics;

            Shape = shape;
            IsDynamic = (mass != 0.0f);
            physics.Props.Add(this);
            Vector3 inertia = Vector3.Zero;
            if (IsDynamic) shape.CalculateLocalInertia(mass, out inertia);
            DefaultMotionState motionState = new DefaultMotionState(start);
            RigidBody.RigidBodyConstructionInfo info = new RigidBody.RigidBodyConstructionInfo(mass, motionState, shape, inertia);
            Body = new RigidBody(info);
            physics.World.AddRigidBody(Body);
        }

 RigidBody CreateBody(float mass, CollisionShape shape, Vector3 offset)
 {
     using (var info = new RigidBodyConstructionInfo(mass, new DefaultMotionState(), shape, Vector3.Zero))
     {
         if (mass != 0.0f)
         {
             info.LocalInertia = info.CollisionShape.CalculateLocalInertia(mass);
         }
         var collisionObject = new RigidBody(info);
         collisionObject.Translate(offset);
         world.AddRigidBody(collisionObject);
         return collisionObject;
     }
 }

        protected RigidBody LocalCreateRigidBody(float mass, Matrix startTransform, CollisionShape shape)
        {
            bool isDynamic = (mass != 0.0f);

            Vector3 localInertia = Vector3.Zero;
            if (isDynamic)
                shape.CalculateLocalInertia(mass, out localInertia);

            DefaultMotionState myMotionState = new DefaultMotionState(startTransform);

            RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);
            body = new RigidBody(rbInfo);
            
            return body;
        }

        static RigidBody CreateBody(float mass, CollisionShape shape, Vector3 offset)
        {
            var constInfo = new RigidBodyConstructionInfo(mass, new DefaultMotionState(), shape, Vector3.Zero);
            if (mass != 0.0f)
            {
                constInfo.LocalInertia = constInfo.CollisionShape.CalculateLocalInertia(mass);
            }
            var collisionObject = new RigidBody(constInfo);
            collisionObject.Translate(offset);
            world.AddRigidBody(collisionObject);

            AddToDisposeQueue(constInfo);
            AddToDisposeQueue(constInfo.MotionState);
            AddToDisposeQueue(collisionObject);
            AddToDisposeQueue(shape);

            return collisionObject;
        }

        public virtual RigidBody LocalCreateRigidBody(float mass, Matrix startTransform, CollisionShape shape)
        {
            //rigidbody is dynamic if and only if mass is non zero, otherwise static
            bool isDynamic = (mass != 0.0f);

            Vector3 localInertia = Vector3.Zero;
            if (isDynamic)
                shape.CalculateLocalInertia(mass, out localInertia);

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

            RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);
            RigidBody body = new RigidBody(rbInfo);

            World.AddRigidBody(body);

            return body;
        }

        public static void CreateCube(CollisionShape cs, Mesh mesh)
        {
            BulletSharp.Math.Vector3 ext = ((BoxShape)cs).HalfExtentsWithMargin;
            float length = ext.X * 2f;
            float width = ext.Y * 2f;
            float height = ext.Z * 2f;

            UnityEngine.Vector3 p0 = new UnityEngine.Vector3(-length * .5f, -width * .5f, height * .5f);
            UnityEngine.Vector3 p1 = new UnityEngine.Vector3(length * .5f, -width * .5f, height * .5f);
            UnityEngine.Vector3 p2 = new UnityEngine.Vector3(length * .5f, -width * .5f, -height * .5f);
            UnityEngine.Vector3 p3 = new UnityEngine.Vector3(-length * .5f, -width * .5f, -height * .5f);

            UnityEngine.Vector3 p4 = new UnityEngine.Vector3(-length * .5f, width * .5f, height * .5f);
            UnityEngine.Vector3 p5 = new UnityEngine.Vector3(length * .5f, width * .5f, height * .5f);
            UnityEngine.Vector3 p6 = new UnityEngine.Vector3(length * .5f, width * .5f, -height * .5f);
            UnityEngine.Vector3 p7 = new UnityEngine.Vector3(-length * .5f, width * .5f, -height * .5f);

            MakeUnityCubeMesh(p0, p1, p2, p3, p4, p5, p6, p7, mesh);
        }

        public override RigidBody LocalCreateRigidBody(float mass, Matrix startTransform, CollisionShape shape)
        {
            //rigidbody is dynamic if and only if mass is non zero, otherwise static
            bool isDynamic = (mass != 0.0f);

            Vector3 localInertia = Vector3.Zero;
            if (isDynamic)
                shape.CalculateLocalInertia(mass, out localInertia);

            //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects

            RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, null, shape, localInertia);
            RigidBody body = new RigidBody(rbInfo);
            rbInfo.Dispose();
            body.ContactProcessingThreshold = defaultContactProcessingThreshold;
            body.WorldTransform = startTransform;

            World.AddRigidBody(body);

            return body;
        }

        void CreateStack(CollisionShape boxShape, int size, float zPos)
        {
            Matrix trans;
            float mass = 1.0f;

            for (int i = 0; i < size; i++)
            {
                // This constructs a row, from left to right
                int rowSize = size - i;
                for (int j = 0; j < rowSize; j++)
                {
                    trans = Matrix.Translation(
                        -rowSize * CubeHalfExtents + CubeHalfExtents + j * 2.0f * CubeHalfExtents,
                        CubeHalfExtents + i * CubeHalfExtents * 2.0f,
                        zPos);

                    RigidBody body = LocalCreateRigidBody(mass, trans, boxShape);
                    body.ActivationState = ActivationState.IslandSleeping;
                }
            }
        }

 public static Vector3[] CreateShape(CollisionShape shape, out uint[] indices)
 {
     switch (shape.ShapeType)
     {
         case BroadphaseNativeType.BoxShape:
             indices = null;
             return CreateBox(shape as BoxShape);
         case BroadphaseNativeType.Box2DShape:
             indices = null;
             return CreateBox2DShape(shape as Box2DShape);
         case BroadphaseNativeType.CapsuleShape:
             return CreateCapsule(shape as CapsuleShape, out indices);
         case BroadphaseNativeType.Convex2DShape:
             return CreateShape((shape as Convex2DShape).ChildShape, out indices);
         case BroadphaseNativeType.ConvexHullShape:
             indices = null;
             return CreateConvexHull(shape as ConvexHullShape);
         case BroadphaseNativeType.ConeShape:
             return CreateCone(shape as ConeShape, out indices);
         case BroadphaseNativeType.CylinderShape:
             return CreateCylinder(shape as CylinderShape, out indices);
         case BroadphaseNativeType.GImpactShape:
             indices = null;
             return CreateTriangleMesh((shape as GImpactMeshShape).MeshInterface);
         case BroadphaseNativeType.MultiSphereShape:
             return CreateMultiSphere(shape as MultiSphereShape, out indices);
         case BroadphaseNativeType.SphereShape:
             return CreateSphere(shape as SphereShape, out indices);
         case BroadphaseNativeType.StaticPlaneShape:
             return CreateStaticPlane(shape as StaticPlaneShape, out indices);
         case BroadphaseNativeType.TriangleMeshShape:
             indices = null;
             return CreateTriangleMesh((shape as TriangleMeshShape).MeshInterface);
     }
     if (shape is PolyhedralConvexShape)
     {
         return CreatePolyhedralConvexShape((shape as PolyhedralConvexShape), out indices);
     }
     throw new NotImplementedException();
 }

        ShapeData CreateShape(CollisionShape shape)
        {
            ShapeData shapeData = new ShapeData();
            uint[] indices;
            Vector3[] vertices = CreateShape(shape, out indices);
            shapeData.VertexCount = vertices.Length / 2;
            shapeData.SetVertexBuffer(device, vertices);

            if (indices != null)
            {
                shapeData.IndexCount = indices.Length;
                ushort[] indices_s = CompactIndexBuffer(indices);
                if (indices_s != null)
                {
                    shapeData.SetIndexBuffer(device, indices_s);
                }
                else
                {
                    shapeData.SetIndexBuffer(device, indices);
                }
            }

            return shapeData;
        }

        void InitSoftBodyInstance(SoftBody softBody, CollisionShape shape)
        {
            var shapeData = InitShapeData(shape);
            shapeData.Instances.Add(new InstanceData()
            {
                WorldTransform = Matrix.Identity,
                Color = softBodyColor
            });

            UpdateSoftBody(softBody, shapeData);
        }

 void InitRigidBodyInstance(CollisionObject colObj, CollisionShape shape, ref Matrix transform)
 {
     if (shape.ShapeType == BroadphaseNativeType.CompoundShape)
     {
         foreach (var child in (shape as CompoundShape).ChildList)
         {
             Matrix childTransform = child.Transform * transform;
             InitRigidBodyInstance(colObj, child.ChildShape, ref childTransform);
         }
     }
     else
     {
         var shapeData = InitShapeData(shape);
         shapeData.Instances.Add(new InstanceData()
         {
             WorldTransform = transform,
             Color = "Ground".Equals(colObj.UserObject) ? groundColor :
                 colObj.ActivationState == ActivationState.ActiveTag ? activeColor : passiveColor
         });
     }
 }

        ShapeData InitShapeData(CollisionShape shape)
        {
            ShapeData shapeData;

            if (shapes.TryGetValue(shape, out shapeData) == false)
            {
                if (shape.ShapeType == BroadphaseNativeType.SoftBodyShape)
                {
                    shapeData = new ShapeData();
                }
                else
                {
                    shapeData = CreateShape(shape);
                }

                // Create an initial instance data buffer for a single instance
                instanceDataDesc.SizeInBytes = InstanceData.SizeInBytes;
                shapeData.InstanceDataBuffer = new Buffer(device, instanceDataDesc);
                shapeData.BufferBindings[1] = new VertexBufferBinding(shapeData.InstanceDataBuffer, instanceDataDesc.SizeInBytes, 0);

                shapes.Add(shape, shapeData);
            }

            return shapeData;
        }