This updates prebuild to remove BulletSimN, implements the BulletSim API in BulletSPlugin using the BulletXNA Bullet physics engine. It also updates the BulletXNA library to be compatible with the changes. OpenSimDefaults has been updated to describe how to switch engines and terrain implementations.

1 files changed, 623 insertions, 262 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
index b6ff52b..49b1730 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
@@ -129,7 +129,12 @@ private sealed class BulletConstraintXNA : BulletConstraint
         get { return "XNAConstraint"; }
     }
 }
+    internal int m_maxCollisions;
+    internal CollisionDesc[] m_collisionArray;
 
+    internal int m_maxUpdatesPerFrame;
+    internal EntityProperties[] m_updateArray;
+    
     private static int m_collisionsThisFrame;
     private BSScene PhysicsScene { get; set; }
 
@@ -148,92 +153,98 @@ private sealed class BulletConstraintXNA : BulletConstraint
     /// <param name="p_2"></param>
     public override bool RemoveObjectFromWorld(BulletWorld pWorld, BulletBody pBody)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
         world.RemoveRigidBody(body);
         return true;
     }
 
-    public override bool AddConstraintToWorld(BulletWorld world, BulletConstraint constrain, bool disableCollisionsBetweenLinkedObjects)
+    public override bool AddConstraintToWorld(BulletWorld pWorld, BulletConstraint pConstraint, bool pDisableCollisionsBetweenLinkedObjects)
     {
-        /* TODO */
-        return false;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
+        world.AddConstraint(constraint, pDisableCollisionsBetweenLinkedObjects);
+        
+        return true;
+
     }
 
-    public override bool RemoveConstraintFromWorld(BulletWorld world, BulletConstraint constrain)
+    public override bool RemoveConstraintFromWorld(BulletWorld pWorld, BulletConstraint pConstraint)
     {
-        /* TODO */
-        return false;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
+        world.RemoveConstraint(constraint);
+        return true;
     }
 
-    public override void SetRestitution(BulletBody pBody, float pRestitution)
+    public override void SetRestitution(BulletBody pCollisionObject, float pRestitution)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetRestitution(pRestitution);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetRestitution(pRestitution);
     }
 
     public override int GetShapeType(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return (int)shape.GetShapeType();
     }
     public override void SetMargin(BulletShape pShape, float pMargin)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         shape.SetMargin(pMargin);
     }
 
     public override float GetMargin(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.GetMargin();
     }
 
     public override void SetLocalScaling(BulletShape pShape, Vector3 pScale)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         IndexedVector3 vec = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
         shape.SetLocalScaling(ref vec);
 
     }
 
-    public override void SetContactProcessingThreshold(BulletBody pBody, float contactprocessingthreshold)
+    public override void SetContactProcessingThreshold(BulletBody pCollisionObject, float contactprocessingthreshold)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetContactProcessingThreshold(contactprocessingthreshold);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetContactProcessingThreshold(contactprocessingthreshold);
     }
 
-    public override void SetCcdMotionThreshold(BulletBody pBody, float pccdMotionThreashold)
+    public override void SetCcdMotionThreshold(BulletBody pCollisionObject, float pccdMotionThreashold)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetCcdMotionThreshold(pccdMotionThreashold);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetCcdMotionThreshold(pccdMotionThreashold);
     }
 
-    public override void SetCcdSweptSphereRadius(BulletBody pBody, float pCcdSweptSphereRadius)
+    public override void SetCcdSweptSphereRadius(BulletBody pCollisionObject, float pCcdSweptSphereRadius)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetCcdSweptSphereRadius(pCcdSweptSphereRadius);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetCcdSweptSphereRadius(pCcdSweptSphereRadius);
     }
 
     public override void SetAngularFactorV(BulletBody pBody, Vector3 pAngularFactor)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.SetAngularFactor(new IndexedVector3(pAngularFactor.X, pAngularFactor.Y, pAngularFactor.Z));
     }
 
-    public override CollisionFlags AddToCollisionFlags(BulletBody pBody, CollisionFlags pcollisionFlags)
+    public override CollisionFlags AddToCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
     {
-        CollisionObject body = ((BulletBodyXNA)pBody).body;
-        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
         existingcollisionFlags |= pcollisionFlags;
-        body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
+        collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
         return (CollisionFlags) (uint) existingcollisionFlags;
     }
 
     public override bool AddObjectToWorld(BulletWorld pWorld, BulletBody pBody)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        CollisionObject cbody = ((BulletBodyXNA)pBody).body;
+        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,
@@ -259,99 +270,110 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return true;
     }
 
-    public override void ForceActivationState(BulletBody pBody, ActivationState pActivationState)
+    public override void ForceActivationState(BulletBody pCollisionObject, ActivationState pActivationState)
     {
-        CollisionObject body = ((BulletBodyXNA)pBody).body;
-        body.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
     }
 
-    public override void UpdateSingleAabb(BulletWorld pWorld, BulletBody pBody)
+    public override void UpdateSingleAabb(BulletWorld pWorld, BulletBody pCollisionObject)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        CollisionObject body = ((BulletBodyXNA)pBody).body;
-        world.UpdateSingleAabb(body);
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        world.UpdateSingleAabb(collisionObject);
     }
 
-    public override void UpdateAabbs(BulletWorld world) { /* TODO */ }
-    public override bool GetForceUpdateAllAabbs(BulletWorld world) { /* TODO */ return false;  }
-    public override void SetForceUpdateAllAabbs(BulletWorld world, bool force) { /* TODO */ }
+    public override void UpdateAabbs(BulletWorld pWorld) {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        world.UpdateAabbs();
+    }
+    public override bool GetForceUpdateAllAabbs(BulletWorld pWorld) {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        return world.GetForceUpdateAllAabbs();
+        
+    }
+    public override void SetForceUpdateAllAabbs(BulletWorld pWorld, bool pForce)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        world.SetForceUpdateAllAabbs(pForce);
+    }
 
-    public override bool SetCollisionGroupMask(BulletBody pBody, uint pGroup, uint pMask)
+    public override bool SetCollisionGroupMask(BulletBody pCollisionObject, uint pGroup, uint pMask)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
-        body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
-        if ((uint) body.GetBroadphaseHandle().m_collisionFilterGroup == 0)
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
+        collisionObject.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
+        if ((uint) collisionObject.GetBroadphaseHandle().m_collisionFilterGroup == 0)
             return false;
         return true;
     }
 
