13 files changed, 1072 insertions, 572 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/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
index bc163eb..2828cab 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
@@ -140,6 +140,25 @@ public struct EntityProperties
     public Vector3 Velocity;
     public Vector3 Acceleration;
     public Vector3 RotationalVelocity;
+
+    public override string ToString()
+    {
+        StringBuilder buff = new StringBuilder();
+        buff.Append("<i=");
+        buff.Append(ID.ToString());
+        buff.Append(",p=");
+        buff.Append(Position.ToString());
+        buff.Append(",r=");
+        buff.Append(Rotation.ToString());
+        buff.Append(",v=");
+        buff.Append(Velocity.ToString());
+        buff.Append(",a=");
+        buff.Append(Acceleration.ToString());
+        buff.Append(",rv=");
+        buff.Append(RotationalVelocity.ToString());
+        buff.Append(">");
+        return buff.ToString();
+    }
 }
 
 // Format of this structure must match the definition in the C++ code
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 87a06c1..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;
 
-            // '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;
+                if (_currentFriction != BSParam.AvatarFriction)
+                {
+                    // Probably starting up walking. Set friction to moving friction.
+                    _currentFriction = BSParam.AvatarFriction;
+                    PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
+                }
 
-            // Should we check for move force being small and forcing velocity to zero?
+                // 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;
 
-            // Add special movement force to allow avatars to walk up stepped surfaces.
-            moveForce += WalkUpStairs();
+                // Should we check for move force being small and forcing velocity to zero?
 
-            // DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce);
-            PhysicsScene.PE.ApplyCentralImpulse(PhysBody, moveForce);
+                // 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);
+                _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);
         }
@@ -652,6 +676,9 @@ public sealed class BSCharacter : BSPhysObject
     public override bool IsStatic {
         get { return false; }
     }
+    public override bool IsPhysicallyActive {
+        get { return true; }
+    }
     public override bool Flying {
         get { return _flying; }
         set {
@@ -850,7 +877,14 @@ public sealed class BSCharacter : BSPhysObject
     {
         _position = entprop.Position;
         _orientation = entprop.Rotation;
-        _velocity = entprop.Velocity;
+
+        // 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.
+        if (!entprop.Velocity.ApproxEquals(_velocity, 0.1f))
+            _velocity = entprop.Velocity;
+
         _acceleration = entprop.Acceleration;
         _rotationalVelocity = entprop.RotationalVelocity;
 
@@ -865,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 6601479..fe7891e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -35,6 +35,7 @@ using System.Collections.Generic;
 using System.Reflection;
 using System.Runtime.InteropServices;
 using OpenMetaverse;
+using OpenSim.Framework;
 using OpenSim.Region.Physics.Manager;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
@@ -154,7 +155,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // Return 'true' if this vehicle is doing vehicle things
         public bool IsActive
         {
-            get { return (Type != Vehicle.TYPE_NONE && !Prim.IsStatic); }
+            get { return (Type != Vehicle.TYPE_NONE && Prim.IsPhysicallyActive); }
         }
 
         #region Vehicle parameter setting
@@ -230,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:
@@ -263,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:
@@ -687,7 +690,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // Bullet does a bunch of smoothing for changing parameters.
                     //    Since the vehicle is demanding this setting, we override Bullet's smoothing
                     //    by telling Bullet the value was the same last time.
-                    PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, m_knownVelocity);
+                    // PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, m_knownVelocity);
                 }
 
                 if ((m_knownChanged & m_knownChangedForce) != 0)
@@ -699,7 +702,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
                 {
                     Prim.ForceRotationalVelocity = m_knownRotationalVelocity;
-                    PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity);
+                    // PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity);
                 }
 
                 if ((m_knownChanged & m_knownChangedRotationalImpulse) != 0)
@@ -719,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;
             }
@@ -942,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;
@@ -956,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 linearMotorCorrectionV = 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 linearMotorVelocityW = linearMotorCorrectionV * 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 (linearMotorVelocityW.Z > 0f)
+                    linearMotorVelocityW.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 += linearMotorVelocityW;
 
