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 Vector3 GetAngularFactor(BulletBody pBody)
 {
     RigidBody body = (pBody as BulletBodyXNA).rigidBody;
     IndexedVector3 iv3 = body.GetAngularFactor();
     return new Vector3(iv3.X, iv3.Y, iv3.Z);
 }

 public override int GetActivationState(BulletBody pBody)
 {
     /* TODO */
     return 0;
 }

 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 void ApplyDamping(BulletBody pBody, float timeStep)
 {
     RigidBody body = (pBody as BulletBodyXNA).rigidBody;
     body.ApplyDamping(timeStep);
 }

 public override CollisionFlags AddToCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
 {
     CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
     CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
     existingcollisionFlags |= pcollisionFlags;
     collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
     return (CollisionFlags)(uint)existingcollisionFlags;
 }

        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 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 bool WantsSleeping(BulletBody pBody)
 {
     RigidBody body = (pBody as BulletBodyXNA).rigidBody;
     return body.WantsSleeping();
 }

 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 void UpdateInertiaTensor(BulletBody pBody)
 {
     RigidBody body = (pBody as BulletBodyXNA).rigidBody;
     if (body != null) // can't update inertia tensor on CollisionObject
         body.UpdateInertiaTensor();
 }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="pWorld"></param>
        /// <param name="pBody1"></param>
        /// <param name="pBody2"></param>
        /// <param name="pjoinPoint"></param>
        /// <param name="puseLinearReferenceFrameA"></param>
        /// <param name="pdisableCollisionsBetweenLinkedBodies"></param>
        /// <returns></returns>
        public override BulletConstraint Create6DofConstraintToPoint(BulletWorld pWorld, BulletBody pBody1,
            BulletBody pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA,
            bool pdisableCollisionsBetweenLinkedBodies)
        {
            DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
            RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
            RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
            IndexedMatrix frame1 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
            IndexedMatrix frame2 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));

            IndexedVector3 joinPoint = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
            IndexedMatrix mat = IndexedMatrix.Identity;
            mat._origin = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
            frame1._origin = body1.GetWorldTransform().Inverse() * joinPoint;
            frame2._origin = body2.GetWorldTransform().Inverse() * joinPoint;

            Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2,
                puseLinearReferenceFrameA);
            consttr.CalculateTransforms();
            world.AddConstraint(consttr, pdisableCollisionsBetweenLinkedBodies);

            return new BulletConstraintXNA(consttr);
        }

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

 public override void ApplyForce(BulletBody pBody, Vector3 force, Vector3 pos)
 {
     RigidBody body = (pBody as BulletBodyXNA).rigidBody;
     IndexedVector3 forceiv3 = new IndexedVector3(force.X, force.Y, force.Z);
     IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
     body.ApplyForce(ref forceiv3, ref posiv3);
 }

        public override BulletConstraint CreateSliderConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
            Vector3 pframe1, Quaternion pframe1rot,
            Vector3 pframe2, Quaternion pframe2rot,
            bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
        {
            SliderConstraint 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 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
                IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z,
                    pframe1rot.W);
                IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
                frame1._origin = frame1v;

                // 20131224 not used            IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
                IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z,
                    pframe2rot.W);
                IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
                frame2._origin = frame1v;

                constrain = new SliderConstraint(rb1, rb2, ref frame1, ref frame2, puseLinearReferenceFrameA);
                world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
            }
            return new BulletConstraintXNA(constrain);
        }

 public override void ApplyGravity(BulletBody pBody)
 {
     RigidBody body = (pBody as BulletBodyXNA).rigidBody;
     body.ApplyGravity();
 }

 public override void ForceActivationState(BulletBody pCollisionObject, ActivationState pActivationState)
 {
     CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
     collisionObject.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
 }