-    public override void ClearAllForces(BulletBody pBody)
+    public override void ClearAllForces(BulletBody pCollisionObject)
     {
-        CollisionObject body = ((BulletBodyXNA)pBody).body;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
         IndexedVector3 zeroVector = new IndexedVector3(0, 0, 0);
-        body.SetInterpolationLinearVelocity(ref zeroVector);
-        body.SetInterpolationAngularVelocity(ref zeroVector);
-        IndexedMatrix bodytransform = body.GetWorldTransform();
+        collisionObject.SetInterpolationLinearVelocity(ref zeroVector);
+        collisionObject.SetInterpolationAngularVelocity(ref zeroVector);
+        IndexedMatrix bodytransform = collisionObject.GetWorldTransform();
 
-        body.SetInterpolationWorldTransform(ref bodytransform);
+        collisionObject.SetInterpolationWorldTransform(ref bodytransform);
 
-        if (body is RigidBody)
+        if (collisionObject is RigidBody)
         {
-            RigidBody rigidbody = body as RigidBody;
+            RigidBody rigidbody = collisionObject as RigidBody;
             rigidbody.SetLinearVelocity(zeroVector);
             rigidbody.SetAngularVelocity(zeroVector);
             rigidbody.ClearForces();
         }
     }
 
-    public override void SetInterpolationAngularVelocity(BulletBody pBody, Vector3 pVector3)
+    public override void SetInterpolationAngularVelocity(BulletBody pCollisionObject, Vector3 pVector3)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
         IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
-        body.SetInterpolationAngularVelocity(ref vec);
+        collisionObject.SetInterpolationAngularVelocity(ref vec);
     }
 
     public override void SetAngularVelocity(BulletBody pBody, Vector3 pVector3)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
         body.SetAngularVelocity(ref vec);
     }
     public override Vector3 GetTotalForce(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = body.GetTotalForce();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
     public override Vector3 GetTotalTorque(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = body.GetTotalTorque();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
     public override Vector3 GetInvInertiaDiagLocal(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = body.GetInvInertiaDiagLocal();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
     public override void SetInvInertiaDiagLocal(BulletBody pBody, Vector3 inert)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = new IndexedVector3(inert.X, inert.Y, inert.Z);
         body.SetInvInertiaDiagLocal(ref iv3);
     }
     public override void ApplyForce(BulletBody pBody, Vector3 force, Vector3 pos)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        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 void ApplyImpulse(BulletBody pBody, Vector3 imp, Vector3 pos)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 impiv3 = new IndexedVector3(imp.X, imp.Y, imp.Z);
         IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
         body.ApplyImpulse(ref impiv3, ref posiv3);
@@ -359,32 +381,32 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override void ClearForces(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.ClearForces();
     }
 
-    public override void SetTranslation(BulletBody pBody, Vector3 _position, Quaternion _orientation)
+    public override void SetTranslation(BulletBody pCollisionObject, Vector3 _position, Quaternion _orientation)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
         IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
         IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
                                                               _orientation.W);
         IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
         mat._origin = vposition;
-        body.SetWorldTransform(mat);
+        collisionObject.SetWorldTransform(mat);
         
     }
 
-    public override Vector3 GetPosition(BulletBody pBody)
+    public override Vector3 GetPosition(BulletBody pCollisionObject)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        IndexedVector3 pos = body.GetInterpolationWorldTransform()._origin;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        IndexedVector3 pos = collisionObject.GetInterpolationWorldTransform()._origin;
         return new Vector3(pos.X, pos.Y, pos.Z);
     }
 
     public override Vector3 CalculateLocalInertia(BulletShape pShape, float pphysMass)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         IndexedVector3 inertia = IndexedVector3.Zero;
         shape.CalculateLocalInertia(pphysMass, out inertia);
         return new Vector3(inertia.X, inertia.Y, inertia.Z);
@@ -392,7 +414,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override void SetMassProps(BulletBody pBody, float pphysMass, Vector3 plocalInertia)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 inertia = new IndexedVector3(plocalInertia.X, plocalInertia.Y, plocalInertia.Z);
         body.SetMassProps(pphysMass, inertia);
     }
@@ -400,73 +422,90 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override void SetObjectForce(BulletBody pBody, Vector3 _force)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 force = new IndexedVector3(_force.X, _force.Y, _force.Z);
         body.SetTotalForce(ref force);
     }
 
-    public override void SetFriction(BulletBody pBody, float _currentFriction)
+    public override void SetFriction(BulletBody pCollisionObject, float _currentFriction)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetFriction(_currentFriction);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetFriction(_currentFriction);
     }
 
     public override void SetLinearVelocity(BulletBody pBody, Vector3 _velocity)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 velocity = new IndexedVector3(_velocity.X, _velocity.Y, _velocity.Z);
         body.SetLinearVelocity(velocity);
     }
 
-    public override void Activate(BulletBody pBody, bool pforceactivation)
+    public override void Activate(BulletBody pCollisionObject, bool pforceactivation)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.Activate(pforceactivation);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.Activate(pforceactivation);
         
     }
 
-    public override Quaternion GetOrientation(BulletBody pBody)
+    public override Quaternion GetOrientation(BulletBody pCollisionObject)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        IndexedQuaternion mat = body.GetInterpolationWorldTransform().GetRotation();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        IndexedQuaternion mat = collisionObject.GetInterpolationWorldTransform().GetRotation();
         return new Quaternion(mat.X, mat.Y, mat.Z, mat.W);
     }
 
-    public override CollisionFlags RemoveFromCollisionFlags(BulletBody pBody, CollisionFlags pcollisionFlags)
+    public override CollisionFlags RemoveFromCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
         existingcollisionFlags &= ~pcollisionFlags;
-        body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
+        collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
         return (CollisionFlags)(uint)existingcollisionFlags;
     }
 
-    public override float GetCcdMotionThreshold(BulletBody obj) { /* TODO */ return 0f;  }
+    public override float GetCcdMotionThreshold(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.GetCcdSquareMotionThreshold();
+    }
 
-    public override float GetCcdSweptSphereRadius(BulletBody obj) { /* TODO */ return 0f;  }
+    public override float GetCcdSweptSphereRadius(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.GetCcdSweptSphereRadius();
+        
+    }
 
-    public override IntPtr GetUserPointer(BulletBody obj) { /* TODO */ return IntPtr.Zero;  }
+    public override IntPtr GetUserPointer(BulletBody pCollisionObject)
+    {
+        CollisionObject shape = (pCollisionObject as BulletBodyXNA).body;
+        return (IntPtr)shape.GetUserPointer();
+    }
 