-            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, linearMotorCorrectionV, linearMotorVelocityW);
         }
 
         public void ComputeLinearTerrainHeightCorrection(float pTimestep)
@@ -1132,8 +1123,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //    a downward raycast to find what is below.
         public void ComputeLinearMotorUp(float pTimestep)
         {
-            Vector3 ret = Vector3.Zero;
-
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
                 // This code tries to decide if the object is not on the ground and then pushing down
@@ -1201,62 +1190,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);
+            // VehicleRotationalVelocity = Vector3.Zero;
 
-            // ==================================================================
-            // 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();
+            ComputeAngularTurning(pTimestep);
 
-            Vector3 deflectionContribution = ComputeAngularDeflection();
+            ComputeAngularVerticalAttraction();
 
-            Vector3 bankingContribution = ComputeAngularBanking();
+            ComputeAngularDeflection();
 
-            // ==================================================================
-            m_lastVertAttractor = verticalAttractionContribution;
-
-            m_lastAngularVelocity = angularMotorContribution
-                                    + verticalAttractionContribution
-                                    + deflectionContribution
-                                    + bankingContribution;
-
-            // 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
@@ -1290,6 +1244,31 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
 
         }
+
+        private void ComputeAngularTurning(float pTimestep)
+        {
+            // The user wants this many radians per second angular change?
+            Vector3 currentAngularV = VehicleRotationalVelocity * Quaternion.Inverse(VehicleOrientation);
+            Vector3 angularMotorContributionV = m_angularMotor.Step(pTimestep, currentAngularV);
+
+            // ==================================================================
+            // 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)
+            {
+                angularMotorContributionV.X = 0f;
+                angularMotorContributionV.Y = 0f;
+            }
+
+            VehicleRotationalVelocity += angularMotorContributionV * VehicleOrientation;
+            VDetailLog("{0},  MoveAngular,angularTurning,angularMotorContrib={1}", Prim.LocalID, angularMotorContributionV);
+        }
+
         // 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
@@ -1298,13 +1277,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 vertContributionV = Vector3.Zero;
+
                 // Take a vector pointing up and convert it from world to vehicle relative coords.
                 Vector3 verticalError = Vector3.UnitZ * VehicleOrientation;
 
@@ -1318,37 +1297,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);
+                vertContributionV.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);
+                vertContributionV.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;
+                    vertContributionV.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 = vertContributionV;     // DEBUG DEBUG
+                vertContributionV /= m_verticalAttractionTimescale;
+
+                VehicleRotationalVelocity += vertContributionV * 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, vertContributionV);
             }
-            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.
@@ -1356,6 +1334,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
             if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
             {
+                Vector3 deflectContributionV = Vector3.Zero;
+
                 // The direction the vehicle is moving
                 Vector3 movingDirection = VehicleVelocity;
                 movingDirection.Normalize();
@@ -1381,18 +1361,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;
+                deflectContributionV = (-deflectionError) * m_angularDeflectionEfficiency;
+                deflectContributionV /= m_angularDeflectionTimescale;
+
+                VehicleRotationalVelocity += deflectContributionV * VehicleOrientation;
 
                 VDetailLog("{0},  MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
-                    Prim.LocalID, movingDirection, pointingDirection, deflectionError, ret);
+                    Prim.LocalID, movingDirection, pointingDirection, deflectionError, deflectContributionV);
                 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
@@ -1423,12 +1404,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 bankingContributionV = 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)
@@ -1445,15 +1426,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;
+                bankingContributionV.Z = -mixedYawAngle;
 
                 // Don't do it all at once.
-                ret /= m_bankingTimescale;
+                bankingContributionV /= m_bankingTimescale;
+
+                VehicleRotationalVelocity += bankingContributionV * 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, bankingContributionV);
             }
-            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..1e3e5d8 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.
@@ -261,11 +261,6 @@ public abstract class BSLinkset
     // Called at taint-time!!
     public abstract bool RemoveBodyDependencies(BSPrim child);
 
-    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
-    //     this routine will restore the removed constraints.
-    // Called at taint-time!!
-    public abstract void RestoreBodyDependencies(BSPrim child);
-
     // ================================================================
     protected virtual float ComputeLinksetMass()
     {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 8c9a774..2c8dd23 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);
