12 files changed, 962 insertions, 473 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;
+    }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 478aeab..7603254 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -56,6 +56,7 @@ public sealed class BSCharacter : BSPhysObject
     private int _physicsActorType;
     private bool _isPhysical;
     private bool _flying;
+    private bool _wasWalking;   // 'true' if the avatar was walking/moving last frame
     private bool _setAlwaysRun;
     private bool _throttleUpdates;
     private bool _floatOnWater;
@@ -83,6 +84,7 @@ public sealed class BSCharacter : BSPhysObject
         _position = pos;
 
         _flying = isFlying;
+        _wasWalking = true;     // causes first step to initialize standing
         _orientation = OMV.Quaternion.Identity;
         _velocity = OMV.Vector3.Zero;
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
@@ -198,25 +200,68 @@ public sealed class BSCharacter : BSPhysObject
             // TODO: Decide if the step parameters should be changed depending on the avatar's
             //     state (flying, colliding, ...). There is code in ODE to do this.
 
-            OMV.Vector3 stepVelocity = _velocityMotor.Step(timeStep);
-
-            // If falling, we keep the world's downward vector no matter what the other axis specify.
-            if (!Flying && !IsColliding)
+            // COMMENTARY: when the user is making the avatar walk, except for falling, the velocity
+            //   specified for the avatar is the one that should be used. For falling, if the avatar
+            //   is not flying and is not colliding then it is presumed to be falling and the Z
+            //   component is not fooled with (thus allowing gravity to do its thing).
+            // When the avatar is standing, though, the user has specified a velocity of zero and
+            //   the avatar should be standing. But if the avatar is pushed by something in the world
+            //   (raising elevator platform, moving vehicle, ...) the avatar should be allowed to
+            //   move. Thus, the velocity cannot be forced to zero. The problem is that small velocity
+            //   errors can creap in and the avatar will slowly float off in some direction.
+            // So, the problem is that, when an avatar is standing, we cannot tell creaping error
+            //   from real pushing.OMV.Vector3.Zero;
+            // The code below keeps setting the velocity to zero hoping the world will keep pushing.
+
+            _velocityMotor.Step(timeStep);
+
+            // If we're not supposed to be moving, make sure things are zero.
+            if (_velocityMotor.ErrorIsZero() && _velocityMotor.TargetValue == OMV.Vector3.Zero && IsColliding)
             {
-                stepVelocity.Z = _velocity.Z;
-                // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
+                // The avatar shouldn't be moving
+                _velocityMotor.Zero();
+                ZeroMotion(true /* inTaintTime */);
+
+                // Standing has more friction on the ground
+                if (_currentFriction != BSParam.AvatarStandingFriction)
+                {
+                    _currentFriction = BSParam.AvatarStandingFriction;
+                    PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
+                }
+                DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1}", LocalID, _velocityMotor.TargetValue);
+
+                _wasWalking = false;
             }
+            else
+            {
+                OMV.Vector3 stepVelocity = _velocityMotor.CurrentValue;
+
+                if (_currentFriction != BSParam.AvatarFriction)
+                {
+                    // Probably starting up walking. Set friction to moving friction.
+                    _currentFriction = BSParam.AvatarFriction;
+                    PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
+                }
+
+                // If falling, we keep the world's downward vector no matter what the other axis specify.
+                if (!Flying && !IsColliding)
+                {
+                    stepVelocity.Z = _velocity.Z;
+                    // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
+                }
 
-            // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
-            OMV.Vector3 moveForce = (stepVelocity - _velocity) * Mass;
+                // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
+                OMV.Vector3 moveForce = (stepVelocity - _velocity) * Mass;
 
-            // Should we check for move force being small and forcing velocity to zero?
+                // Should we check for move force being small and forcing velocity to zero?
 
-            // Add special movement force to allow avatars to walk up stepped surfaces.
-            moveForce += WalkUpStairs();
+                // Add special movement force to allow avatars to walk up stepped surfaces.
+                moveForce += WalkUpStairs();
 
-            // DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce);
-            PhysicsScene.PE.ApplyCentralImpulse(PhysBody, moveForce);
+                DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce);
+                PhysicsScene.PE.ApplyCentralImpulse(PhysBody, moveForce);
+                _wasWalking = true;
+            }
         });
     }
 
@@ -559,27 +604,6 @@ public sealed class BSCharacter : BSPhysObject
             PhysicsScene.AssertInTaintTime("BSCharacter.ForceVelocity");
 
             _velocity = value;
-            // Depending on whether the avatar is moving or not, change the friction
-            //    to keep the avatar from slipping around
-            if (_velocity.Length() == 0)
-            {
-                if (_currentFriction != BSParam.AvatarStandingFriction)
-                {
-                    _currentFriction = BSParam.AvatarStandingFriction;
-                    if (PhysBody.HasPhysicalBody)
-                        PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
-                }
-            }
-            else
-            {
-                if (_currentFriction != BSParam.AvatarFriction)
-                {
-                    _currentFriction = BSParam.AvatarFriction;
-                    if (PhysBody.HasPhysicalBody)
-                        PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
-                }
-            }
-
             PhysicsScene.PE.SetLinearVelocity(PhysBody, _velocity);
             PhysicsScene.PE.Activate(PhysBody, true);
         }
@@ -854,7 +878,7 @@ public sealed class BSCharacter : BSPhysObject
         _position = entprop.Position;
         _orientation = entprop.Rotation;
 