-    public override void SetUserPointer(BulletBody obj, IntPtr val) { /* TODO */ }
+    public override void SetUserPointer(BulletBody pCollisionObject, IntPtr val)
+    {
+        CollisionObject shape = (pCollisionObject as BulletBodyXNA).body;
+        shape.SetUserPointer(val);
+    }
 
     public override void SetGravity(BulletBody pBody, Vector3 pGravity)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 gravity = new IndexedVector3(pGravity.X, pGravity.Y, pGravity.Z);
         body.SetGravity(gravity);
     }
 
     public override bool DestroyConstraint(BulletWorld pWorld, BulletConstraint pConstraint)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        TypedConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
         world.RemoveConstraint(constraint);
         return true;
     }
 
     public override bool SetLinearLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
         IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
         constraint.SetLinearLowerLimit(lowlimit);
@@ -476,7 +515,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override bool SetAngularLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
         IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
         constraint.SetAngularLowerLimit(lowlimit);
@@ -486,20 +525,20 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override void SetConstraintNumSolverIterations(BulletConstraint pConstraint, float cnt)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         constraint.SetOverrideNumSolverIterations((int)cnt);
     }
 
     public override bool CalculateTransforms(BulletConstraint pConstraint)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         constraint.CalculateTransforms();
         return true;
     }
 
     public override void SetConstraintEnable(BulletConstraint pConstraint, float p_2)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         constraint.SetEnabled((p_2 == 0) ? false : true);
     }
 
@@ -508,9 +547,9 @@ private sealed class BulletConstraintXNA : BulletConstraint
     public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
 
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        RigidBody body1 = ((BulletBodyXNA)pBody1).rigidBody;
-        RigidBody body2 = ((BulletBodyXNA)pBody2).rigidBody;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
         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);
@@ -542,9 +581,9 @@ private sealed class BulletConstraintXNA : BulletConstraint
     /// <returns></returns>
     public override BulletConstraint Create6DofConstraintToPoint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        RigidBody body1 = ((BulletBodyXNA)pBody1).rigidBody;
-        RigidBody body2 = ((BulletBodyXNA)pBody2).rigidBody;
+        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));
 
@@ -563,7 +602,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
     //SetFrames(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
     public override bool SetFrames(BulletConstraint pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         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);
@@ -579,109 +618,110 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override Vector3 GetLinearVelocity(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = body.GetLinearVelocity();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
     public override Vector3 GetAngularVelocity(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = body.GetAngularVelocity();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
     public override Vector3 GetVelocityInLocalPoint(BulletBody pBody, Vector3 pos)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
         IndexedVector3 iv3 = body.GetVelocityInLocalPoint(ref posiv3);
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
-    public override void Translate(BulletBody pBody, Vector3 trans)
+    public override void Translate(BulletBody pCollisionObject, Vector3 trans)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.Translate(new IndexedVector3(trans.X,trans.Y,trans.Z));
     }
     public override void UpdateDeactivation(BulletBody pBody, float timeStep)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.UpdateDeactivation(timeStep);
     }
 
     public override bool WantsSleeping(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         return body.WantsSleeping();
     }
 
     public override void SetAngularFactor(BulletBody pBody, float factor)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.SetAngularFactor(factor);
     }
 
     public override Vector3 GetAngularFactor(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 =  body.GetAngularFactor();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
 
-    public override bool IsInWorld(BulletWorld pWorld, BulletBody pBody)
+    public override bool IsInWorld(BulletWorld pWorld, BulletBody pCollisionObject)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        CollisionObject body = ((BulletBodyXNA)pBody).body;
-        return world.IsInWorld(body);
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        return world.IsInWorld(collisionObject);
     }
 
-    public override void AddConstraintRef(BulletBody pBody, BulletConstraint pConstrain)
+    public override void AddConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        TypedConstraint constrain = ((BulletConstraintXNA)pConstrain).constrain;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        TypedConstraint constrain = (pConstraint as BulletConstraintXNA).constrain;
         body.AddConstraintRef(constrain);
     }
 
-    public override void RemoveConstraintRef(BulletBody pBody, BulletConstraint pConstrain)
+    public override void RemoveConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        TypedConstraint constrain = ((BulletConstraintXNA)pConstrain).constrain;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        TypedConstraint constrain = (pConstraint as BulletConstraintXNA).constrain;
         body.RemoveConstraintRef(constrain);
     }
 
     public override BulletConstraint GetConstraintRef(BulletBody pBody, int index)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         return new BulletConstraintXNA(body.GetConstraintRef(index));
     }
 
     public override int GetNumConstraintRefs(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         return body.GetNumConstraintRefs();
     }
 
-    public override void SetInterpolationLinearVelocity(BulletBody pBody, Vector3 VehicleVelocity)
+    public override void SetInterpolationLinearVelocity(BulletBody pCollisionObject, Vector3 VehicleVelocity)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
         IndexedVector3 velocity = new IndexedVector3(VehicleVelocity.X, VehicleVelocity.Y, VehicleVelocity.Z);
-        body.SetInterpolationLinearVelocity(ref velocity);
+        collisionObject.SetInterpolationLinearVelocity(ref velocity);
     }
 
     public override bool UseFrameOffset(BulletConstraint pConstraint, float onOff)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         constraint.SetUseFrameOffset((onOff == 0) ? false : true);
         return true;
     }
     //SetBreakingImpulseThreshold(m_constraint.ptr, threshold);
     public override bool SetBreakingImpulseThreshold(BulletConstraint pConstraint, float threshold)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         constraint.SetBreakingImpulseThreshold(threshold);
         return true;
     }
     //BulletSimAPI.SetAngularDamping(Prim.PhysBody.ptr, angularDamping);
     public override void SetAngularDamping(BulletBody pBody, float angularDamping)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         float lineardamping = body.GetLinearDamping();
         body.SetDamping(lineardamping, angularDamping);
 
@@ -689,111 +729,183 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override void UpdateInertiaTensor(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.UpdateInertiaTensor();
     }
 
     public override void RecalculateCompoundShapeLocalAabb(BulletShape pCompoundShape)
     {
-        CompoundShape shape = ((BulletShapeXNA)pCompoundShape).shape as CompoundShape;
+        CompoundShape shape = (pCompoundShape as BulletShapeXNA).shape as CompoundShape;
         shape.RecalculateLocalAabb();
     }
 
     //BulletSimAPI.GetCollisionFlags(PhysBody.ptr)
