public BSConstraintHinge(BulletWorld world, BulletBody obj1, BulletBody obj2,
     Vector3 pivotInA, Vector3 pivotInB,
     Vector3 axisInA, Vector3 axisInB,
     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
     : base(world)
 {
     m_body1 = obj1;
     m_body2 = obj2;
     m_constraint = PhysicsScene.PE.CreateHingeConstraint(world, obj1, obj2,
                         pivotInA, pivotInB, axisInA, axisInB,
                         useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies);
     m_enabled = true;
 }

 // Create a btGeneric6DofConstraint
 public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2,
     Vector3 frame1, Quaternion frame1rot,
     Vector3 frame2, Quaternion frame2rot,
     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
     : base(world)
 {
     m_body1 = obj1;
     m_body2 = obj2;
     m_constraint = PhysicsScene.PE.Create6DofConstraint(m_world, m_body1, m_body2,
                         frame1, frame1rot,
                         frame2, frame2rot,
                         useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies);
     m_enabled = true;
     world.physicsScene.DetailLog("{0},BS6DofConstraint,createFrame,wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
                     BSScene.DetailLogZero, world.worldID,
                     obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
 }

 // 6 Dof constraint based on a midpoint between the two constrained bodies
 public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2,
     Vector3 joinPoint,
     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
     : base(world)
 {
     m_body1 = obj1;
     m_body2 = obj2;
     if (!obj1.HasPhysicalBody || !obj2.HasPhysicalBody)
     {
         world.physicsScene.DetailLog(
             "{0},BS6DOFConstraint,badBodyPtr,wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
             BSScene.DetailLogZero, world.worldID,
             obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
         world.physicsScene.Logger.ErrorFormat(
             "{0} Attempt to build 6DOF constraint with missing bodies: wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
             LogHeader, world.worldID, obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
         m_enabled = false;
     }
     else
     {
         m_constraint = PhysicsScene.PE.Create6DofConstraintToPoint(m_world, m_body1, m_body2,
             joinPoint,
             useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies);
         PhysicsScene.DetailLog(
             "{0},BS6DofConstraint,createMidPoint,wID={1}, csrt={2}, rID={3}, rBody={4}, cID={5}, cBody={6}",
             BSScene.DetailLogZero, world.worldID, m_constraint.AddrString,
             obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
         if (!m_constraint.HasPhysicalConstraint)
         {
             world.physicsScene.Logger.ErrorFormat(
                 "{0} Failed creation of 6Dof constraint. rootID={1}, childID={2}",
                 LogHeader, obj1.ID, obj2.ID);
             m_enabled = false;
         }
         else
         {
             m_enabled = true;
         }
     }
 }

 public override bool UpdateParameter(BulletWorld world, uint localID, String parm, float value)
 {
     /* TODO */
     return false;
 }

 //PhysicsScene.World.ptr, false
 public override BulletShape CreateCompoundShape(BulletWorld pWorld, bool enableDynamicAabbTree)
 {
     return new BulletShapeXNA(new CompoundShape(enableDynamicAabbTree), BSPhysicsShapeType.SHAPE_COMPOUND);
 }

 public override void DestroyObject(BulletWorld pWorld, BulletBody pBody)
 {
     DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
     CollisionObject co = (pBody as BulletBodyXNA).rigidBody;
     RigidBody bo = co as RigidBody;
     if (bo == null)
     {
         if (world.IsInWorld(co))
         {
             world.RemoveCollisionObject(co);
         }
     }
     else
     {
         if (world.IsInWorld(bo))
         {
             world.RemoveRigidBody(bo);
         }
     }
     if (co != null)
     {
         if (co.GetUserPointer() != null)
         {
             uint localId = (uint)co.GetUserPointer();
             if (specialCollisionObjects.ContainsKey(localId))
             {
                 specialCollisionObjects.Remove(localId);
             }
         }
     }
 }

 public override BulletConstraint CreatePoint2PointConstraint(BulletWorld pWorld, BulletBody pBody1,
     BulletBody pBody2,
     Vector3 ppivotInA, Vector3 ppivotInB,
     bool pdisableCollisionsBetweenLinkedBodies)
 {
     Point2PointConstraint constrain = null;
     DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
     RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
     RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
     if (rb1 != null && rb2 != null)
     {
         IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z);
         IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z);
         constrain = new Point2PointConstraint(rb1, rb2, ref pivotInA, ref pivotInB);
         world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
     }
     return new BulletConstraintXNA(constrain);
 }

 public override bool DeleteCollisionShape(BulletWorld pWorld, BulletShape pShape)
 {
     //TODO:
     return false;
 }

        private int PhysicsStepint(BulletWorld pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep,
            out int updatedEntityCount,
            out EntityProperties[] updatedEntities, out int collidersCount, out CollisionDesc[] colliders,
            int maxCollisions, int maxUpdates)
        {
            int numSimSteps = 0;
            Array.Clear(UpdatedObjects, 0, UpdatedObjects.Length);
            Array.Clear(UpdatedCollisions, 0, UpdatedCollisions.Length);
            LastEntityProperty = 0;

            LastCollisionDesc = 0;

            updatedEntityCount = 0;
            collidersCount = 0;

            if (pWorld is BulletWorldXNA)
            {
                DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;

                world.LastCollisionDesc = 0;
                world.LastEntityProperty = 0;
                numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep);

                PersistentManifold contactManifold;
                CollisionObject objA;
                CollisionObject objB;
                ManifoldPoint manifoldPoint;
                PairCachingGhostObject pairCachingGhostObject;

                m_collisionsThisFrame = 0;
                int numManifolds = world.GetDispatcher().GetNumManifolds();
                for (int j = 0; j < numManifolds; j++)
                {
                    contactManifold = world.GetDispatcher().GetManifoldByIndexInternal(j);
                    int numContacts = contactManifold.GetNumContacts();
                    if (numContacts == 0)
                        continue;

                    objA = contactManifold.GetBody0() as CollisionObject;
                    objB = contactManifold.GetBody1() as CollisionObject;

                    manifoldPoint = contactManifold.GetContactPoint(0);
                    //IndexedVector3 contactPoint = manifoldPoint.GetPositionWorldOnB();
                    // IndexedVector3 contactNormal = -manifoldPoint.m_normalWorldOnB; // make relative to A

                    RecordCollision(this, objA, objB, manifoldPoint.GetPositionWorldOnB(),
                        -manifoldPoint.m_normalWorldOnB, manifoldPoint.GetDistance());
                    m_collisionsThisFrame++;
                    if (m_collisionsThisFrame >= 9999999)
                        break;
                }

                foreach (GhostObject ghostObject in specialCollisionObjects.Values)
                {
                    pairCachingGhostObject = ghostObject as PairCachingGhostObject;
                    if (pairCachingGhostObject != null)
                    {
                        RecordGhostCollisions(pairCachingGhostObject);
                    }
                }

                updatedEntityCount = LastEntityProperty;
                updatedEntities = UpdatedObjects;

                collidersCount = LastCollisionDesc;
                colliders = UpdatedCollisions;
            }
            else
            {
                //if (updatedEntities is null)
                //updatedEntities = new List<BulletXNA.EntityProperties>();
                //updatedEntityCount = 0;

                //collidersCount = 0;

                updatedEntities = new EntityProperties[0];

                colliders = new CollisionDesc[0];
            }
            return numSimSteps;
        }

        public override BulletShape CreateHullShape(BulletWorld pWorld, int pHullCount, float[] pConvHulls)
        {
            DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
            CompoundShape compoundshape = new CompoundShape(false);

            compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin);
            int ii = 1;

            for (int i = 0; i < pHullCount; i++)
            {
                int vertexCount = (int)pConvHulls[ii];

                IndexedVector3 centroid = new IndexedVector3(pConvHulls[ii + 1], pConvHulls[ii + 2], pConvHulls[ii + 3]);
                IndexedMatrix childTrans = IndexedMatrix.Identity;
                childTrans._origin = centroid;

                List<IndexedVector3> virts = new List<IndexedVector3>();
                int ender = ((ii + 4) + (vertexCount * 3));
                for (int iii = ii + 4; iii < ender; iii += 3)
                {
                    virts.Add(new IndexedVector3(pConvHulls[iii], pConvHulls[iii + 1], pConvHulls[iii + 2]));
                }
                ConvexHullShape convexShape = new ConvexHullShape(virts, vertexCount);
                convexShape.SetMargin(world.WorldSettings.Params.collisionMargin);
                compoundshape.AddChildShape(ref childTrans, convexShape);
                ii += (vertexCount * 3 + 4);
            }

            return new BulletShapeXNA(compoundshape, BSPhysicsShapeType.SHAPE_HULL);
        }

        public override bool AddObjectToWorld(BulletWorld pWorld, BulletBody pBody)
        {
            DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
            CollisionObject cbody = (pBody as BulletBodyXNA).body;
            RigidBody rbody = cbody as RigidBody;

            // Bullet resets several variables when an object is added to the world. In particular,
            //   BulletXNA resets position and rotation. Gravity is also reset depending on the static/dynamic
            //   type. Of course, the collision flags in the broadphase proxy are initialized to default.
            IndexedMatrix origPos = cbody.GetWorldTransform();
            if (rbody != null)
            {
                IndexedVector3 origGrav = rbody.GetGravity();
                world.AddRigidBody(rbody);
                rbody.SetGravity(origGrav);
            }
            else
            {
                world.AddCollisionObject(cbody);
            }
            cbody.SetWorldTransform(origPos);

            pBody.ApplyCollisionMask(pWorld.physicsScene);

            //if (body.GetBroadphaseHandle() != null)
            //    world.UpdateSingleAabb(body);
            return true;
        }

 private int PhysicsStep2(BulletWorld pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep,
     out int updatedEntityCount, out EntityProperties[] updatedEntities,
     out int collidersCount, out CollisionDesc[] colliders)
 {
     int epic = PhysicsStepint(pWorld, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount,
         out updatedEntities,
         out collidersCount, out colliders, m_maxCollisions, m_maxUpdatesPerFrame);
     return epic;
 }

        public override bool AddConstraintToWorld(BulletWorld pWorld, BulletConstraint pConstraint,
            bool pDisableCollisionsBetweenLinkedObjects)
        {
            DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
            TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
            world.AddConstraint(constraint, pDisableCollisionsBetweenLinkedObjects);

            return true;
        }

 private static EntityProperties GetDebugProperties(BulletWorld pWorld, BulletBody pCollisionObject)
 {
     EntityProperties ent = new EntityProperties();
     // 20131224 not used        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
     CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
     IndexedMatrix transform = collisionObject.GetWorldTransform();
     IndexedVector3 LinearVelocity = collisionObject.GetInterpolationLinearVelocity();
     IndexedVector3 AngularVelocity = collisionObject.GetInterpolationAngularVelocity();
     IndexedQuaternion rotation = transform.GetRotation();
     ent.Acceleration = Vector3.Zero;
     ent.ID = (uint)collisionObject.GetUserPointer();
     ent.Position = new Vector3(transform._origin.X, transform._origin.Y, transform._origin.Z);
     ent.Rotation = new Quaternion(rotation.X, rotation.Y, rotation.Z, rotation.W);
     ent.Velocity = new Vector3(LinearVelocity.X, LinearVelocity.Y, LinearVelocity.Z);
     ent.RotationalVelocity = new Vector3(AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z);
     return ent;
 }

 public override void UpdateSingleAabb(BulletWorld pWorld, BulletBody pCollisionObject)
 {
     DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
     CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
     world.UpdateSingleAabb(collisionObject);
 }

 public override BulletConstraint CreateGearConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
     Vector3 paxisInA, Vector3 paxisInB,
     float pratio, bool pdisableCollisionsBetweenLinkedBodies)
 {
     Generic6DofConstraint constrain = null;
     /*   BulletXNA does not have a gear constraint
     GearConstraint constrain = null;
     DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
     RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
     RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
     if (rb1 != null && rb2 != null)
     {
     IndexedVector3 axis1 = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z);
     IndexedVector3 axis2 = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z);
     constrain = new GearConstraint(rb1, rb2, ref axis1, ref axis2, pratio);
     world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
     }
     */
     return new BulletConstraintXNA(constrain);
 }

        //BuildCapsuleShape(physicsScene.World.ptr, 1f, 1f, prim.Scale);
        public override BulletShape BuildCapsuleShape(BulletWorld pWorld, float pRadius, float pHeight, Vector3 pScale)
        {
            DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
            IndexedVector3 scale = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
            CapsuleShapeZ capsuleShapeZ = new CapsuleShapeZ(pRadius, pHeight);
            capsuleShapeZ.SetMargin(world.WorldSettings.Params.collisionMargin);
            capsuleShapeZ.SetLocalScaling(ref scale);

            return new BulletShapeXNA(capsuleShapeZ, BSPhysicsShapeType.SHAPE_CAPSULE);
            ;
        }

        //sim.ptr, shape.ptr,prim.LocalID, prim.RawPosition, prim.RawOrientation
        public override BulletBody CreateGhostFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID,
            Vector3 pRawPosition, Quaternion pRawOrientation)
        {
            // 20131224 not used        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
            IndexedMatrix bodyTransform = new IndexedMatrix();
            bodyTransform._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
            bodyTransform.SetRotation(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y, pRawOrientation.Z,
                pRawOrientation.W));
            GhostObject gObj = new PairCachingGhostObject();
            gObj.SetWorldTransform(bodyTransform);
            CollisionShape shape = (pShape as BulletShapeXNA).shape;
            gObj.SetCollisionShape(shape);
            gObj.SetUserPointer(pLocalID);

            if (specialCollisionObjects.ContainsKey(pLocalID))
                specialCollisionObjects[pLocalID] = gObj;
            else
                specialCollisionObjects.Add(pLocalID, gObj);

            // TODO: Add to Special CollisionObjects!
            return new BulletBodyXNA(pLocalID, gObj);
        }

 public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape, HACDParams parms)
 {
     /* TODO */
     return null;
 }

        public override BulletShape CreateMeshShape(BulletWorld pWorld, int pIndicesCount, int[] indices,
            int pVerticesCount, float[] verticesAsFloats)
        {
            //DumpRaw(indices,verticesAsFloats,pIndicesCount,pVerticesCount);

            for (int iter = 0; iter < pVerticesCount; iter++)
            {
                if (verticesAsFloats[iter] > 0 && verticesAsFloats[iter] < 0.0001) verticesAsFloats[iter] = 0;
                if (verticesAsFloats[iter] < 0 && verticesAsFloats[iter] > -0.0001) verticesAsFloats[iter] = 0;
            }

            ObjectArray<int> indicesarr = new ObjectArray<int>(indices);
            ObjectArray<float> vertices = new ObjectArray<float>(verticesAsFloats);
            DumpRaw(indicesarr, vertices, pIndicesCount, pVerticesCount);
            DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
            IndexedMesh mesh = new IndexedMesh();
            mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
            mesh.m_numTriangles = pIndicesCount / 3;
            mesh.m_numVertices = pVerticesCount;
            mesh.m_triangleIndexBase = indicesarr;
            mesh.m_vertexBase = vertices;
            mesh.m_vertexStride = 3;
            mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
            mesh.m_triangleIndexStride = 3;

            TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
            tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
            BvhTriangleMeshShape meshShape = new BvhTriangleMeshShape(tribuilder, true, true);
            meshShape.SetMargin(world.WorldSettings.Params.collisionMargin);
            // world.UpdateSingleAabb(meshShape);
            return new BulletShapeXNA(meshShape, BSPhysicsShapeType.SHAPE_MESH);
        }