-        // Smooth velocity. OpenSimulator is very sensitive to changes in velocity of the avatar
+        // Smooth velocity. OpenSimulator is VERY sensitive to changes in velocity of the avatar
         //    and will send agent updates to the clients if velocity changes by more than
         //    0.001m/s. Bullet introduces a lot of jitter in the velocity which causes many
         //    extra updates.
@@ -875,7 +899,7 @@ public sealed class BSCharacter : BSPhysObject
         CurrentEntityProperties = entprop;
 
         // Tell the linkset about value changes
-        Linkset.UpdateProperties(this, true);
+        Linkset.UpdateProperties(UpdatedProperties.EntPropUpdates, this);
 
         // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
         // base.RequestPhysicsterseUpdate();
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index f2c7cec..dbe44de 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -231,6 +231,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     break;
                 case Vehicle.ANGULAR_MOTOR_DIRECTION:
                     m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
+                    m_angularMotor.Zero();
                     m_angularMotor.SetTarget(m_angularMotorDirection);
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
@@ -264,6 +265,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f);
                     pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f);
                     m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    m_angularMotor.Zero();
                     m_angularMotor.SetTarget(m_angularMotorDirection);
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
@@ -720,10 +722,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         // Since the computation of terrain height can be a little involved, this routine
         //    is used to fetch the height only once for each vehicle simulation step.