+                }
+            }
         }
     }
 
@@ -228,13 +290,6 @@ public sealed class BSLinksetCompound : BSLinkset
         return ret;
     }
 
-    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
-    //     this routine will restore the removed constraints.
-    // Called at taint-time!!
-    public override void RestoreBodyDependencies(BSPrim child)
-    {
-    }
-
     // When the linkset is built, the child shape is added to the compound shape relative to the
     //    root shape. The linkset then moves around but this does not move the actual child
     //    prim. The child prim's location must be recomputed based on the location of the root shape.
@@ -322,7 +377,7 @@ public sealed class BSLinksetCompound : BSLinkset
     // Constraint linksets are rebuilt every time.
     // Note that this works for rebuilding just the root after a linkset is taken apart.
     // Called at taint time!!
-    private bool disableCOM = true;     // disable until we get this debugged
+    private bool disableCOM = false;     // disable until we get this debugged
     private void RecomputeLinksetCompound()
     {
         try
@@ -345,12 +400,16 @@ public sealed class BSLinksetCompound : BSLinkset
             }                                           // DEBUG DEBUG
             else
             {
-                centerOfMass = ComputeLinksetGeometricCenter();
-                centerDisplacement = centerOfMass - LinksetRoot.RawPosition;
+                centerOfMass = ComputeLinksetCenterOfMass();
+                // 'centerDisplacement' is the value to *add* to all the shape offsets
+                centerDisplacement = LinksetRoot.RawPosition - centerOfMass;
 
                 // Since we're displacing the center of the shape, we need to move the body in the world
                 LinksetRoot.PositionDisplacement = centerDisplacement;
 
+                // This causes the root prim position to be set properly based on the new PositionDisplacement
+                LinksetRoot.ForcePosition = LinksetRoot.RawPosition;
+                // Update the local transform for the root child shape so it is offset from the <0,0,0> which is COM
                 PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, -centerDisplacement, OMV.Quaternion.Identity, false);
                 DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}",
                                         LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement);
@@ -372,15 +431,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);
                     }
@@ -390,7 +441,7 @@ public sealed class BSLinksetCompound : BSLinkset
 
                     if (cPrim.PhysShape.isNativeShape)
                     {
-                        // A native shape is turning into a hull collision shape because native
+                        // A native shape is turned into a hull collision shape because native
                         //    shapes are not shared so we have to hullify it so it will be tracked
                         //    and freed at the correct time. This also solves the scaling problem
                         //    (native shapes scaled but hull/meshes are assumed to not be).
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index d0b2a56..3011465 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
     }
@@ -110,14 +110,6 @@ public sealed class BSLinksetConstraints : BSLinkset
         return ret;
     }
 
-    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
-    // this routine will restore the removed constraints.
-    // Called at taint-time!!
-    public override void RestoreBodyDependencies(BSPrim child)
-    {
-        // The Refresh operation queued by RemoveBodyDependencies() will build any missing constraints.
-    }
-
     // ================================================================
 
     // Add a new child to the linkset.
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..9460daf 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; } ),
@@ -619,11 +645,8 @@ public static class BSParam
                 entries.Add(new PhysParameterEntry(pd.name, pd.desc));
             }
 
-            // make the list in alphabetical order for estetic reasons
-            entries.Sort(delegate(PhysParameterEntry ppe1, PhysParameterEntry ppe2)
-            {
-                return ppe1.name.CompareTo(ppe2.name);
-            });
+            // make the list alphabetical for estetic reasons
+            entries.Sort((ppe1, ppe2) => { return ppe1.name.CompareTo(ppe2.name); });
 
             SettableParameters = entries.ToArray();
         }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 821f470..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()
@@ -139,6 +149,11 @@ public abstract class BSPhysObject : PhysicsActor
     public abstract bool IsStatic { get; }
     public abstract bool IsSelected { get; }
 
+    // It can be confusing for an actor to know if it should move or update an object
+    //    depeneding on the setting of 'selected', 'physical, ...
+    // This flag is the true test -- if true, the object is being acted on in the physical world
+    public abstract bool IsPhysicallyActive { get; }
+
     // Materialness
     public MaterialAttributes.Material Material { get; private set; }
     public override void SetMaterial(int material)