-    public override CollisionFlags GetCollisionFlags(BulletBody pBody)
+    public override CollisionFlags GetCollisionFlags(BulletBody pCollisionObject)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        uint flags = (uint)body.GetCollisionFlags();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        uint flags = (uint)collisionObject.GetCollisionFlags();
         return (CollisionFlags) flags;
     }
 
     public override void SetDamping(BulletBody pBody, float pLinear, float pAngular)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.SetDamping(pLinear, pAngular);
     }
     //PhysBody.ptr, PhysicsScene.Params.deactivationTime);
-    public override void SetDeactivationTime(BulletBody pBody, float pDeactivationTime)
+    public override void SetDeactivationTime(BulletBody pCollisionObject, float pDeactivationTime)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetDeactivationTime(pDeactivationTime);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetDeactivationTime(pDeactivationTime);
     }
     //SetSleepingThresholds(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
     public override void SetSleepingThresholds(BulletBody pBody, float plinearSleepingThreshold, float pangularSleepingThreshold)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.SetSleepingThresholds(plinearSleepingThreshold, pangularSleepingThreshold);
     }
 
-    public override CollisionObjectTypes GetBodyType(BulletBody pBody)
+    public override CollisionObjectTypes GetBodyType(BulletBody pCollisionObject)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        return (CollisionObjectTypes)(int) body.GetInternalType();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return (CollisionObjectTypes)(int) collisionObject.GetInternalType();
     }
 
-    public override void ApplyGravity(BulletBody obj) { /* TODO */ }
+    public override void ApplyGravity(BulletBody pBody)
+    {
 
-    public override Vector3 GetGravity(BulletBody obj) { /* TODO */ return Vector3.Zero;  }
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.ApplyGravity();
+    }
 
-    public override void SetLinearDamping(BulletBody obj, float lin_damping) { /* TODO */ }
+    public override Vector3 GetGravity(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        IndexedVector3 gravity = body.GetGravity();
+        return new Vector3(gravity.X, gravity.Y, gravity.Z);
+    }
 
-    public override float GetLinearDamping(BulletBody obj) { /* TODO */ return 0f;  }
+    public override void SetLinearDamping(BulletBody pBody, float lin_damping)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        float angularDamping = body.GetAngularDamping();
+        body.SetDamping(lin_damping, angularDamping); 
+    }
 
-    public override float GetAngularDamping(BulletBody obj) { /* TODO */ return 0f;  }
+    public override float GetLinearDamping(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        return body.GetLinearDamping();
+    }
+
+    public override float GetAngularDamping(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        return body.GetAngularDamping();
+    }
 
-    public override float GetLinearSleepingThreshold(BulletBody obj) { /* TODO */ return 0f;  }
+    public override float GetLinearSleepingThreshold(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        return body.GetLinearSleepingThreshold();
+    }
 
-    public override void ApplyDamping(BulletBody obj, float timeStep) { /* TODO */ }
+    public override void ApplyDamping(BulletBody pBody, float timeStep)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.ApplyDamping(timeStep);
+    }
 
-    public override Vector3 GetLinearFactor(BulletBody obj) { /* TODO */ return Vector3.Zero; }
+    public override Vector3 GetLinearFactor(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        IndexedVector3 linearFactor = body.GetLinearFactor();
+        return new Vector3(linearFactor.X, linearFactor.Y, linearFactor.Z);
+    }
 
-    public override void SetLinearFactor(BulletBody obj, Vector3 factor) { /* TODO */ }
+    public override void SetLinearFactor(BulletBody pBody, Vector3 factor)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.SetLinearFactor(new IndexedVector3(factor.X, factor.Y, factor.Z));
+    }
 
-    public override void SetCenterOfMassByPosRot(BulletBody obj, Vector3 pos, Quaternion rot) { /* TODO */ }
+    public override void SetCenterOfMassByPosRot(BulletBody pBody, Vector3 pos, Quaternion rot)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        IndexedQuaternion quat = new IndexedQuaternion(rot.X, rot.Y, rot.Z,rot.W);
+        IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(quat);
+        mat._origin = new IndexedVector3(pos.X, pos.Y, pos.Z);
+        body.SetCenterOfMassTransform( ref mat);
+        /* TODO: double check this */
+    }
 
     //BulletSimAPI.ApplyCentralForce(PhysBody.ptr, fSum);
     public override void ApplyCentralForce(BulletBody pBody, Vector3 pfSum)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
         body.ApplyCentralForce(ref fSum);
     }
     public override void ApplyCentralImpulse(BulletBody pBody, Vector3 pfSum)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
         body.ApplyCentralImpulse(ref fSum);
     }
     public override void ApplyTorque(BulletBody pBody, Vector3 pfSum)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
         body.ApplyTorque(ref fSum);
     }
     public override void ApplyTorqueImpulse(BulletBody pBody, Vector3 pfSum)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
         body.ApplyTorqueImpulse(ref fSum);
     }
 
-    public override void DestroyObject(BulletWorld p, BulletBody p_2)
+    public override void DestroyObject(BulletWorld pWorld, BulletBody pBody)
     {
-        //TODO:
+        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);
+            }
+        }
+       
     }
 
     public override void Shutdown(BulletWorld pWorld)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         world.Cleanup();
     }
 
-    public override BulletShape DuplicateCollisionShape(BulletWorld sim, BulletShape srcShape, uint id)
+    public override BulletShape DuplicateCollisionShape(BulletWorld pWorld, BulletShape pShape, uint id)
     {
-        return null;
+        CollisionShape shape1 = (pShape as BulletShapeXNA).shape;
+
+        // TODO:  Turn this from a reference copy to a Value Copy.
+        BulletShapeXNA shape2 = new BulletShapeXNA(shape1, BSPhysicsShapeType.SHAPE_UNKNOWN);
+        
+        return shape2;
     }
 
-    public override bool DeleteCollisionShape(BulletWorld p, BulletShape p_2)
+    public override bool DeleteCollisionShape(BulletWorld pWorld, BulletShape pShape)
     {
         //TODO:
         return false;
@@ -802,17 +914,40 @@ private sealed class BulletConstraintXNA : BulletConstraint
                
     public override BulletBody CreateBodyFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
     {
-        CollisionWorld world = ((BulletWorldXNA)pWorld).world;
+        CollisionWorld world = (pWorld as BulletWorldXNA).world;
         IndexedMatrix mat =
             IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
                                                                      pRawOrientation.Z, pRawOrientation.W));
         mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         //UpdateSingleAabb(world, shape);
         // TODO: Feed Update array into null