+        Vector3 lastRememberedHeightPos;
         private float GetTerrainHeight(Vector3 pos)
         {
-            if ((m_knownHas & m_knownChangedTerrainHeight) == 0)
+            if ((m_knownHas & m_knownChangedTerrainHeight) == 0 || pos != lastRememberedHeightPos)
             {
+                lastRememberedHeightPos = pos;
                 m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
                 m_knownHas |= m_knownChangedTerrainHeight;
             }
@@ -943,10 +947,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // ==================================================================
             // Clamp high or low velocities
             float newVelocityLengthSq = VehicleVelocity.LengthSquared();
-            if (newVelocityLengthSq > 1000f)
+            if (newVelocityLengthSq > BSParam.VehicleMaxLinearVelocity)
             {
                 VehicleVelocity /= VehicleVelocity.Length();
-                VehicleVelocity *= 1000f;
+                VehicleVelocity *= BSParam.VehicleMaxLinearVelocity;
             }
             else if (newVelocityLengthSq < 0.001f)
                 VehicleVelocity = Vector3.Zero;
@@ -957,39 +961,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         public void ComputeLinearVelocity(float pTimestep)
         {
-            Vector3 linearMotorStep = m_linearMotor.Step(pTimestep);
+            // Step the motor from the current value. Get the correction needed this step.
+            Vector3 currentVel = VehicleVelocity * Quaternion.Inverse(VehicleOrientation);
+            Vector3 linearMotorCorrection = m_linearMotor.Step(pTimestep, currentVel);
 
-            // The movement computed in the linear motor is relative to the vehicle
-            //     coordinates. Rotate the movement to world coordinates.
-            Vector3 linearMotorVelocity = linearMotorStep * VehicleOrientation;
+            // Motor is vehicle coordinates. Rotate it to world coordinates
+            Vector3 linearMotorVelocity = linearMotorCorrection * VehicleOrientation;
 
-            // If we're a ground vehicle, don't loose any Z action (like gravity acceleration).
-            float mixFactor = 1f;   // 1 means use all linear motor Z value, 0 means use all existing Z
+            // If we're a ground vehicle, don't add any upward Z movement
             if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
             {
-                if (!Prim.IsColliding)
-                {
-                    // If a ground vehicle and not on the ground, I want gravity effect
-                    mixFactor = 0.2f;
-                }
-            }
-            else
-            {
-                // I'm not a ground vehicle but don't totally loose the effect of the environment
-                mixFactor = 0.8f;
+                if (linearMotorVelocity.Z > 0f)
+                    linearMotorVelocity.Z = 0f;
             }
-            linearMotorVelocity.Z = mixFactor * linearMotorVelocity.Z + (1f - mixFactor) * VehicleVelocity.Z;
-
-            // What we want to contribute to the vehicle's existing velocity
-            Vector3 linearMotorForce = linearMotorVelocity - VehicleVelocity;
-
-            // Act against the inertia of the vehicle
-            linearMotorForce *= m_vehicleMass;
 
-            VehicleAddForceImpulse(linearMotorForce * pTimestep);
+            // Add this correction to the velocity to make it faster/slower.
+            VehicleVelocity += linearMotorVelocity;
 
-            VDetailLog("{0},  MoveLinear,velocity,vehVel={1},step={2},stepVel={3},mix={4},force={5}",
-                        Prim.LocalID, VehicleVelocity, linearMotorStep, linearMotorVelocity, mixFactor, linearMotorForce);
+            VDetailLog("{0},  MoveLinear,velocity,vehVel={1},correction={2},force={3}",
+                        Prim.LocalID, VehicleVelocity, linearMotorCorrection, linearMotorVelocity);
         }
 
         public void ComputeLinearTerrainHeightCorrection(float pTimestep)
@@ -1202,62 +1192,27 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //     set directly on the vehicle.
         private void MoveAngular(float pTimestep)
         {
-            // The user wants this many radians per second angular change?
-            Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
-
-            // ==================================================================
-            // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
-            //    This flag prevents linear deflection parallel to world z-axis. This is useful
-            //    for preventing ground vehicles with large linear deflection, like bumper cars,
-            //    from climbing their linear deflection into the sky. 
-            // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
-            // TODO: This is here because this is where ODE put it but documentation says it
-            //    is a linear effect. Where should this check go?
-            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
-            {
-                angularMotorContribution.X = 0f;
-                angularMotorContribution.Y = 0f;
-                VDetailLog("{0},  MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
-            }
-
-            Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction();
+            // VehicleRotationalVelocity = Vector3.Zero;
 
-            Vector3 deflectionContribution = ComputeAngularDeflection();
+            ComputeAngularTurning(pTimestep);
 
-            Vector3 bankingContribution = ComputeAngularBanking();
+            ComputeAngularVerticalAttraction();
 
-            // ==================================================================
-            m_lastVertAttractor = verticalAttractionContribution;
-
-            m_lastAngularVelocity = angularMotorContribution
-                                    + verticalAttractionContribution
-                                    + deflectionContribution
-                                    + bankingContribution;
+            ComputeAngularDeflection();
 
-            // Add of the above computation are made relative to vehicle coordinates.
-            // Convert to world coordinates.
-            m_lastAngularVelocity *= VehicleOrientation;
+            ComputeAngularBanking();
 
             // ==================================================================
-            // Apply the correction velocity.
-            // TODO: Should this be applied as an angular force (torque)?
-            if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
-            {
-                VehicleRotationalVelocity = m_lastAngularVelocity;
-
-                VDetailLog("{0},  MoveAngular,done,nonZero,angMotorContrib={1},vertAttrContrib={2},bankContrib={3},deflectContrib={4},totalContrib={5}",
-                                    Prim.LocalID,
-                                    angularMotorContribution, verticalAttractionContribution,
-                                    bankingContribution, deflectionContribution,
-                                    m_lastAngularVelocity
-                                    );
-            }
-            else
+            if (VehicleRotationalVelocity.ApproxEquals(Vector3.Zero, 0.01f))
             {
                 // The vehicle is not adding anything angular wise.
                 VehicleRotationalVelocity = Vector3.Zero;
                 VDetailLog("{0},  MoveAngular,done,zero", Prim.LocalID);
             }
+            else
+            {
+                VDetailLog("{0},  MoveAngular,done,nonZero,angVel={1}", Prim.LocalID, VehicleRotationalVelocity);
+            }
 
             // ==================================================================
             //Offset section
@@ -1291,6 +1246,31 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
 
         }
+
+        private void ComputeAngularTurning(float pTimestep)
+        {
+            // The user wants this many radians per second angular change?
+            Vector3 currentAngular = VehicleRotationalVelocity * Quaternion.Inverse(VehicleOrientation);
+            Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep, currentAngular);
+
+            // ==================================================================
+            // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
+            //    This flag prevents linear deflection parallel to world z-axis. This is useful
+            //    for preventing ground vehicles with large linear deflection, like bumper cars,
+            //    from climbing their linear deflection into the sky. 
+            // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
+            // TODO: This is here because this is where ODE put it but documentation says it
+            //    is a linear effect. Where should this check go?
+            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
+            {
+                angularMotorContribution.X = 0f;
+                angularMotorContribution.Y = 0f;
+            }
+
+            VehicleRotationalVelocity += angularMotorContribution * VehicleOrientation;
+            VDetailLog("{0},  MoveAngular,angularTurning,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
+        }
+
         // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
         //      Some vehicles, like boats, should always keep their up-side up. This can be done by
         //      enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to
@@ -1299,13 +1279,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //      and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An
         //      efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an
         //      efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay.
-        public Vector3 ComputeAngularVerticalAttraction()
+        public void ComputeAngularVerticalAttraction()
         {
-            Vector3 ret = Vector3.Zero;
-
             // If vertical attaction timescale is reasonable
             if (enableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
             {
+                Vector3 vertContribution = Vector3.Zero;
+
                 // Take a vector pointing up and convert it from world to vehicle relative coords.
                 Vector3 verticalError = Vector3.UnitZ * VehicleOrientation;
 
@@ -1319,37 +1299,36 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 // Y error means needed rotation around X axis and visa versa.
                 // Since the error goes from zero to one, the asin is the corresponding angle.
-                ret.X = (float)Math.Asin(verticalError.Y);
+                vertContribution.X = (float)Math.Asin(verticalError.Y);
                 // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
-                ret.Y = -(float)Math.Asin(verticalError.X);
+                vertContribution.Y = -(float)Math.Asin(verticalError.X);
 
                 // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
                 if (verticalError.Z < 0f)
                 {
-                    ret.X += PIOverFour;
-                    ret.Y += PIOverFour;
+                    vertContribution.X += PIOverFour;
+                    // vertContribution.Y -= PIOverFour;
                 }
 
-                // 'ret' is now the necessary velocity to correct tilt in one second.
+                // 'vertContrbution' is now the necessary angular correction to correct tilt in one second.
                 //     Correction happens over a number of seconds.
-                Vector3 unscaledContrib = ret;
-                ret /= m_verticalAttractionTimescale;
+                Vector3 unscaledContrib = vertContribution;     // DEBUG DEBUG
+                vertContribution /= m_verticalAttractionTimescale;
+
+                VehicleRotationalVelocity += vertContribution * VehicleOrientation;
 
                 VDetailLog("{0},  MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},ts={4},vertAttr={5}",
-                                Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, m_verticalAttractionTimescale, ret);
+                                Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, m_verticalAttractionTimescale, vertContribution);
             }
-            return ret;
         }
 
-        // Return the angular correction to correct the direction the vehicle is pointing to be
+        // Angular correction to correct the direction the vehicle is pointing to be
         //      the direction is should want to be pointing.
         // The vehicle is moving in some direction and correct its orientation to it is pointing
         //     in that direction.
         // TODO: implement reference frame.
-        public Vector3 ComputeAngularDeflection()
+        public void ComputeAngularDeflection()
         {
-            Vector3 ret = Vector3.Zero;
-
             // Since angularMotorUp and angularDeflection are computed independently, they will calculate
             //     approximately the same X or Y correction. When added together (when contributions are combined)
             //     this creates an over-correction and then wabbling as the target is overshot.
@@ -1357,6 +1336,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
             if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
             {
+                Vector3 deflectContribution = Vector3.Zero;
+
                 // The direction the vehicle is moving
                 Vector3 movingDirection = VehicleVelocity;
                 movingDirection.Normalize();
@@ -1382,18 +1363,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
 
                 // Scale the correction by recovery timescale and efficiency
-                ret = (-deflectionError) * m_angularDeflectionEfficiency;
-                ret /= m_angularDeflectionTimescale;
+                deflectContribution = (-deflectionError) * m_angularDeflectionEfficiency;
+                deflectContribution /= m_angularDeflectionTimescale;
+
+                VehicleRotationalVelocity += deflectContribution * VehicleOrientation;
 
                 VDetailLog("{0},  MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
-                    Prim.LocalID, movingDirection, pointingDirection, deflectionError, ret);
+                    Prim.LocalID, movingDirection, pointingDirection, deflectionError, deflectContribution);
                 VDetailLog("{0},  MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}",
                     Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
             }
-            return ret;
         }
 
-        // Return an angular change to rotate the vehicle around the Z axis when the vehicle
+        // Angular change to rotate the vehicle around the Z axis when the vehicle
         //     is tipped around the X axis.
         // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
         //      The vertical attractor feature must be enabled in order for the banking behavior to
@@ -1424,12 +1406,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //      world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to
         //      bank quickly then give it a banking timescale of about a second or less, otherwise you can
         //      make a sluggish vehicle by giving it a timescale of several seconds. 
-        public Vector3 ComputeAngularBanking()
+        public void ComputeAngularBanking()
         {
-            Vector3 ret = Vector3.Zero;
-
             if (enableAngularBanking && m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
             {
+                Vector3 bankingContribution = Vector3.Zero;
+
                 // Rotate a UnitZ vector (pointing up) to how the vehicle is oriented.
                 // As the vehicle rolls to the right or left, the Y value will increase from
                 //     zero (straight up) to 1 or -1 (full tilt right  or left)
@@ -1446,15 +1428,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 mixedYawAngle = ClampInRange(-20f, mixedYawAngle, 20f);
 
                 // Build the force vector to change rotation from what it is to what it should be
-                ret.Z = -mixedYawAngle;
+                bankingContribution.Z = -mixedYawAngle;
 
                 // Don't do it all at once.
-                ret /= m_bankingTimescale;
+                bankingContribution /= m_bankingTimescale;
+
+                VehicleRotationalVelocity += bankingContribution * VehicleOrientation;
 
                 VDetailLog("{0},  MoveAngular,Banking,rollComp={1},speed={2},rollComp={3},yAng={4},mYAng={5},ret={6}",
-                            Prim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, ret);
+                            Prim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, bankingContribution);
             }
-            return ret;
         }
 
         // This is from previous instantiations of XXXDynamics.cs.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index cbd160f..580ea4e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -252,7 +252,7 @@ public abstract class BSLinkset
     //      of the linkset is received.
     // Passed flag is update came from physics engine (true) or the user (false).
     // Called at taint-time!!
-    public abstract void UpdateProperties(BSPhysObject physObject, bool physicalUpdate);
+    public abstract void UpdateProperties(UpdatedProperties whichUpdated, BSPhysObject physObject);
 
     // Routine used when rebuilding the body of the root of the linkset
     // Destroy all the constraints have have been made to root.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 8c9a774..27d8ad0 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -51,6 +51,21 @@ sealed class BSLinksetCompoundInfo : BSLinksetInfo
         OffsetFromCenterOfMass = p;
         OffsetRot = r;
     }
+    // 'centerDisplacement' is the distance from the root the the center-of-mass (Bullet 'zero' of the shape)
+    public BSLinksetCompoundInfo(int indx, BSPhysObject root, BSPhysObject child, OMV.Vector3 centerDisplacement)
+    {
+        // Each child position and rotation is given relative to the center-of-mass.
+        OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(root.RawOrientation);
+        OMV.Vector3 displacementFromRoot = (child.RawPosition - root.RawPosition) * invRootOrientation;
+        OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
+        OMV.Quaternion displacementRot = child.RawOrientation * invRootOrientation;
+
+        // Save relative position for recomputing child's world position after moving linkset.
+        Index = indx;
+        OffsetFromRoot = displacementFromRoot;
+        OffsetFromCenterOfMass = displacementFromCOM;
+        OffsetRot = displacementRot;
+    }
     public override void Clear()
     {
         Index = 0;
@@ -182,24 +197,71 @@ public sealed class BSLinksetCompound : BSLinkset
 
     // 'physicalUpdate' is true if these changes came directly from the physics engine. Don't need to rebuild then.
     // Called at taint-time.
-    public override void UpdateProperties(BSPhysObject updated, bool physicalUpdate)
+    public override void UpdateProperties(UpdatedProperties whichUpdated, BSPhysObject updated)
     {
         // The user moving a child around requires the rebuilding of the linkset compound shape
         // One problem is this happens when a border is crossed -- the simulator implementation
-        //    is to store the position into the group which causes the move of the object
+        //    stores the position into the group which causes the move of the object
         //    but it also means all the child positions get updated.
         //    What would cause an unnecessary rebuild so we make sure the linkset is in a
         //    region before bothering to do a rebuild.
-        if (!IsRoot(updated) 
-                && !physicalUpdate 
-                && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
+        if (!IsRoot(updated) && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
         {
-            // TODO: replace this with are calculation of the child prim's orientation and pos.
-            // TODO: for the moment, don't rebuild the compound shape.
-            //    This is often just the car turning its wheels. When we can just reorient the one
-            //    member shape of the compound shape, the overhead of rebuilding won't be a problem.
-            // updated.LinksetInfo = null;
-            // ScheduleRebuild(updated);
+            // If a child of the linkset is updating only the position or rotation, that can be done
+            //    without rebuilding the linkset.
+            // If a handle for the child can be fetch, we update the child here. If a rebuild was
+            //    scheduled by someone else, the rebuild will just replace this setting.
+
+            bool updatedChild = false;
+            // Anything other than updating position or orientation usually means a physical update
+            //     and that is caused by us updating the object.
+            if ((whichUpdated & ~(UpdatedProperties.Position | UpdatedProperties.Orientation)) == 0)
+            {
+                // Gather the child info. It might not be there if the linkset is in transition.
+                BSLinksetCompoundInfo lsi = updated.LinksetInfo as BSLinksetCompoundInfo;
+                if (LinksetRoot.PhysShape.HasPhysicalShape && lsi != null)
+                {
+                    if (PhysicsScene.PE.IsCompound(LinksetRoot.PhysShape))
+                    {
+                        BulletShape linksetChildShape = PhysicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape, lsi.Index);
+                        if (linksetChildShape.HasPhysicalShape)
+                        {
+                            // Compute the offset from the center-of-gravity
+                            BSLinksetCompoundInfo newLsi = new BSLinksetCompoundInfo(lsi.Index, LinksetRoot, updated, LinksetRoot.PositionDisplacement);
+                            PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, lsi.Index,
+                                                                        newLsi.OffsetFromCenterOfMass,
+                                                                        newLsi.OffsetRot,
+                                                                        true /* shouldRecalculateLocalAabb */);
+                            DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1}newLsi={2}",
+                                                                        updated.LocalID, whichUpdated, newLsi);
+                            updated.LinksetInfo = newLsi;
+                            updatedChild = true;
+                        }
+                        else    // DEBUG DEBUG
+                        {       // DEBUG DEBUG
+                            DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}",
+                                                                        updated.LocalID, linksetChildShape);
+                        }       // DEBUG DEBUG
+                    }
+                    else    // DEBUG DEBUG
+                    {       // DEBUG DEBUG
+                        DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,notCompound", updated.LocalID);
+                    }       // DEBUG DEBUG
+                }
+                else    // DEBUG DEBUG
+                {       // DEBUG DEBUG
+                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,rootPhysShape={1},lsi={2}",
+                                updated.LocalID, LinksetRoot.PhysShape, lsi == null ? "NULL" : lsi.ToString());
+                }       // DEBUG DEBUG
+                if (!updatedChild)
+                {
+                    // If couldn't do the individual child, the linkset needs a rebuild to incorporate the new child info.
+                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild.schedulingRebuild,whichUpdated={1}",
+                                                                    updated.LocalID, whichUpdated);
+                    updated.LinksetInfo = null; // setting to 'null' causes relative position to be recomputed.
+                    ScheduleRebuild(updated);
+                }
+            }
         }
     }
 