@@ -302,8 +317,9 @@ public abstract class BSPhysObject : PhysicsActor
     public virtual bool SendCollisions()
     {
         bool ret = true;
+
         // If the 'no collision' call, force it to happen right now so quick collision_end
-        bool force = (CollisionCollection.Count == 0);
+        bool force = (CollisionCollection.Count == 0 && CollisionsLastTick.Count != 0);
 
         // throttle the collisions to the number of milliseconds specified in the subscription
         if (force || (PhysicsScene.SimulationNowTime >= NextCollisionOkTime))
@@ -318,7 +334,7 @@ public abstract class BSPhysObject : PhysicsActor
                 ret = false;
             }
 
-            // DetailLog("{0},{1}.SendCollisionUpdate,call,numCollisions={2}", LocalID, TypeName, CollisionCollection.Count);
+            DetailLog("{0},{1}.SendCollisionUpdate,call,numCollisions={2}", LocalID, TypeName, CollisionCollection.Count);
             base.SendCollisionUpdate(CollisionCollection);
 
             // Remember the collisions from this tick for some collision specific processing.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 79fe632..f80084a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -132,8 +132,8 @@ public sealed class BSPrim : BSPhysObject
         base.Destroy();
 
         // Undo any links between me and any other object
-        BSPhysObject parentBefore = Linkset.LinksetRoot;
-        int childrenBefore = Linkset.NumberOfChildren;
+        BSPhysObject parentBefore = Linkset.LinksetRoot;    // DEBUG DEBUG
+        int childrenBefore = Linkset.NumberOfChildren;      // DEBUG DEBUG
 
         Linkset = Linkset.RemoveMeFromLinkset(this);
 
@@ -311,16 +311,18 @@ 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);
+
             });
         }
     }
+
     public override OMV.Vector3 ForcePosition {
         get {
             _position = PhysicsScene.PE.GetPosition(PhysBody) - PositionDisplacement;
@@ -335,25 +337,6 @@ public sealed class BSPrim : BSPhysObject
             }
         }
     }
-    // Override to have position displacement immediately update the physical position.
-    // A feeble attempt to keep the sim and physical positions in sync
-    // Must be called at taint time.
-    public override OMV.Vector3 PositionDisplacement
-    {
-        get
-        {
-            return base.PositionDisplacement;
-        }
-        set
-        {
-            base.PositionDisplacement = value;
-            PhysicsScene.TaintedObject(PhysicsScene.InTaintTime, "BSPrim.setPosition", delegate()
-            {
-                if (PhysBody.HasPhysicalBody)
-                    PhysicsScene.PE.SetTranslation(PhysBody, _position + base.PositionDisplacement, _orientation);
-            });
-        }
-    }
 
     // Check that the current position is sane and, if not, modify the position to make it so.
     // Check for being below terrain and being out of bounds.
@@ -370,11 +353,11 @@ public sealed class BSPrim : BSPhysObject
             return ret;
         }
 
-        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
+        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
         OMV.Vector3 upForce = OMV.Vector3.Zero;
         if (RawPosition.Z < terrainHeight)
         {
-            DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
+            DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, RawPosition, terrainHeight);
             float targetHeight = terrainHeight + (Size.Z / 2f);
             // If the object is below ground it just has to be moved up because pushing will
             //     not get it through the terrain
@@ -502,6 +485,12 @@ public sealed class BSPrim : BSPhysObject
                 RegisterPreStepAction("BSPrim.setForce", LocalID,
                     delegate(float timeStep)
                     {
+                        if (!IsPhysicallyActive)
+                        {
+                            UnRegisterPreStepAction("BSPrim.setForce", LocalID);
+                            return;
+                        }
+
                         DetailLog("{0},BSPrim.setForce,preStep,force={1}", LocalID, _force);
                         if (PhysBody.HasPhysicalBody)
                         {
@@ -627,6 +616,12 @@ public sealed class BSPrim : BSPhysObject
                 RegisterPreStepAction("BSPrim.setTorque", LocalID,
                     delegate(float timeStep)
                     {
+                        if (!IsPhysicallyActive)
+                        {
+                            UnRegisterPreStepAction("BSPrim.setTorque", LocalID);
+                            return;
+                        }
+
                         if (PhysBody.HasPhysicalBody)
                             AddAngularForce(_torque, false, true);
                     }
@@ -670,12 +665,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);
+
             });
         }
     }