-        RigidBody body = new RigidBody(0,new SimMotionState(world,pLocalID,mat,null),shape,IndexedVector3.Zero);
-        
+        SimMotionState motionState = new SimMotionState(world, pLocalID, mat, null);
+        RigidBody body = new RigidBody(0,motionState,shape,IndexedVector3.Zero);
+        RigidBodyConstructionInfo constructionInfo = new RigidBodyConstructionInfo(0, new SimMotionState(world, pLocalID, mat, null),shape,IndexedVector3.Zero)
+                                                         {
+                                                             m_mass = 0
+                                                         };
+        /*
+            m_mass = mass;
+                        m_motionState =motionState;
+                        m_collisionShape = collisionShape;
+                        m_localInertia = localInertia;
+                        m_linearDamping = 0f;
+                        m_angularDamping = 0f;
+                        m_friction = 0.5f;
+                        m_restitution = 0f;
+                        m_linearSleepingThreshold = 0.8f;
+                        m_angularSleepingThreshold = 1f;
+                        m_additionalDamping = false;
+                        m_additionalDampingFactor = 0.005f;
+                        m_additionalLinearDampingThresholdSqr = 0.01f;
+                        m_additionalAngularDampingThresholdSqr = 0.01f;
+                        m_additionalAngularDampingFactor = 0.01f;
+            m_startWorldTransform = IndexedMatrix.Identity;
+        */
         body.SetUserPointer(pLocalID);
+        
         return new BulletBodyXNA(pLocalID, body);
     }
 
@@ -825,7 +960,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
                                                                      pRawOrientation.Z, pRawOrientation.W));
         mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
 
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
 
         // TODO: Feed Update array into null
         RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero);
@@ -834,21 +969,43 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return new BulletBodyXNA(pLocalID, body);
     }
     //(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
-    public override CollisionFlags SetCollisionFlags(BulletBody pBody, CollisionFlags collisionFlags)
+    public override CollisionFlags SetCollisionFlags(BulletBody pCollisionObject, CollisionFlags collisionFlags)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags);
-        return (CollisionFlags)body.GetCollisionFlags();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags);
+        return (CollisionFlags)collisionObject.GetCollisionFlags();
     }
 
-    public override Vector3 GetAnisotripicFriction(BulletConstraint pconstrain) { /* TODO */ return Vector3.Zero; }
+    public override Vector3 GetAnisotripicFriction(BulletConstraint pconstrain)
+    {
+
+        /* TODO */ 
+        return Vector3.Zero;
+    }
     public override Vector3 SetAnisotripicFriction(BulletConstraint pconstrain, Vector3 frict) { /* TODO */ return Vector3.Zero; }
     public override bool HasAnisotripicFriction(BulletConstraint pconstrain) { /* TODO */ return false; }
     public override float GetContactProcessingThreshold(BulletBody pBody) { /* TODO */ return 0f; }
-    public override bool IsStaticObject(BulletBody pBody) { /* TODO */ return false; }
-    public override bool IsKinematicObject(BulletBody pBody) { /* TODO */ return false; }
-    public override bool IsStaticOrKinematicObject(BulletBody pBody) { /* TODO */ return false; }
-    public override bool HasContactResponse(BulletBody pBody) { /* TODO */ return false; }
+    public override bool IsStaticObject(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.IsStaticObject();
+        
+    }
+    public override bool IsKinematicObject(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.IsKinematicObject();
+    }
+    public override bool IsStaticOrKinematicObject(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.IsStaticOrKinematicObject();
+    }
+    public override bool HasContactResponse(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.HasContactResponse();
+    }
     public override int GetActivationState(BulletBody pBody) { /* TODO */ return 0; }
     public override void SetActivationState(BulletBody pBody, int state) { /* TODO */ }
     public override float GetDeactivationTime(BulletBody pBody) { /* TODO */ return 0f; }
@@ -859,15 +1016,15 @@ private sealed class BulletConstraintXNA : BulletConstraint
     public override float GetHitFraction(BulletBody pBody) { /* TODO */ return 0f; }
 
     //(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
-    public override void SetHitFraction(BulletBody pBody, float pHitFraction)
+    public override void SetHitFraction(BulletBody pCollisionObject, float pHitFraction)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetHitFraction(pHitFraction);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetHitFraction(pHitFraction);
     }
     //BuildCapsuleShape(physicsScene.World.ptr, 1f, 1f, prim.Scale);
     public override BulletShape BuildCapsuleShape(BulletWorld pWorld, float pRadius, float pHeight, Vector3 pScale)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        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);
@@ -881,14 +1038,24 @@ private sealed class BulletConstraintXNA : BulletConstraint
                                             int maxUpdates, ref EntityProperties[] updateArray
                                             )
     {
+
+        m_updateArray = updateArray;
+        m_collisionArray = collisionArray;
         /* TODO */
-        return new BulletWorldXNA(1, null, null);
+        ConfigurationParameters[] configparms = new ConfigurationParameters[1];
+        configparms[0] = parms;
+        Vector3 worldExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight);
+        m_maxCollisions = maxCollisions;
+        m_maxUpdatesPerFrame = maxUpdates;
+
+
+        return new BulletWorldXNA(1, PhysicsScene, BSAPIXNA.Initialize2(worldExtent, configparms, maxCollisions, ref collisionArray, maxUpdates, ref updateArray, null));
     }
 