@@ -372,15 +434,7 @@ public sealed class BSLinksetCompound : BSLinkset
                     BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo;
                     if (lci == null)
                     {
-                        // Each child position and rotation is given relative to the center-of-mass.
-                        OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
-                        OMV.Vector3 displacementFromRoot = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
-                        OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
-                        OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
-
-                        // Save relative position for recomputing child's world position after moving linkset.
-                        lci = new BSLinksetCompoundInfo(memberIndex, displacementFromCOM, displacementRot);
-                        lci.OffsetFromRoot = displacementFromRoot;
+                        lci = new BSLinksetCompoundInfo(memberIndex, LinksetRoot, cPrim, centerDisplacement);
                         cPrim.LinksetInfo = lci;
                         DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci);
                     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index d0b2a56..89f186c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -83,7 +83,7 @@ public sealed class BSLinksetConstraints : BSLinkset
     }
 
     // Called at taint-time!!
-    public override void UpdateProperties(BSPhysObject updated, bool inTaintTime)
+    public override void UpdateProperties(UpdatedProperties whichUpdated, BSPhysObject pObj)
     {
         // Nothing to do for constraints on property updates
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index 6d0db2e..9501e2d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -138,7 +138,8 @@ public class BSVMotor : BSMotor
         CurrentValue = TargetValue = Vector3.Zero;
     }
 