@@ -727,6 +723,12 @@ public sealed class BSPrim : BSPhysObject
         get { return !IsPhantom && !_isVolumeDetect; }
     }
 
+    // The object is moving and is actively being dynamic in the physical world
+    public override bool IsPhysicallyActive
+    {
+        get { return !_isSelected && IsPhysical; }
+    }
+
     // Make gravity work if the object is physical and not selected
     // Called at taint-time!!
     private void SetObjectDynamic(bool forceRebuild)
@@ -971,10 +973,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;
             });
         }
@@ -987,7 +989,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);
             }
         }
@@ -1055,10 +1059,16 @@ public sealed class BSPrim : BSPhysObject
 
                 RegisterPreStepAction("BSPrim.PIDTarget", LocalID, delegate(float timeStep)
                 {
+                    if (!IsPhysicallyActive)
+                    {
+                        UnRegisterPreStepAction("BSPrim.PIDTarget", LocalID);
+                        return;
+                    }
+
                     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())
@@ -1102,6 +1112,9 @@ public sealed class BSPrim : BSPhysObject
 
                 RegisterPreStepAction("BSPrim.Hover", LocalID, delegate(float timeStep)
                 {
+                    if (!IsPhysicallyActive)
+                        return;
+
                     _hoverMotor.SetCurrent(RawPosition.Z);
                     _hoverMotor.SetTarget(ComputeCurrentPIDHoverHeight());
                     float targetHeight = _hoverMotor.Step(timeStep);
@@ -1166,28 +1179,25 @@ 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) {
         // for an object, doesn't matter if force is a pushforce or not
-        if (!IsStatic)
+        if (IsPhysicallyActive)
         {
             if (force.IsFinite())
             {
-                float magnitude = force.Length();
-                if (magnitude > BSParam.MaxAddForceMagnitude)
-                {
-                    // Force has a limit
-                    force = force / magnitude * BSParam.MaxAddForceMagnitude;
-                }
-
-                OMV.Vector3 addForce = force;
                 // 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
@@ -1209,19 +1219,13 @@ public sealed class BSPrim : BSPhysObject
 
     public void AddForceImpulse(OMV.Vector3 impulse, bool pushforce, bool inTaintTime) {
         // for an object, doesn't matter if force is a pushforce or not
-        if (!IsStatic)
+        if (!IsPhysicallyActive)
         {
             if (impulse.IsFinite())
             {
-                float magnitude = impulse.Length();
-                if (magnitude > BSParam.MaxAddForceMagnitude)
-                {
-                    // Force has a limit
-                    impulse = impulse / magnitude * BSParam.MaxAddForceMagnitude;
-                }
-
+                OMV.Vector3 addImpulse = Util.ClampV(impulse, BSParam.MaxAddForceMagnitude);
                 // DetailLog("{0},BSPrim.addForceImpulse,call,impulse={1}", LocalID, impulse);
-                OMV.Vector3 addImpulse = impulse;
+
                 PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddImpulse", delegate()
                 {
                     // Bullet adds this impulse immediately to the velocity
@@ -1584,11 +1588,6 @@ public sealed class BSPrim : BSPhysObject
     // Called at taint-time!!!
     public void CreateGeomAndObject(bool forceRebuild)
     {
-        // If this prim is part of a linkset, we must remove and restore the physical
-        //    links if the body is rebuilt.
-        bool needToRestoreLinkset = false;
-        bool needToRestoreVehicle = false;
-
         // Create the correct physical representation for this type of object.
         // Updates PhysBody and PhysShape with the new information.
         // Ignore 'forceRebuild'. This routine makes the right choices and changes of necessary.
@@ -1597,21 +1596,10 @@ public sealed class BSPrim : BSPhysObject
             // Called if the current prim body is about to be destroyed.
             // Remove all the physical dependencies on the old body.
             // (Maybe someday make the changing of BSShape an event to be subscribed to by BSLinkset, ...)
-            needToRestoreLinkset = Linkset.RemoveBodyDependencies(this);
-            needToRestoreVehicle = _vehicle.RemoveBodyDependencies(this);
+            Linkset.RemoveBodyDependencies(this);
+            _vehicle.RemoveBodyDependencies(this);
         });
 
-        if (needToRestoreLinkset)
-        {
-            // If physical body dependencies were removed, restore them
-            Linkset.RestoreBodyDependencies(this);
-        }
-        if (needToRestoreVehicle)
-        {
-            // If physical body dependencies were removed, restore them
-            _vehicle.RestoreBodyDependencies(this);
-        }
-
         // Make sure the properties are set on the new object
         UpdatePhysicalParameters();
         return;
@@ -1628,17 +1616,28 @@ 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;
+            }
+
+            // DetailLog("{0},BSPrim.UpdateProperties,entry,entprop={1}", LocalID, entprop);   // DEBUG DEBUG
+
+            // Undo any center-of-mass displacement that might have been done.
+            if (PositionDisplacement != OMV.Vector3.Zero)
+            {
+                // Correct for any rotation around the center-of-mass
+                // TODO!!!
+                entprop.Position -= PositionDisplacement;
             }
 
             // Assign directly to the local variables so the normal set actions do not happen
-            entprop.Position -= PositionDisplacement;
             _position = entprop.Position;
             _orientation = entprop.Rotation;
             _velocity = entprop.Velocity;
             _acceleration = entprop.Acceleration;
             _rotationalVelocity = entprop.RotationalVelocity;
 
+            // DetailLog("{0},BSPrim.UpdateProperties,afterAssign,entprop={1}", LocalID, entprop);   // DEBUG DEBUG
+
             // The sanity check can change the velocity and/or position.
             if (IsPhysical && PositionSanityCheck(true))
             {
@@ -1647,8 +1646,7 @@ public sealed class BSPrim : BSPhysObject
             }
 
             OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;   // DEBUG DEBUG DEBUG
-            DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},dir={3},vel={4},rotVel={5}",
-                    LocalID, _position, _orientation, direction, _velocity, _rotationalVelocity);
+            DetailLog("{0},BSPrim.UpdateProperties,call,entProp={1},dir={2}", LocalID, entprop, direction);
 
             // remember the current and last set values
             LastEntityProperties = CurrentEntityProperties;
@@ -1667,7 +1665,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..34fd2a0 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
@@ -709,8 +708,8 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     //    TriggerPreStepEvent
     //    DoOneTimeTaints
     //    Step()
-    //       ProcessAndForwardCollisions
-    //       ProcessAndForwardPropertyUpdates
+    //       ProcessAndSendToSimulatorCollisions
+    //       ProcessAndSendToSimulatorPropertyUpdates
     //    TriggerPostStepEvent
 
     // Calls to the PhysicsActors can't directly call into the physics engine
@@ -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 d4545f7..801f690 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -1,22 +1,22 @@
 CURRENT PRIORITIES
 =================================================
-Mantis 6040 script http://opensimulator.org/mantis/view.php?id=6040
-        Msg Kayaker on OSGrid when working
+Deleting a linkset while standing on the root will leave the physical shape of the root behind.
+        Not sure if it is because standing on it. Done with large prim linksets.
+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
-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)
@@ -72,8 +72,15 @@ 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.
 Revisit CollisionMargin. Builders notice the 0.04 spacing between prims.
 Duplicating a physical prim causes old prim to jump away
         Dup a phys prim and the original become unselected and thus interacts w/ selected prim.
@@ -121,7 +128,7 @@ Physical and phantom will drop through the terrain
 LINKSETS
 ======================================================
 Editing a child of a linkset causes the child to go phantom
-        Move a child prim once when it is physical and can never move it again without it going phantom

+        Move a child prim once when it is physical and can never move it again without it going phantom
 Offset the center of the linkset to be the geometric center of all the prims
         Not quite the same as the center-of-gravity
 Linksets should allow collisions to individual children
@@ -306,4 +313,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