-    private static object Initialize2(Vector3 worldExtent, 
+    private static DiscreteDynamicsWorld Initialize2(Vector3 worldExtent, 
                         ConfigurationParameters[] o,
-                        int mMaxCollisionsPerFrame, ref List<BulletXNA.CollisionDesc> collisionArray,
-                        int mMaxUpdatesPerFrame, ref List<BulletXNA.EntityProperties> updateArray, 
+                        int mMaxCollisionsPerFrame, ref CollisionDesc[] collisionArray,
+                        int mMaxUpdatesPerFrame, ref EntityProperties[] updateArray, 
                         object mDebugLogCallbackHandle)
     {
         CollisionWorld.WorldData.ParamData p = new CollisionWorld.WorldData.ParamData();
@@ -968,8 +1135,13 @@ private sealed class BulletConstraintXNA : BulletConstraint
         SequentialImpulseConstraintSolver m_solver = new SequentialImpulseConstraintSolver();
 
         DiscreteDynamicsWorld world = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, cci);
-        world.UpdatedObjects = updateArray;
-        world.UpdatedCollisions = collisionArray;
+        
+        
+        world.UpdatedObjects = BSAPIXNA.GetBulletXNAEntityStruct(BSAPIXNA.BulletSimEntityStructToByteArray(updateArray, updateArray.Length));
+        world.UpdatedCollisions = BSAPIXNA.GetBulletXNACollisionStruct(BSAPIXNA.BulletSimCollisionStructToByteArray(collisionArray, collisionArray.Length));
+        world.LastCollisionDesc = 0;
+        world.LastEntityProperty = 0;
+
         world.WorldSettings.Params = p;
         world.SetForceUpdateAllAabbs(p.shouldForceUpdateAllAabbs != 0);
         world.GetSolverInfo().m_solverMode = SolverMode.SOLVER_USE_WARMSTARTING | SolverMode.SOLVER_SIMD;
@@ -1003,7 +1175,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
         world.GetSolverInfo().m_restingContactRestitutionThreshold = 2;
         world.SetForceUpdateAllAabbs(true);
 
-
+        //BSParam.TerrainImplementation = 0;
         world.SetGravity(new IndexedVector3(0,0,p.gravity));
 
         return world;
@@ -1011,7 +1183,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
     //m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL
     public override bool SetConstraintParam(BulletConstraint pConstraint, ConstraintParams paramIndex, float paramvalue, ConstraintParamAxis axis)
     {
-        Generic6DofConstraint constrain = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constrain = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
         {
             constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 0);
@@ -1034,7 +1206,8 @@ private sealed class BulletConstraintXNA : BulletConstraint
     public override bool PushUpdate(BulletBody pCollisionObject)
     {
         bool ret = false;
-        RigidBody rb = ((BulletBodyXNA)pCollisionObject).rigidBody;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        RigidBody rb = collisionObject as RigidBody;
         if (rb != null)
         {
             SimMotionState sms = rb.GetMotionState() as SimMotionState;
@@ -1052,57 +1225,57 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override float GetAngularMotionDisc(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.GetAngularMotionDisc();
     }
     public override float GetContactBreakingThreshold(BulletShape pShape, float defaultFactor)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.GetContactBreakingThreshold(defaultFactor);
     }
     public override bool IsCompound(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsCompound();
     }
     public override bool IsSoftBody(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsSoftBody();
     }
     public override bool IsPolyhedral(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsPolyhedral();
     }
     public override bool IsConvex2d(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsConvex2d();
     }
     public override bool IsConvex(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsConvex();
     }
     public override bool IsNonMoving(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsNonMoving();
     }
     public override bool IsConcave(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsConcave();
     }
     public override bool IsInfinite(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsInfinite();
     }
     public override bool IsNativeShape(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         bool ret;
         switch (shape.GetShapeType())
         {
@@ -1119,38 +1292,53 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return ret;
     }
 
-    public override void SetShapeCollisionMargin(BulletShape shape, float margin) { /* TODO */ }
+    public override void SetShapeCollisionMargin(BulletShape pShape, float pMargin)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        shape.SetMargin(pMargin);
+    }
 
     //sim.ptr, shape.ptr,prim.LocalID, prim.RawPosition, prim.RawOrientation
     public override BulletBody CreateGhostFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        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 = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         gObj.SetCollisionShape(shape);
         gObj.SetUserPointer(pLocalID);
         // TODO: Add to Special CollisionObjects!
         return new BulletBodyXNA(pLocalID, gObj);
     }
 
-    public override void SetCollisionShape(BulletWorld pWorld, BulletBody pObj, BulletShape pShape)
+    public override void SetCollisionShape(BulletWorld pWorld, BulletBody pCollisionObject, BulletShape pShape)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        CollisionObject obj = ((BulletBodyXNA)pObj).body;
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
-        obj.SetCollisionShape(shape);
-
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        if (pShape == null)
+        {
+            collisionObject.SetCollisionShape(new EmptyShape());
+        }
+        else
+        {
+            CollisionShape shape = (pShape as BulletShapeXNA).shape;
+            collisionObject.SetCollisionShape(shape);
+        }
+    }
+    public override BulletShape GetCollisionShape(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        CollisionShape shape = collisionObject.GetCollisionShape();
+        return new BulletShapeXNA(shape,BSPhysicsShapeType.SHAPE_UNKNOWN);
     }
-    public override BulletShape GetCollisionShape(BulletBody obj) { /* TODO */ return null; }
 
     //(PhysicsScene.World.ptr, nativeShapeData)
     public override BulletShape BuildNativeShape(BulletWorld pWorld, ShapeData pShapeData)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         CollisionShape shape = null;
         switch (pShapeData.Type)
         {
@@ -1185,15 +1373,15 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override int GetNumberOfCompoundChildren(BulletShape pCompoundShape)
     {
-        CompoundShape compoundshape = ((BulletShapeXNA)pCompoundShape).shape as CompoundShape;
+        CompoundShape compoundshape = (pCompoundShape as BulletShapeXNA).shape as CompoundShape;
         return compoundshape.GetNumChildShapes();
     }
     //LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot
     public override void AddChildShapeToCompoundShape(BulletShape pCShape, BulletShape paddShape, Vector3 displacementPos, Quaternion displacementRot)
     {
         IndexedMatrix relativeTransform = new IndexedMatrix();
-        CompoundShape compoundshape = ((BulletShapeXNA)pCShape).shape as CompoundShape;
-        CollisionShape addshape = ((BulletShapeXNA)paddShape).shape;
+        CompoundShape compoundshape = (pCShape as BulletShapeXNA).shape as CompoundShape;
+        CollisionShape addshape = (paddShape as BulletShapeXNA).shape;
 
         relativeTransform._origin = new IndexedVector3(displacementPos.X, displacementPos.Y, displacementPos.Z);
         relativeTransform.SetRotation(new IndexedQuaternion(displacementRot.X,displacementRot.Y,displacementRot.Z,displacementRot.W));
@@ -1203,7 +1391,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape pCShape, int pii)
     {
-        CompoundShape compoundshape = ((BulletShapeXNA)pCShape).shape as CompoundShape;
+        CompoundShape compoundshape = (pCShape as BulletShapeXNA).shape as CompoundShape;
         CollisionShape ret = null;
         ret = compoundshape.GetChildShape(pii);
         compoundshape.RemoveChildShapeByIndex(pii);
@@ -1222,12 +1410,12 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return new BulletShapeXNA(m_planeshape, BSPhysicsShapeType.SHAPE_GROUNDPLANE);
     }
 
-    public override BulletConstraint CreateHingeConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody ppBody2, Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+    public override BulletConstraint CreateHingeConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
     {
         HingeConstraint constrain = null;
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        RigidBody rb1 = ((BulletBodyXNA)pBody1).rigidBody;
-        RigidBody rb2 = ((BulletBodyXNA)ppBody2).rigidBody;
+        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);
@@ -1241,7 +1429,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override BulletShape CreateHullShape(BulletWorld pWorld, int pHullCount, float[] pConvHulls)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         CompoundShape compoundshape = new CompoundShape(false);
         
         compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin);
@@ -1271,7 +1459,11 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return new BulletShapeXNA(compoundshape, BSPhysicsShapeType.SHAPE_HULL);
     }
 