-    // Compute the next step and return the new current value
+    // Compute the next step and return the new current value.
+    // Returns the correction needed to move 'current' to 'target'.
     public virtual Vector3 Step(float timeStep)
     {
         if (!Enabled) return TargetValue;
@@ -148,9 +149,10 @@ public class BSVMotor : BSMotor
 
         Vector3 correction = Vector3.Zero;
         Vector3 error = TargetValue - CurrentValue;
+        LastError = error;
         if (!ErrorIsZero(error))
         {
-            correction = Step(timeStep, error);
+            correction = StepError(timeStep, error);
 
             CurrentValue += correction;
 
@@ -187,20 +189,31 @@ public class BSVMotor : BSMotor
         else
         {
             // Difference between what we have and target is small. Motor is done.
+            if (TargetValue.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
+            {
+                // The target can step down to nearly zero but not get there.  If close to zero
+                //     it is really zero.
+                TargetValue = Vector3.Zero;
+            }
             CurrentValue = TargetValue;
-            MDetailLog("{0},  BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
-                        BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
+            MDetailLog("{0},  BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}",
+                        BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue);
         }
 
-        return CurrentValue;
+        return correction;
+    }
+    // version of step that sets the current value before doing the step
+    public virtual Vector3 Step(float timeStep, Vector3 current)
+    {
+        CurrentValue = current;
+        return Step(timeStep);
     }
-    public virtual Vector3 Step(float timeStep, Vector3 error)
+    public virtual Vector3 StepError(float timeStep, Vector3 error)
     {
         if (!Enabled) return Vector3.Zero;
 
-        LastError = error;
         Vector3 returnCorrection = Vector3.Zero;
-        if (!ErrorIsZero())
+        if (!ErrorIsZero(error))
         {
             // correction =  error / secondsItShouldTakeToCorrect
             Vector3 correctionAmount;
@@ -302,9 +315,10 @@ public class BSFMotor : BSMotor
 
         float correction = 0f;
         float error = TargetValue - CurrentValue;
+        LastError = error;
         if (!ErrorIsZero(error))
         {
-            correction = Step(timeStep, error);
+            correction = StepError(timeStep, error);
 
             CurrentValue += correction;
 
@@ -339,6 +353,12 @@ public class BSFMotor : BSMotor
         else
         {
             // Difference between what we have and target is small. Motor is done.
+            if (Util.InRange<float>(TargetValue, -ErrorZeroThreshold, ErrorZeroThreshold))
+            {
+                // The target can step down to nearly zero but not get there.  If close to zero
+                //     it is really zero.
+                TargetValue = 0f;
+            }
             CurrentValue = TargetValue;
             MDetailLog("{0},  BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
                         BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
@@ -347,13 +367,12 @@ public class BSFMotor : BSMotor
         return CurrentValue;
     }
 
-    public virtual float Step(float timeStep, float error)
+    public virtual float StepError(float timeStep, float error)
     {
         if (!Enabled) return 0f;
 
-        LastError = error;
         float returnCorrection = 0f;
-        if (!ErrorIsZero())
+        if (!ErrorIsZero(error))
         {
             // correction =  error / secondsItShouldTakeToCorrect
             float correctionAmount;
@@ -440,8 +459,8 @@ public class BSPIDVMotor : BSVMotor
         }
     }
 
-    // Ignore Current and Target Values and just advance the PID computation on this error.
-    public override Vector3 Step(float timeStep, Vector3 error)
+    // Advance the PID computation on this error.
+    public override Vector3 StepError(float timeStep, Vector3 error)
     {
         if (!Enabled) return Vector3.Zero;
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index 3e80aa4..da7438a 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -45,6 +45,9 @@ public static class BSParam
 
     public static float MinimumObjectMass { get; private set; }
     public static float MaximumObjectMass { get; private set; }
+    public static float MaxLinearVelocity { get; private set; }
+    public static float MaxAngularVelocity { get; private set; }
+    public static float MaxAddForceMagnitude { get; private set; }
 
     public static float LinearDamping { get; private set; }
     public static float AngularDamping { get; private set; }
@@ -79,6 +82,8 @@ public static class BSParam
         public static float AvatarStepApproachFactor { get; private set; }
         public static float AvatarStepForceFactor { get; private set; }
 
+    public static float VehicleMaxLinearVelocity { get; private set; }
+    public static float VehicleMaxAngularVelocity { get; private set; }
     public static float VehicleAngularDamping { get; private set; }
     public static float VehicleDebuggingEnabled { get; private set; }
 
@@ -103,7 +108,6 @@ public static class BSParam
     public const float MaxDensity = 22587f;
     public const float MinRestitution = 0f;
     public const float MaxRestitution = 1f;
-    public const float MaxAddForceMagnitude = 20f;
 
     // ===========================================================================
     public delegate void ParamUser(BSScene scene, IConfig conf, string paramName, float val);
@@ -232,11 +236,7 @@ public static class BSParam
             (s,cf,p,v) => { s.m_maxUpdatesPerFrame = cf.GetInt(p, (int)v); },
             (s) => { return (float)s.m_maxUpdatesPerFrame; },
             (s,p,l,v) => { s.m_maxUpdatesPerFrame = (int)v; } ),
-        new ParameterDefn("MaxTaintsToProcessPerStep", "Number of update taints to process before each simulation step",
-            500f,
-            (s,cf,p,v) => { s.m_taintsToProcessPerStep = cf.GetInt(p, (int)v); },
-            (s) => { return (float)s.m_taintsToProcessPerStep; },
-            (s,p,l,v) => { s.m_taintsToProcessPerStep = (int)v; } ),
+
         new ParameterDefn("MinObjectMass", "Minimum object mass (0.0001)",
             0.0001f,
             (s,cf,p,v) => { MinimumObjectMass = cf.GetFloat(p, v); },
@@ -247,6 +247,22 @@ public static class BSParam
             (s,cf,p,v) => { MaximumObjectMass = cf.GetFloat(p, v); },
             (s) => { return (float)MaximumObjectMass; },
             (s,p,l,v) => { MaximumObjectMass = v; } ),
+        new ParameterDefn("MaxLinearVelocity", "Maximum velocity magnitude that can be assigned to an object",
+            1000.0f,
+            (s,cf,p,v) => { MaxLinearVelocity = cf.GetFloat(p, v); },
+            (s) => { return (float)MaxLinearVelocity; },
+            (s,p,l,v) => { MaxLinearVelocity = v; } ),
+        new ParameterDefn("MaxAngularVelocity", "Maximum rotational velocity magnitude that can be assigned to an object",
+            1000.0f,
+            (s,cf,p,v) => { MaxAngularVelocity = cf.GetFloat(p, v); },
+            (s) => { return (float)MaxAngularVelocity; },
+            (s,p,l,v) => { MaxAngularVelocity = v; } ),
+        // LL documentation says thie number should be 20f for llApplyImpulse and 200f for llRezObject
+        new ParameterDefn("MaxAddForceMagnitude", "Maximum force that can be applied by llApplyImpulse (SL says 20f)",
+            20000.0f,
+            (s,cf,p,v) => { MaxAddForceMagnitude = cf.GetFloat(p, v); },
+            (s) => { return (float)MaxAddForceMagnitude; },
+            (s,p,l,v) => { MaxAddForceMagnitude = v; } ),
 
         new ParameterDefn("PID_D", "Derivitive factor for motion smoothing",
             2200f,
@@ -423,8 +439,18 @@ public static class BSParam
             (s) => { return AvatarStepForceFactor; },
             (s,p,l,v) => { AvatarStepForceFactor = v; } ),
 
+        new ParameterDefn("VehicleMaxLinearVelocity", "Maximum velocity magnitude that can be assigned to a vehicle",
+            1000.0f,
+            (s,cf,p,v) => { VehicleMaxLinearVelocity = cf.GetFloat(p, v); },
+            (s) => { return (float)VehicleMaxLinearVelocity; },
+            (s,p,l,v) => { VehicleMaxLinearVelocity = v; } ),
+        new ParameterDefn("VehicleMaxAngularVelocity", "Maximum rotational velocity magnitude that can be assigned to a vehicle",
+            12.0f,
+            (s,cf,p,v) => { VehicleMaxAngularVelocity = cf.GetFloat(p, v); },
+            (s) => { return (float)VehicleMaxAngularVelocity; },
+            (s,p,l,v) => { VehicleMaxAngularVelocity = v; } ),
         new ParameterDefn("VehicleAngularDamping", "Factor to damp vehicle angular movement per second (0.0 - 1.0)",
-            0.95f,
+            0.0f,
             (s,cf,p,v) => { VehicleAngularDamping = cf.GetFloat(p, v); },
             (s) => { return VehicleAngularDamping; },
             (s,p,l,v) => { VehicleAngularDamping = v; } ),
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 5353c75..027c786 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -55,6 +55,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
  * BS.ApplyCentralForce BS.ApplyTorque              
  */
 
+// Flags used to denote which properties updates when making UpdateProperties calls to linksets, etc.
+public enum UpdatedProperties : uint
+{
+    Position                = 1 << 0,
+    Orientation             = 1 << 1,
+    Velocity                = 1 << 2,
+    Acceleration            = 1 << 3,
+    RotationalVelocity      = 1 << 4,
+    EntPropUpdates          = Position | Orientation | Velocity | Acceleration | RotationalVelocity,
+}
 public abstract class BSPhysObject : PhysicsActor
 {
     protected BSPhysObject()
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index aaa6fe5..e6b8507 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -311,13 +311,14 @@ public sealed class BSPrim : BSPhysObject
             _position = value;
             PositionSanityCheck(false);
 
-            // A linkset might need to know if a component information changed.
-            Linkset.UpdateProperties(this, false);
-
             PhysicsScene.TaintedObject("BSPrim.setPosition", delegate()
             {
                 DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 ForcePosition = _position;
+
+                // A linkset might need to know if a component information changed.
+                Linkset.UpdateProperties(UpdatedProperties.Position, this);
+
             });
         }
     }
@@ -682,12 +683,13 @@ public sealed class BSPrim : BSPhysObject
                 return;
             _orientation = value;
 
-            // A linkset might need to know if a component information changed.
-            Linkset.UpdateProperties(this, false);
-
             PhysicsScene.TaintedObject("BSPrim.setOrientation", delegate()
             {
                 ForceOrientation = _orientation;
+
+                // A linkset might need to know if a component information changed.
+                Linkset.UpdateProperties(UpdatedProperties.Orientation, this);
+
             });
         }
     }
@@ -989,10 +991,10 @@ public sealed class BSPrim : BSPhysObject
         }
         set {
             _rotationalVelocity = value;
+            Util.ClampV(_rotationalVelocity, BSParam.MaxAngularVelocity);
             // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
             PhysicsScene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
             {
-                DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
                 ForceRotationalVelocity = _rotationalVelocity;
             });
         }
@@ -1005,7 +1007,9 @@ public sealed class BSPrim : BSPhysObject
             _rotationalVelocity = value;
             if (PhysBody.HasPhysicalBody)
             {
+                DetailLog("{0},BSPrim.ForceRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
                 PhysicsScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity);
+                // PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity);
                 ActivateIfPhysical(false);
             }
         }
@@ -1082,7 +1086,7 @@ public sealed class BSPrim : BSPhysObject
                     OMV.Vector3 origPosition = RawPosition;     // DEBUG DEBUG (for printout below)
 
                     // 'movePosition' is where we'd like the prim to be at this moment.
-                    OMV.Vector3 movePosition = _targetMotor.Step(timeStep);
+                    OMV.Vector3 movePosition = RawPosition + _targetMotor.Step(timeStep);
 
                     // If we are very close to our target, turn off the movement motor.
                     if (_targetMotor.ErrorIsZero())
@@ -1193,10 +1197,14 @@ public sealed class BSPrim : BSPhysObject
     public override float APIDDamping { set { return; } }
 
     public override void AddForce(OMV.Vector3 force, bool pushforce) {
+        // Per documentation, max force is limited.
+        OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
+
         // Since this force is being applied in only one step, make this a force per second.
-        OMV.Vector3 addForce = force / PhysicsScene.LastTimeStep;
-        AddForce(addForce, pushforce, false);
+        addForce /= PhysicsScene.LastTimeStep;
+        AddForce(addForce, pushforce, false /* inTaintTime */);
     }
+
     // Applying a force just adds this to the total force on the object.
     // This added force will only last the next simulation tick.
     public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
@@ -1205,9 +1213,9 @@ public sealed class BSPrim : BSPhysObject
         {
             if (force.IsFinite())
             {
-                OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
                 // DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce);
 
+                OMV.Vector3 addForce = force;
                 PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
                 {
                     // Bullet adds this central force to the total force for this tick
@@ -1642,7 +1650,7 @@ public sealed class BSPrim : BSPhysObject
             // TODO: handle physics introduced by Bullet with computed vehicle physics.
             if (_vehicle.IsActive)
             {
-                entprop.RotationalVelocity = OMV.Vector3.Zero;
+                // entprop.RotationalVelocity = OMV.Vector3.Zero;
             }
 
             // Assign directly to the local variables so the normal set actions do not happen
@@ -1681,7 +1689,7 @@ public sealed class BSPrim : BSPhysObject
              */
 
         // The linkset implimentation might want to know about this.
-        Linkset.UpdateProperties(this, true);
+        Linkset.UpdateProperties(UpdatedProperties.EntPropUpdates, this);
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index e0b4992..8075b73 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -81,7 +81,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     internal long m_simulationStep = 0;
     internal float NominalFrameRate { get; set; }
     public long SimulationStep { get { return m_simulationStep; } }
-    internal int m_taintsToProcessPerStep;
     internal float LastTimeStep { get; private set; }
 
     // Physical objects can register for prestep or poststep events
@@ -847,8 +846,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     #endregion // Taints
 
-    #region INI and command line parameter processing
-
     #region IPhysicsParameters
     // Get the list of parameters this physics engine supports
     public PhysParameterEntry[] GetParameterList()
@@ -945,8 +942,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     #endregion IPhysicsParameters
 
-    #endregion Runtime settable parameters
-
     // Invoke the detailed logger and output something if it's enabled.
     public void DetailLog(string msg, params Object[] args)
     {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
index 9bfec19..41bab26 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -1,23 +1,20 @@
 CURRENT PRIORITIES
 =================================================
-Mantis 6040 script http://opensimulator.org/mantis/view.php?id=6040
-        Msg Kayaker on OSGrid when working
+Child movement in linkset (don't rebuild linkset)
+Vehicle angular vertical attraction
+vehicle angular banking
+Center-of-gravity
+Vehicle angular deflection
+        Preferred orientation angular correction fix
+when should angular and linear motor targets be zeroed? when selected?
+        Need a vehicle.clear()? Or an 'else' in prestep if not physical.
 Teravus llMoveToTarget script debug
         Mixing of hover, buoyancy/gravity, moveToTarget, into one force
-Boats floating at proper level
 Nebadon vehicles turning funny in arena
 limitMotorUp calibration (more down?)
 llRotLookAt
 llLookAt
-Vehicle angular vertical attraction
-Vehicle angular deflection
-        Preferred orientation angular correction fix
-vehicle angular banking
 Avatars walking up stairs (HALF DONE)
-    Radius of the capsule affects ability to climb edges.
-Vehicle movement on terrain smoothness
-When is force introduced by SetForce removed? The prestep action could go forever.
-Boats float low in the water (DONE)
 Avatar movement
         flying into a wall doesn't stop avatar who keeps appearing to move through the obstacle (DONE)
         walking up stairs is not calibrated correctly (stairs out of Kepler cabin)
@@ -73,8 +70,12 @@ Incorporate inter-relationship of angular corrections. For instance, angularDefl
 
 GENERAL TODO LIST:
 =================================================
+llMoveToTarget objects are not effected by gravity until target is removed.
 Implement llSetPhysicalMaterial.
+        extend it with Center-of-mass, rolling friction, density
 Implement llSetForceAndTorque.
+Change BSPrim.moveToTarget to used forces rather than changing position
+        Changing position allows one to move through walls
 Implement an avatar mesh shape. The Bullet capsule is way too limited.
         Consider just hand creating a vertex/index array in a new BSShapeAvatar.
 Verify/fix phantom, volume-detect objects do not fall to infinity. Should stop at terrain.
@@ -310,4 +311,12 @@ Remove HeightmapInfo from terrain specification (DONE)
     Since C++ code does not need terrain height, this structure et al are not needed.
 Surfboard go wonky when turning (DONE)
         Angular motor direction is global coordinates rather than local coordinates?
-        (Resolution: made angular motor direction correct coordinate system)
\ No newline at end of file
+        (Resolution: made angular motor direction correct coordinate system)
+Mantis 6040 script http://opensimulator.org/mantis/view.php?id=6040 (DONE)
+        Msg Kayaker on OSGrid when working
+        (Resolution: LINEAR_DIRECTION is in vehicle coords. Test script does the
+        same in SL as in OS/BulletSim)
+Boats float low in the water (DONE)
+Boats floating at proper level (DONE)
+When is force introduced by SetForce removed? The prestep action could go forever. (DONE)
+        (Resolution: setForce registers a prestep action which keeps applying the force)
\ No newline at end of file