-    public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape) { /* TODO */ return null; }
+    public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape)
+    {
+        /* TODO */ return null;
+
+    }
 
     public override BulletShape CreateMeshShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
     {
@@ -1286,7 +1478,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
         ObjectArray<int> indicesarr = new ObjectArray<int>(indices);
         ObjectArray<float> vertices = new ObjectArray<float>(verticesAsFloats);
         DumpRaw(indicesarr,vertices,pIndicesCount,pVerticesCount);
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         IndexedMesh mesh = new IndexedMesh();
         mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
         mesh.m_numTriangles = pIndicesCount/3;
@@ -1402,7 +1594,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override bool TranslationalLimitMotor(BulletConstraint pConstraint, float ponOff, float targetVelocity, float maxMotorForce)
     {
-        TypedConstraint tconstrain = ((BulletConstraintXNA)pConstraint).constrain;
+        TypedConstraint tconstrain = (pConstraint as BulletConstraintXNA).constrain;
         bool onOff = ponOff != 0;
         bool ret = false;
 
@@ -1428,47 +1620,45 @@ private sealed class BulletConstraintXNA : BulletConstraint
         /* TODO */
         updatedEntityCount = 0;
         collidersCount = 0;
-        return 1;
+        
+
+        int ret = PhysicsStep2(world,timeStep,maxSubSteps,fixedTimeStep,out updatedEntityCount,out world.physicsScene.m_updateArray, out collidersCount, out world.physicsScene.m_collisionArray);
+
+        return ret;
     }
 
     private int PhysicsStep2(BulletWorld pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep, 
-                    out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, 
-                    out int collidersCount, out List<BulletXNA.CollisionDesc>colliders)
+                    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);
+                                out collidersCount, out colliders, m_maxCollisions, m_maxUpdatesPerFrame);
         return epic;
     }
 
-    private static int PhysicsStepint(BulletWorld pWorld,float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, out int collidersCount, out List<BulletXNA.CollisionDesc> colliders)
+    private static 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;
-      
 
-        //if (updatedEntities is null)
-        //    updatedEntities = new List<BulletXNA.EntityProperties>();
-
-        //if (colliders is null)
-        //    colliders = new List<BulletXNA.CollisionDesc>();
+        updatedEntityCount = 0;
+        collidersCount = 0;
         
 
         if (pWorld is BulletWorldXNA)
         {
-            DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+            DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
 
+            world.LastCollisionDesc = 0;
+            world.LastEntityProperty = 0;
+            world.UpdatedObjects = new BulletXNA.EntityProperties[maxUpdates];
+            world.UpdatedCollisions = new BulletXNA.CollisionDesc[maxCollisions];
             numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep);
             int updates = 0;
 
-            updatedEntityCount = world.UpdatedObjects.Count;
-            updatedEntities = new List<BulletXNA.EntityProperties>(world.UpdatedObjects);
-            updatedEntityCount = updatedEntities.Count;
-            world.UpdatedObjects.Clear();
+            
             
 
-            collidersCount = world.UpdatedCollisions.Count;
-            colliders = new List<BulletXNA.CollisionDesc>(world.UpdatedCollisions);
-
-            world.UpdatedCollisions.Clear();
             m_collisionsThisFrame = 0;
             int numManifolds = world.GetDispatcher().GetNumManifolds();
             for (int j = 0; j < numManifolds; j++)
@@ -1493,16 +1683,31 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
             }
 
+            updatedEntityCount = world.LastEntityProperty;
+            updatedEntities = GetBulletSimEntityStruct(BulletXNAEntityStructToByteArray(world.UpdatedObjects, world.LastEntityProperty));
+
+
+
+
+            collidersCount = world.LastCollisionDesc;
+            colliders =
+                GetBulletSimCollisionStruct(BulletXNACollisionStructToByteArray(world.UpdatedCollisions, world.LastCollisionDesc));//new List<BulletXNA.CollisionDesc>(world.UpdatedCollisions);
 
         }
         else
         {
             //if (updatedEntities is null)
-            updatedEntities = new List<BulletXNA.EntityProperties>();
-            updatedEntityCount = 0;
+            //updatedEntities = new List<BulletXNA.EntityProperties>();
+            //updatedEntityCount = 0;
             //if (colliders is null)
-            colliders = new List<BulletXNA.CollisionDesc>();
-            collidersCount = 0;
+            //colliders = new List<BulletXNA.CollisionDesc>();
+            //collidersCount = 0;
+           
+            updatedEntities = new EntityProperties[0];
+
+            
+            colliders = new CollisionDesc[0];
+        
         }
         return numSimSteps;
     }
@@ -1535,22 +1740,23 @@ private sealed class BulletConstraintXNA : BulletConstraint
                                                 point = contact,
                                                 normal = contactNormal
                                             };
-        world.UpdatedCollisions.Add(cDesc);
+        if (world.LastCollisionDesc < world.UpdatedCollisions.Length)
+            world.UpdatedCollisions[world.LastCollisionDesc++] = (cDesc);
         m_collisionsThisFrame++;
 
 
     }
-    private static EntityProperties GetDebugProperties(BulletWorld pWorld, BulletBody pBody)
+    private static EntityProperties GetDebugProperties(BulletWorld pWorld, BulletBody pCollisionObject)
     {
         EntityProperties ent = new EntityProperties();
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        IndexedMatrix transform = body.GetWorldTransform();
-        IndexedVector3 LinearVelocity = body.GetInterpolationLinearVelocity();
-        IndexedVector3 AngularVelocity = body.GetInterpolationAngularVelocity();
+        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)body.GetUserPointer();
+        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);
@@ -1562,19 +1768,19 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override Vector3 GetLocalScaling(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         IndexedVector3 scale = shape.GetLocalScaling();
         return new Vector3(scale.X,scale.Y,scale.Z);
     }
 
     public bool RayCastGround(BulletWorld pWorld, Vector3 _RayOrigin, float pRayHeight, BulletBody NotMe)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         if (world != null)
         {
             if (NotMe is BulletBodyXNA && NotMe.HasPhysicalBody)
             {
-                CollisionObject AvoidBody = ((BulletBodyXNA)NotMe).body;
+                CollisionObject AvoidBody = (NotMe as BulletBodyXNA).body;
                 
                 IndexedVector3 rOrigin = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z);
                 IndexedVector3 rEnd = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z - pRayHeight);
@@ -1594,5 +1800,160 @@ private sealed class BulletConstraintXNA : BulletConstraint
         }
         return false;
     }
+    
+    public static unsafe BulletXNA.CollisionDesc[] GetBulletXNACollisionStruct(byte[] buffer)
+    {
+        int count = buffer.Length/sizeof (BulletXNA.CollisionDesc);
+        BulletXNA.CollisionDesc[] result = new BulletXNA.CollisionDesc[count];
+        BulletXNA.CollisionDesc* ptr;
+        fixed (byte* localBytes = new byte[buffer.Length])
+        {
+            for (int i = 0; i < buffer.Length; i++)
+            {
+                localBytes[i] = buffer[i];
+            }
+            for (int i=0;i<count;i++)
+            {
+                ptr = (BulletXNA.CollisionDesc*) (localBytes + sizeof (BulletXNA.CollisionDesc)*i);
+                result[i] = new BulletXNA.CollisionDesc();
+                result[i] = *ptr;
+            }
+        }
+        return result;
+    }
+
+    public static unsafe CollisionDesc[] GetBulletSimCollisionStruct(byte[] buffer)
+    {
+        int count = buffer.Length / sizeof(CollisionDesc);
+        CollisionDesc[] result = new CollisionDesc[count];
+        CollisionDesc* ptr;
+        fixed (byte* localBytes = new byte[buffer.Length])
+        {
+            for (int i = 0; i < buffer.Length; i++)
+            {
+                localBytes[i] = buffer[i];
+            }
+            for (int i = 0; i < count; i++)
+            {
+                ptr = (CollisionDesc*)(localBytes + sizeof(CollisionDesc) * i);
+                result[i] = new CollisionDesc();
+                result[i] = *ptr;
+            }
+        }
+        return result;
+    }
+    public static unsafe byte[] BulletSimCollisionStructToByteArray(CollisionDesc[] CollisionDescArray, int count)
+    {
+        int arrayLength = CollisionDescArray.Length > count ? count : CollisionDescArray.Length;
+        byte[] byteArray = new byte[sizeof(CollisionDesc) * arrayLength];
+        fixed (CollisionDesc* floatPointer = CollisionDescArray)
+        {
+            fixed (byte* bytePointer = byteArray)
+            {
+                CollisionDesc* read = floatPointer;
+                CollisionDesc* write = (CollisionDesc*)bytePointer;
+                for (int i = 0; i < arrayLength; i++)
+                {
+                    *write++ = *read++;
+                }
+            }
+        }
+        return byteArray;
+    }
+    public static unsafe byte[] BulletXNACollisionStructToByteArray(BulletXNA.CollisionDesc[] CollisionDescArray, int count)
+    {
+        int arrayLength = CollisionDescArray.Length > count ? count : CollisionDescArray.Length;
+        byte[] byteArray = new byte[sizeof(BulletXNA.CollisionDesc) * arrayLength];
+        fixed (BulletXNA.CollisionDesc* floatPointer = CollisionDescArray)
+        {
+            fixed (byte* bytePointer = byteArray)
+            {
+                BulletXNA.CollisionDesc* read = floatPointer;
+                BulletXNA.CollisionDesc* write = (BulletXNA.CollisionDesc*)bytePointer;
+                for (int i = 0; i < arrayLength; i++)
+                {
+                    *write++ = *read++;
+                }
+            }
+        }
+        return byteArray;
+    }
+    public static unsafe BulletXNA.EntityProperties[] GetBulletXNAEntityStruct(byte[] buffer)
+    {
+        int count = buffer.Length / sizeof(BulletXNA.EntityProperties);
+        BulletXNA.EntityProperties[] result = new BulletXNA.EntityProperties[count];
+        BulletXNA.EntityProperties* ptr;
+        fixed (byte* localBytes = new byte[buffer.Length])
+        {
+            for (int i = 0; i < buffer.Length; i++)
+            {
+                localBytes[i] = buffer[i];
+            }
+            for (int i = 0; i < count; i++)
+            {
+                ptr = (BulletXNA.EntityProperties*)(localBytes + sizeof(BulletXNA.EntityProperties) * i);
+                result[i] = new BulletXNA.EntityProperties();
+                result[i] = *ptr;
+            }
+        }
+        return result;
+    }
+
+    public static unsafe EntityProperties[] GetBulletSimEntityStruct(byte[] buffer)
+    {
+        int count = buffer.Length / sizeof(EntityProperties);
+        EntityProperties[] result = new EntityProperties[count];
+        EntityProperties* ptr;
+        fixed (byte* localBytes = new byte[buffer.Length])
+        {
+            for (int i = 0; i < buffer.Length; i++)
+            {
+                localBytes[i] = buffer[i];
+            }
+            for (int i = 0; i < count; i++)
+            {
+                ptr = (EntityProperties*)(localBytes + sizeof(EntityProperties) * i);
+                result[i] = new EntityProperties();
+                result[i] = *ptr;
+            }
+        }
+        return result;
+    }
+    public static unsafe byte[] BulletSimEntityStructToByteArray(EntityProperties[] CollisionDescArray, int count)
+    {
+        int arrayLength = CollisionDescArray.Length > count ? count : CollisionDescArray.Length;
+        byte[] byteArray = new byte[sizeof(EntityProperties) * arrayLength];
+        fixed (EntityProperties* floatPointer = CollisionDescArray)
+        {
+            fixed (byte* bytePointer = byteArray)
+            {
+                EntityProperties* read = floatPointer;
+                EntityProperties* write = (EntityProperties*)bytePointer;
+                for (int i = 0; i < arrayLength; i++)
+                {
+                    *write++ = *read++;
+                }
+            }
+        }
+        return byteArray;
+    }
+    public static unsafe byte[] BulletXNAEntityStructToByteArray(BulletXNA.EntityProperties[] CollisionDescArray, int count)
+    {
+        int arrayLength = CollisionDescArray.Length > count ? count : CollisionDescArray.Length;
+        byte[] byteArray = new byte[sizeof(BulletXNA.EntityProperties) * arrayLength];
+        fixed (BulletXNA.EntityProperties* floatPointer = CollisionDescArray)
+        {
+            fixed (byte* bytePointer = byteArray)
+            {
+                BulletXNA.EntityProperties* read = floatPointer;
+                BulletXNA.EntityProperties* write = (BulletXNA.EntityProperties*)bytePointer;
+                for (int i = 0; i < arrayLength; i++)
+                {
+                    *write++ = *read++;
+                }
+            }
+        }
+        return byteArray;
+    }
 }
 }