5 files changed, 533 insertions, 242 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index dc0c008..09e1f0c 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -41,7 +41,7 @@ public class BSCharacter : PhysicsActor
 
     private BSScene _scene;
     private String _avName;
-    private bool _stopped;
+    // private bool _stopped;
     private Vector3 _size;
     private Vector3 _scale;
     private PrimitiveBaseShape _pbs;
@@ -134,9 +134,9 @@ public class BSCharacter : PhysicsActor
     {
         base.RequestPhysicsterseUpdate();
     }
-
+    // No one calls this method so I don't know what it could possibly mean
     public override bool Stopped { 
-        get { return _stopped; } 
+        get { return false; } 
     }
     public override Vector3 Size { 
         get { return _size; } 
@@ -391,52 +391,47 @@ public class BSCharacter : PhysicsActor
         _mass = _density * _avatarVolume;
     }
 
-    // Set to 'true' if the individual changed items should be checked
-    //   (someday RequestPhysicsTerseUpdate() will take a bitmap of changed properties)
-    const bool SHOULD_CHECK_FOR_INDIVIDUAL_CHANGES = false;
-
     // The physics engine says that properties have updated. Update same and inform
     // the world that things have changed.
     public void UpdateProperties(EntityProperties entprop)
     {
+        /*
         bool changed = false;
-        if (SHOULD_CHECK_FOR_INDIVIDUAL_CHANGES) {
-            // we assign to the local variables so the normal set action does not happen
-            if (_position != entprop.Position) {
-                _position = entprop.Position;
-                changed = true;
-            }
-            if (_orientation != entprop.Rotation) {
-                _orientation = entprop.Rotation;
-                changed = true;
-            }
-            if (_velocity != entprop.Velocity) {
-                _velocity = entprop.Velocity;
-                changed = true;
-            }
-            if (_acceleration != entprop.Acceleration) {
-                _acceleration = entprop.Acceleration;
-                changed = true;
-            }
-            if (_rotationalVelocity != entprop.RotationalVelocity) {
-                _rotationalVelocity = entprop.RotationalVelocity;
-                changed = true;
-            }
-            if (changed) {
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-                // Avatar movement is not done by generating this event. There is code in the heartbeat
-                //   loop that updates avatars.
-                // base.RequestPhysicsterseUpdate();
-            }
-        }
-        else {
+        // we assign to the local variables so the normal set action does not happen
+        if (_position != entprop.Position) {
             _position = entprop.Position;
+            changed = true;
+        }
+        if (_orientation != entprop.Rotation) {
             _orientation = entprop.Rotation;
+            changed = true;
+        }
+        if (_velocity != entprop.Velocity) {
             _velocity = entprop.Velocity;
+            changed = true;
+        }
+        if (_acceleration != entprop.Acceleration) {
             _acceleration = entprop.Acceleration;
+            changed = true;
+        }
+        if (_rotationalVelocity != entprop.RotationalVelocity) {
             _rotationalVelocity = entprop.RotationalVelocity;
+            changed = true;
+        }
+        if (changed) {
+            // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
+            // Avatar movement is not done by generating this event. There is code in the heartbeat
+            //   loop that updates avatars.
             // base.RequestPhysicsterseUpdate();
         }
+        */
+        _position = entprop.Position;
+        _orientation = entprop.Rotation;
+        _velocity = entprop.Velocity;
+        _acceleration = entprop.Acceleration;
+        _rotationalVelocity = entprop.RotationalVelocity;
+        // Avatars don't report theirr changes the usual way. Changes are checked for in the heartbeat loop.
+        // base.RequestPhysicsterseUpdate();
     }
 
     // Called by the scene when a collision with this object is reported
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index eb20eb3..c197e61 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -57,7 +57,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private int frcount = 0;                                        // Used to limit dynamics debug output to
                                                                         // every 100th frame
 
-        // private BSScene m_parentScene = null;
         private BSPrim m_prim;      // the prim this dynamic controller belongs to
 
         // Vehicle properties
@@ -131,8 +130,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_type = Vehicle.TYPE_NONE;
         }
 
-        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue, float timestep)
         {
+            DetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
@@ -229,8 +229,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
         }//end ProcessFloatVehicleParam
 
-        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
+        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue, float timestep)
         {
+            DetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
@@ -265,6 +266,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
         {
+            DetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.REFERENCE_FRAME:
@@ -278,6 +280,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessVehicleFlags(int pParam, bool remove)
         {
+            DetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", m_prim.LocalID, pParam, remove);
             if (remove)
             {
                 if (pParam == -1)
@@ -434,6 +437,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessTypeChange(Vehicle pType)
         {
+            DetailLog("{0},ProcessTypeChange,type={1}", m_prim.LocalID, pType);
             // Set Defaults For Type
             m_type = pType;
             switch (pType)
@@ -594,11 +598,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
                     m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     break;
-
             }
         }//end SetDefaultsForType
 
-        internal void Step(float pTimestep,  BSScene pParentScene)
+        internal void Step(float pTimestep)
         {
             if (m_type == Vehicle.TYPE_NONE) return;
 
@@ -606,21 +609,34 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             if (frcount > 100)
                 frcount = 0;
 
-            MoveLinear(pTimestep, pParentScene);
+            MoveLinear(pTimestep);
             MoveAngular(pTimestep);
             LimitRotation(pTimestep);
+
+            DetailLog("{0},Dynamics,done,pos={1},force={2},velocity={3},angvel={4}", 
+                    m_prim.LocalID, m_prim.Position, m_prim.Force, m_prim.Velocity, m_prim.RotationalVelocity);
         }// end Step
 
-        private void MoveLinear(float pTimestep, BSScene _pParentScene)
+        private void MoveLinear(float pTimestep)
         {
-            if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))  // requested m_linearMotorDirection is significant
+            // requested m_linearMotorDirection is significant
+            // if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
+            if (m_linearMotorDirection.LengthSquared() > 0.0001f)
             {
+                Vector3 origDir = m_linearMotorDirection;
+                Vector3 origVel = m_lastLinearVelocityVector;
+
                 // add drive to body
-                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
-                m_lastLinearVelocityVector += (addAmount*10);  // lastLinearVelocityVector is the current body velocity vector?
+                // Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
+                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale);
+                // lastLinearVelocityVector is the current body velocity vector?
+                // RA: Not sure what the *10 is for. A correction for pTimestep?
+                // m_lastLinearVelocityVector += (addAmount*10);  
+                m_lastLinearVelocityVector += addAmount;  
 
                 // This will work temporarily, but we really need to compare speed on an axis
                 // KF: Limit body velocity to applied velocity?
+                // Limit the velocity vector to less than the last set linear motor direction
                 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
                     m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
                 if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
@@ -630,76 +646,93 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 // decay applied velocity
                 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
-                //Console.WriteLine("decay: " + decayfraction);
                 m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
-                //Console.WriteLine("actual: " + m_linearMotorDirection);
+
+                /*
+                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/m_linearMotorTimescale;
+                m_lastLinearVelocityVector += addAmount;
+
+                float decayfraction = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+                m_linearMotorDirection *= decayfraction;
+
+                 */
+
+                DetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},add={3},decay={4},dir={5},vel={6}",
+                    m_prim.LocalID, origDir, origVel, addAmount, decayfraction, m_linearMotorDirection, m_lastLinearVelocityVector);
             }
             else
-            {        // requested is not significant
-                    // if what remains of applied is small, zero it.
-                if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
-                    m_lastLinearVelocityVector = Vector3.Zero;
+            {
+                // if what remains of applied is small, zero it.
+                // if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
+                //     m_lastLinearVelocityVector = Vector3.Zero;
+                m_linearMotorDirection = Vector3.Zero;
+                m_lastLinearVelocityVector = Vector3.Zero;
             }
 
             // convert requested object velocity to world-referenced vector
-            m_dir = m_lastLinearVelocityVector;
-            Quaternion rot = m_prim.Orientation;
-            Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
-            m_dir *= rotq;                            // apply obj rotation to velocity vector
+            Quaternion rotq = m_prim.Orientation;
+            m_dir = m_lastLinearVelocityVector * rotq;
+
+            // Add the various forces into m_dir which will be our new direction vector (velocity)
 
-            // add Gravity andBuoyancy
+            // add Gravity and Buoyancy
             // KF: So far I have found no good method to combine a script-requested
             // .Z velocity and gravity. Therefore only 0g will used script-requested
             // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
             Vector3 grav = Vector3.Zero;
-            // There is some gravity, make a gravity force vector
-            // that is applied after object velocity.
-            float objMass = m_prim.Mass;
+            // There is some gravity, make a gravity force vector that is applied after object velocity.
             // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            grav.Z = _pParentScene.DefaultGravity.Z * objMass * (1f - m_VehicleBuoyancy);
+            grav.Z = m_prim.Scene.DefaultGravity.Z * m_prim.Mass * (1f - m_VehicleBuoyancy);
             // Preserve the current Z velocity
             Vector3 vel_now = m_prim.Velocity;
             m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
 
             Vector3 pos = m_prim.Position;
+            Vector3 posChange = pos;
 //            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
-            Vector3 posChange = new Vector3();
-            posChange.X = pos.X - m_lastPositionVector.X;
-            posChange.Y = pos.Y - m_lastPositionVector.Y;
-            posChange.Z = pos.Z - m_lastPositionVector.Z;
             double Zchange = Math.Abs(posChange.Z);
             if (m_BlockingEndPoint != Vector3.Zero)
             {
+                bool changed = false;
                 if (pos.X >= (m_BlockingEndPoint.X - (float)1))
                 {
                     pos.X -= posChange.X + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                 }
                 if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
                 {
                     pos.Y -= posChange.Y + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                 }
                 if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
                 {
                     pos.Z -= posChange.Z + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                 }
                 if (pos.X <= 0)
                 {
                     pos.X += posChange.X + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                 }
                 if (pos.Y <= 0)
                 {
                     pos.Y += posChange.Y + 1;
+                    changed = true;
+                }
+                if (changed)
+                {
                     m_prim.Position = pos;
+                    DetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
+                                m_prim.LocalID, m_BlockingEndPoint, posChange, pos);
                 }
             }
-            if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
+
+            // If below the terrain, move us above the ground a little.
+            if (pos.Z < m_prim.Scene.GetTerrainHeightAtXYZ(pos))
             {
-                pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
+                pos.Z = m_prim.Scene.GetTerrainHeightAtXYZ(pos) + 2;
                 m_prim.Position = pos;
+                DetailLog("{0},MoveLinear,terrainHeight,pos={1}", m_prim.LocalID, pos);
             }
 
             // Check if hovering
@@ -708,11 +741,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // We should hover, get the target height
                 if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
+                    m_VhoverTargetHeight = m_prim.Scene.GetWaterLevel() + m_VhoverHeight;
                 }
                 if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
+                    m_VhoverTargetHeight = m_prim.Scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
                 }
                 if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                 {
@@ -746,6 +779,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     }
                 }
 
+                DetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", m_prim.LocalID, pos, m_dir, m_VhoverHeight, m_VhoverTargetHeight);
+
 //                m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
 //                m_VhoverTimescale = 0f;        // time to acheive height
 //                pTimestep  is time since last frame,in secs
@@ -774,12 +809,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 {
                     grav.Z = (float)(grav.Z * 1.125);
                 }
-                float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                float terraintemp = m_prim.Scene.GetTerrainHeightAtXYZ(pos);
                 float postemp = (pos.Z - terraintemp);
                 if (postemp > 2.5f)
                 {
                     grav.Z = (float)(grav.Z * 1.037125);
                 }
+                DetailLog("{0},MoveLinear,limitMotorUp,grav={1}", m_prim.LocalID, grav);
                 //End Experimental Values
             }
             if ((m_flags & (VehicleFlag.NO_X)) != 0)
@@ -800,32 +836,39 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // Apply velocity
             m_prim.Velocity = m_dir;
             // apply gravity force
-            m_prim.Force = grav;
-
+            // Why is this set here? The physics engine already does gravity.
+            // m_prim.AddForce(grav, false);
+            // m_prim.Force = grav;
 
-            // apply friction
+            // Apply friction
             Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
             m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
+
+            DetailLog("{0},MoveLinear,done,pos={1},vel={2},force={3},decay={4}", 
+                        m_prim.LocalID, m_lastPositionVector, m_dir, grav, decayamount);
+
         } // end MoveLinear()
 
         private void MoveAngular(float pTimestep)
         {
-            /*
-            private Vector3 m_angularMotorDirection = Vector3.Zero;            // angular velocity requested by LSL motor
-            private int m_angularMotorApply = 0;                            // application frame counter
-             private float m_angularMotorVelocity = 0;                        // current angular motor velocity (ramps up and down)
-            private float m_angularMotorTimescale = 0;                        // motor angular velocity ramp up rate
-            private float m_angularMotorDecayTimescale = 0;                    // motor angular velocity decay rate
-            private Vector3 m_angularFrictionTimescale = Vector3.Zero;        // body angular velocity  decay rate
-            private Vector3 m_lastAngularVelocity = Vector3.Zero;            // what was last applied to body
-            */
+            // m_angularMotorDirection         // angular velocity requested by LSL motor
+            // m_angularMotorApply             // application frame counter
+            // m_angularMotorVelocity          // current angular motor velocity (ramps up and down)
+            // m_angularMotorTimescale         // motor angular velocity ramp up rate
+            // m_angularMotorDecayTimescale    // motor angular velocity decay rate
+            // m_angularFrictionTimescale      // body angular velocity  decay rate
+            // m_lastAngularVelocity           // what was last applied to body
 
             // Get what the body is doing, this includes 'external' influences
             Vector3 angularVelocity = m_prim.RotationalVelocity;
-   //         Vector3 angularVelocity = Vector3.Zero;
 
             if (m_angularMotorApply > 0)
             {
+                // Rather than snapping the angular motor velocity from the old value to
+                //    a newly set velocity, this routine steps the value from the previous
+                //    value (m_angularMotorVelocity) to the requested value (m_angularMotorDirection).
+                // There are m_angularMotorApply steps.
+                Vector3 origAngularVelocity = m_angularMotorVelocity;
                 // ramp up to new value
                 //   current velocity  +=                         error                       /    (time to get there / step interval)
                 //                               requested speed            -  last motor speed
@@ -833,23 +876,21 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) /  (m_angularMotorTimescale / pTimestep);
                 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) /  (m_angularMotorTimescale / pTimestep);
 
+                DetailLog("{0},MoveAngular,angularMotorApply,apply={1},origvel={2},dir={3},vel={4}", 
+                        m_prim.LocalID,m_angularMotorApply,origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
+
                 m_angularMotorApply--;        // This is done so that if script request rate is less than phys frame rate the expected
                                             // velocity may still be acheived.
             }
             else
             {
-                // no motor recently applied, keep the body velocity
-        /*        m_angularMotorVelocity.X = angularVelocity.X;
-                m_angularMotorVelocity.Y = angularVelocity.Y;
-                m_angularMotorVelocity.Z = angularVelocity.Z; */
-
+                // No motor recently applied, keep the body velocity
                 // and decay the velocity
                 m_angularMotorVelocity -= m_angularMotorVelocity /  (m_angularMotorDecayTimescale / pTimestep);
             } // end motor section
 
             // Vertical attractor section
             Vector3 vertattr = Vector3.Zero;
-
             if (m_verticalAttractionTimescale < 300)
             {
                 float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
@@ -871,7 +912,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // Error is 0 (no error) to +/- 2 (max error)
                 // scale it by VAservo
                 verterr = verterr * VAservo;
-//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
 
                 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
                 // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
@@ -884,11 +924,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 vertattr.X += bounce * angularVelocity.X;
                 vertattr.Y += bounce * angularVelocity.Y;
 
+                DetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}", 
+                            m_prim.LocalID, verterr, bounce, vertattr);
+
             } // else vertical attractor is off
 
-    //        m_lastVertAttractor = vertattr;
+            // m_lastVertAttractor = vertattr;
 
             // Bank section tba
+
             // Deflection section tba
 
             // Sum velocities
@@ -898,11 +942,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 m_lastAngularVelocity.X = 0;
                 m_lastAngularVelocity.Y = 0;
+                DetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
             }
 
             if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
             {
                 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
+                DetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
             }
 
              // apply friction
@@ -912,10 +958,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // Apply to the body
             m_prim.RotationalVelocity = m_lastAngularVelocity;
 
+            DetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", m_prim.LocalID, decayamount, m_lastAngularVelocity);
         } //end MoveAngular
+
         internal void LimitRotation(float timestep)
         {
-            Quaternion rotq = m_prim.Orientation;    // rotq = rotation of object
+            Quaternion rotq = m_prim.Orientation;
             Quaternion m_rot = rotq;
             bool changed = false;
             if (m_RollreferenceFrame != Quaternion.Identity)
@@ -923,18 +971,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 if (rotq.X >= m_RollreferenceFrame.X)
                 {
                     m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
+                    changed = true;
                 }
                 if (rotq.Y >= m_RollreferenceFrame.Y)
                 {
                     m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
+                    changed = true;
                 }
                 if (rotq.X <= -m_RollreferenceFrame.X)
                 {
                     m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
+                    changed = true;
                 }
                 if (rotq.Y <= -m_RollreferenceFrame.Y)
                 {
                     m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
+                    changed = true;
                 }
                 changed = true;
             }
@@ -944,8 +996,23 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 m_rot.Y = 0;
                 changed = true;
             }
+            if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
+            {
+                m_rot.X = 0;
+                m_rot.Y = 0;
+                changed = true;
+            }
             if (changed)
                 m_prim.Orientation = m_rot;
+
+            DetailLog("{0},LimitRotation,done,changed={1},orig={2},new={3}", m_prim.LocalID, changed, rotq, m_rot);
+        }
+
+        // Invoke the detailed logger and output something if it's enabled.
+        private void DetailLog(string msg, params Object[] args)
+        {
+            if (m_prim.Scene.VehicleLoggingEnabled)
+                m_prim.Scene.PhysicsLogging.Write(msg, args);
         }
     }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 130f1ca..71a4303 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -42,6 +42,8 @@ public sealed class BSPrim : PhysicsActor
     private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
     private static readonly string LogHeader = "[BULLETS PRIM]";
 
+    private void DebugLog(string mm, params Object[] xx) { if (_scene.shouldDebugLog) m_log.DebugFormat(mm, xx); }
+
     private IMesh _mesh;
     private PrimitiveBaseShape _pbs;
     private ShapeData.PhysicsShapeType _shapeType;
@@ -50,6 +52,7 @@ public sealed class BSPrim : PhysicsActor
     private List<ConvexResult> _hulls;
 
     private BSScene _scene;
+    public BSScene Scene { get { return _scene; } }
     private String _avName;
     private uint _localID = 0;
 
@@ -86,8 +89,8 @@ public sealed class BSPrim : PhysicsActor
     private bool _kinematic;
     private float _buoyancy;
 
-    private List<BSPrim> _childrenPrims;
     private BSPrim _parentPrim;
+    private List<BSPrim> _childrenPrims;
 
     private int _subscribedEventsMs = 0;
     private int _nextCollisionOkTime = 0;
@@ -145,9 +148,19 @@ public sealed class BSPrim : PhysicsActor
     public void Destroy()
     {
         // m_log.DebugFormat("{0}: Destroy, id={1}", LogHeader, LocalID);
+        // DetailLog("{0},Destroy", LocalID);
         // Undo any vehicle properties
         _vehicle.ProcessTypeChange(Vehicle.TYPE_NONE);
         _scene.RemoveVehiclePrim(this);     // just to make sure
+
+        // undo any dependance with/on other objects
+        if (_parentPrim != null)
+        {
+            // If I'm someone's child, tell them to forget about me.
+            _parentPrim.RemoveChildFromLinkset(this);
+            _parentPrim = null;
+        }
+
         _scene.TaintedObject(delegate()
         {
             // everything in the C# world will get garbage collected. Tell the C++ world to free stuff.
@@ -202,7 +215,8 @@ public sealed class BSPrim : PhysicsActor
     // link me to the specified parent
     public override void link(PhysicsActor obj) {
         BSPrim parent = obj as BSPrim;
-        // m_log.DebugFormat("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, obj.LocalID);
+        DebugLog("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, obj.LocalID);
+        DetailLog("{0},link,parent={1}", LocalID, obj.LocalID);
         // TODO: decide if this parent checking needs to happen at taint time
         if (_parentPrim == null)
         {
@@ -225,7 +239,7 @@ public sealed class BSPrim : PhysicsActor
                 else
                 {
                     // asking to reparent a prim should not happen
-                    m_log.ErrorFormat("{0}: Reparenting a prim. ", LogHeader);
+                    m_log.ErrorFormat("{0}: link(): Reparenting a prim. ", LogHeader);
                 }
             }
         }
@@ -236,7 +250,9 @@ public sealed class BSPrim : PhysicsActor
     public override void delink() {
         // TODO: decide if this parent checking needs to happen at taint time
         // Race condition here: if link() and delink() in same simulation tick, the delink will not happen
-        // m_log.DebugFormat("{0}: delink {1}/{2}", LogHeader, _avName, _localID);
+        DebugLog("{0}: delink {1}/{2}. Parent={3}", LogHeader, _avName, _localID, 
+                (_parentPrim==null ? "NULL" : _parentPrim._avName+"/"+_parentPrim.LocalID.ToString()));
+        DetailLog("{0},delink,parent={1}", LocalID, (_parentPrim==null ? "NULL" : _parentPrim.LocalID.ToString()));
         if (_parentPrim != null)
         {
             _parentPrim.RemoveChildFromLinkset(this);
@@ -252,8 +268,10 @@ public sealed class BSPrim : PhysicsActor
         {
             if (!_childrenPrims.Contains(child))
             {
+                DebugLog("{0}: AddChildToLinkset: adding child {1} to {2}", LogHeader, child.LocalID, this.LocalID);
+                DetailLog("{0},AddChildToLinkset,child={1}", LocalID, pchild.LocalID);
                 _childrenPrims.Add(child);
-                child.ParentPrim = this;    // the child has gained a parent
+                child._parentPrim = this;    // the child has gained a parent
                 RecreateGeomAndObject();    // rebuild my shape with the new child added
             }
         });
@@ -269,9 +287,14 @@ public sealed class BSPrim : PhysicsActor
         {
             if (_childrenPrims.Contains(child))
             {
-                BulletSimAPI.RemoveConstraint(_scene.WorldID, child.LocalID, this.LocalID);
+                DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
+                DetailLog("{0},RemoveChildToLinkset,child={1}", LocalID, pchild.LocalID);
+                if (!BulletSimAPI.RemoveConstraintByID(_scene.WorldID, child.LocalID))
+                {
+                    m_log.ErrorFormat("{0}: RemoveChildFromLinkset: Failed remove constraint for {1}", LogHeader, child.LocalID);
+                }
                 _childrenPrims.Remove(child);
-                child.ParentPrim = null;    // the child has lost its parent
+                child._parentPrim = null;    // the child has lost its parent
                 RecreateGeomAndObject();    // rebuild my shape with the child removed
             }
             else
@@ -282,11 +305,6 @@ public sealed class BSPrim : PhysicsActor
         return;
     }
 
-    public BSPrim ParentPrim
-    {
-        set { _parentPrim = value; }
-    }
-
     // return true if we are the root of a linkset (there are children to manage)
     public bool IsRootOfLinkset
     {
@@ -304,20 +322,28 @@ public sealed class BSPrim : PhysicsActor
         base.RequestPhysicsterseUpdate();
     }
 
-    public override void LockAngularMotion(OMV.Vector3 axis) { return; }
+    public override void LockAngularMotion(OMV.Vector3 axis)
+    { 
+        DetailLog("{0},LockAngularMotion,call,axis={1}", LocalID, axis);
+        return;
+    }
 
     public override OMV.Vector3 Position { 
         get { 
-            // don't do the following GetObjectPosition because this function is called a zillion times
+            // child prims move around based on their parent. Need to get the latest location
+            if (_parentPrim != null)
+                _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
+            // don't do the GetObjectPosition for root elements because this function is called a zillion times
             // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
             return _position; 
         } 
         set {
             _position = value;
+            // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
             _scene.TaintedObject(delegate()
             {
+                DetailLog("{0},SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
-                // m_log.DebugFormat("{0}: setPosition: id={1}, position={2}", LogHeader, _localID, _position);
             });
         } 
     }
@@ -330,6 +356,7 @@ public sealed class BSPrim : PhysicsActor
             _force = value;
             _scene.TaintedObject(delegate()
             {
+                DetailLog("{0},SetForce,taint,force={1}", LocalID, _force);
                 BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
             });
         } 
@@ -341,15 +368,23 @@ public sealed class BSPrim : PhysicsActor
         } 
         set {
             Vehicle type = (Vehicle)value;
-            _vehicle.ProcessTypeChange(type);
             _scene.TaintedObject(delegate()
             {
+                DetailLog("{0},SetVehicleType,taint,type={1}", LocalID, type);
+                _vehicle.ProcessTypeChange(type);
                 if (type == Vehicle.TYPE_NONE)
                 {
                     _scene.RemoveVehiclePrim(this);
                 }
                 else
                 {
+                    _scene.TaintedObject(delegate()
+                    {
+                        // Tell the physics engine to clear state
+                        IntPtr obj = BulletSimAPI.GetBodyHandleWorldID2(_scene.WorldID, LocalID);
+                        BulletSimAPI.ClearForces2(obj);
+                    });
+
                     // make it so the scene will call us each tick to do vehicle things
                     _scene.AddVehiclePrim(this);
                 }
@@ -359,37 +394,52 @@ public sealed class BSPrim : PhysicsActor
     }
     public override void VehicleFloatParam(int param, float value) 
     {
-        _vehicle.ProcessFloatVehicleParam((Vehicle)param, value);
+        m_log.DebugFormat("{0} VehicleFloatParam. {1} <= {2}", LogHeader, param, value);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessFloatVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
+        });
     }
     public override void VehicleVectorParam(int param, OMV.Vector3 value) 
     {
-        _vehicle.ProcessVectorVehicleParam((Vehicle)param, value);
+        m_log.DebugFormat("{0} VehicleVectorParam. {1} <= {2}", LogHeader, param, value);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessVectorVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
+        });
     }
     public override void VehicleRotationParam(int param, OMV.Quaternion rotation) 
     {
-        _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
+        m_log.DebugFormat("{0} VehicleRotationParam. {1} <= {2}", LogHeader, param, rotation);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
+        });
     }
     public override void VehicleFlags(int param, bool remove) 
     {
-        _vehicle.ProcessVehicleFlags(param, remove);
+        m_log.DebugFormat("{0} VehicleFlags. {1}. Remove={2}", LogHeader, param, remove);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessVehicleFlags(param, remove);
+        });
     }
-    // Called each simulation step to advance vehicle characteristics
+
+    // Called each simulation step to advance vehicle characteristics.
+    // Called from Scene when doing simulation step so we're in taint processing time.
     public void StepVehicle(float timeStep)
     {
-        _vehicle.Step(timeStep, _scene);
+        _vehicle.Step(timeStep);
     }
 
     // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more
     public override void SetVolumeDetect(int param) {
         bool newValue = (param != 0);
-        if (_isVolumeDetect != newValue)
+        _isVolumeDetect = newValue;
+        _scene.TaintedObject(delegate()
         {
-            _isVolumeDetect = newValue;
-            _scene.TaintedObject(delegate()
-            {
-                SetObjectDynamic();
-            });
-        }
+            SetObjectDynamic();
+        });
         return; 
     }
 
@@ -397,9 +447,11 @@ public sealed class BSPrim : PhysicsActor
     public override OMV.Vector3 CenterOfMass { get { return OMV.Vector3.Zero; } }
     public override OMV.Vector3 Velocity { 
         get { return _velocity; } 
-        set { _velocity = value; 
+        set {
+            _velocity = value;
             _scene.TaintedObject(delegate()
             {
+                DetailLog("{0},SetVelocity,taint,vel={1}", LocalID, _velocity);
                 BulletSimAPI.SetObjectVelocity(_scene.WorldID, LocalID, _velocity);
             });
         } 
@@ -407,6 +459,7 @@ public sealed class BSPrim : PhysicsActor
     public override OMV.Vector3 Torque { 
         get { return _torque; } 
         set { _torque = value; 
+            DetailLog("{0},SetTorque,call,torque={1}", LocalID, _torque);
         } 
     }
     public override float CollisionScore { 
@@ -419,13 +472,21 @@ public sealed class BSPrim : PhysicsActor
         set { _acceleration = value; }
     }
     public override OMV.Quaternion Orientation { 
-        get { return _orientation; } 
+        get {
+            if (_parentPrim != null)
+            {
+                // children move around because tied to parent. Get a fresh value.
+                _orientation = BulletSimAPI.GetObjectOrientation(_scene.WorldID, LocalID);
+            }
+            return _orientation;
+        } 
         set {
             _orientation = value;
-            // m_log.DebugFormat("{0}: set orientation: id={1}, ori={2}", LogHeader, LocalID, _orientation);
+            // TODO: what does it mean if a child in a linkset changes its orientation? Rebuild the constraint?
             _scene.TaintedObject(delegate()
             {
                 // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
+                DetailLog("{0},SetOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
             });
         } 
@@ -458,8 +519,9 @@ public sealed class BSPrim : PhysicsActor
         get { return !IsPhantom && !_isVolumeDetect; }
     }
 
-    // make gravity work if the object is physical and not selected
-    // no locking here because only called when it is safe
+    // Make gravity work if the object is physical and not selected
+    // No locking here because only called when it is safe
+    // Only called at taint time so it is save to call into Bullet.
     private void SetObjectDynamic()
     {
         // m_log.DebugFormat("{0}: ID={1}, SetObjectDynamic: IsStatic={2}, IsSolid={3}", LogHeader, _localID, IsStatic, IsSolid);
@@ -476,6 +538,7 @@ public sealed class BSPrim : PhysicsActor
             RecreateGeomAndObject();
 
         }
+        DetailLog("{0},SetObjectDynamic,taint,static={1},solid={2},mass={3}", LocalID, IsStatic, IsSolid, _mass);
         BulletSimAPI.SetObjectProperties(_scene.WorldID, LocalID, IsStatic, IsSolid, SubscribedEvents(), _mass);
     }
 
@@ -516,11 +579,24 @@ public sealed class BSPrim : PhysicsActor
         set { _floatOnWater = value; } 
     }
     public override OMV.Vector3 RotationalVelocity { 
-        get { return _rotationalVelocity; } 
-        set { _rotationalVelocity = value; 
+        get {
+            /*
+            OMV.Vector3 pv = OMV.Vector3.Zero;
+            // if close to zero, report zero
+            // This is copied from ODE but I'm not sure why it returns zero but doesn't
+            //    zero the property in the physics engine.
+            if (_rotationalVelocity.ApproxEquals(pv, 0.2f))
+                return pv;
+             */
+
+            return _rotationalVelocity;
+        } 
+        set {
+            _rotationalVelocity = value;
             // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
             _scene.TaintedObject(delegate()
             {
+                DetailLog("{0},SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
                 BulletSimAPI.SetObjectAngularVelocity(_scene.WorldID, LocalID, _rotationalVelocity);
             });
         } 
@@ -533,11 +609,13 @@ public sealed class BSPrim : PhysicsActor
     }
     public override float Buoyancy { 
         get { return _buoyancy; } 
-        set { _buoyancy = value;
-        _scene.TaintedObject(delegate()
-        {
-            BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
-        });
+        set {
+            _buoyancy = value;
+            _scene.TaintedObject(delegate()
+            {
+                DetailLog("{0},SetBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
+                BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
+            });
         } 
     }
 
@@ -573,27 +651,45 @@ public sealed class BSPrim : PhysicsActor
     public override float APIDStrength { set { return; } }
     public override float APIDDamping { set { return; } }
 
+    private List<OMV.Vector3> m_accumulatedForces = new List<OMV.Vector3>();
     public override void AddForce(OMV.Vector3 force, bool pushforce) {
         if (force.IsFinite())
         {
-            _force.X += force.X;
-            _force.Y += force.Y;
-            _force.Z += force.Z;
+            // _force += force;
+            lock (m_accumulatedForces)
+                m_accumulatedForces.Add(new OMV.Vector3(force));
         }
         else
         {
             m_log.WarnFormat("{0}: Got a NaN force applied to a Character", LogHeader);
+            return;
         }
         _scene.TaintedObject(delegate()
         {
-            BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
+            lock (m_accumulatedForces)
+            {
+                if (m_accumulatedForces.Count > 0)
+                {
+                    OMV.Vector3 fSum = OMV.Vector3.Zero;
+                    foreach (OMV.Vector3 v in m_accumulatedForces)
+                    {
+                        fSum += v;
+                    }
+                    m_accumulatedForces.Clear();
+
+                    DetailLog("{0},SetObjectForce,taint,force={1}", LocalID, fSum);
+                    BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, fSum);
+                }
+            }
         });
     }
 
     public override void AddAngularForce(OMV.Vector3 force, bool pushforce) { 
+        DetailLog("{0},AddAngularForce,call,angForce={1},push={2}", LocalID, force, pushforce);
         // m_log.DebugFormat("{0}: AddAngularForce. f={1}, push={2}", LogHeader, force, pushforce);
     }
     public override void SetMomentum(OMV.Vector3 momentum) { 
+        DetailLog("{0},SetMomentum,call,mom={1}", LocalID, momentum);
     }
     public override void SubscribeEvents(int ms) { 
         _subscribedEventsMs = ms;
@@ -918,6 +1014,7 @@ public sealed class BSPrim : PhysicsActor
                 {
                     // m_log.DebugFormat("{0}: CreateGeom: Defaulting to sphere of size {1}", LogHeader, _size);
                     _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE;
+                    DetailLog("{0},CreateGeom,sphere", LocalID);
                     // Bullet native objects are scaled by the Bullet engine so pass the size in
                     _scale = _size;
                 }
@@ -925,6 +1022,7 @@ public sealed class BSPrim : PhysicsActor
             else
             {
                 // m_log.DebugFormat("{0}: CreateGeom: Defaulting to box. lid={1}, size={2}", LogHeader, LocalID, _size);
+                DetailLog("{0},CreateGeom,box", LocalID);
                 _shapeType = ShapeData.PhysicsShapeType.SHAPE_BOX;
                 _scale = _size;
             }
@@ -961,10 +1059,12 @@ public sealed class BSPrim : PhysicsActor
         // if this new shape is the same as last time, don't recreate the mesh
         if (_meshKey == newMeshKey) return;
 
+        DetailLog("{0},CreateGeomMesh,create,key={1}", LocalID, _meshKey);
         // Since we're recreating new, get rid of any previously generated shape
         if (_meshKey != 0)
         {
             // m_log.DebugFormat("{0}: CreateGeom: deleting old mesh. lID={1}, Key={2}", LogHeader, _localID, _meshKey);
+            DetailLog("{0},CreateGeomMesh,deleteOld,key={1}", LocalID, _meshKey);
             BulletSimAPI.DestroyMesh(_scene.WorldID, _meshKey);
             _mesh = null;
             _meshKey = 0;
@@ -981,7 +1081,6 @@ public sealed class BSPrim : PhysicsActor
         int vi = 0;
         foreach (OMV.Vector3 vv in vertices)
         {
-            // m_log.DebugFormat("{0}:  {1}: <{2:0.00}, {3:0.00}, {4:0.00}>", LogHeader, vi / 3, vv.X, vv.Y, vv.Z);
             verticesAsFloats[vi++] = vv.X;
             verticesAsFloats[vi++] = vv.Y;
             verticesAsFloats[vi++] = vv.Z;
@@ -995,6 +1094,7 @@ public sealed class BSPrim : PhysicsActor
         _shapeType = ShapeData.PhysicsShapeType.SHAPE_MESH;
         // meshes are already scaled by the meshmerizer
         _scale = new OMV.Vector3(1f, 1f, 1f);
+        DetailLog("{0},CreateGeomMesh,done", LocalID);
         return;
     }
 
@@ -1008,13 +1108,17 @@ public sealed class BSPrim : PhysicsActor
         // if the hull hasn't changed, don't rebuild it
         if (newHullKey == _hullKey) return;
 
+        DetailLog("{0},CreateGeomHull,create,key={1}", LocalID, _meshKey);
+        
         // Since we're recreating new, get rid of any previously generated shape
         if (_hullKey != 0)
         {
             // m_log.DebugFormat("{0}: CreateGeom: deleting old hull. Key={1}", LogHeader, _hullKey);
+            DetailLog("{0},CreateGeomHull,deleteOldHull,key={1}", LocalID, _meshKey);
             BulletSimAPI.DestroyHull(_scene.WorldID, _hullKey);
             _hullKey = 0;
             _hulls.Clear();
+            DetailLog("{0},CreateGeomHull,deleteOldMesh,key={1}", LocalID, _meshKey);
             BulletSimAPI.DestroyMesh(_scene.WorldID, _meshKey);
             _mesh = null;   // the mesh cannot match either
             _meshKey = 0;
@@ -1111,6 +1215,7 @@ public sealed class BSPrim : PhysicsActor
         _shapeType = ShapeData.PhysicsShapeType.SHAPE_HULL;
         // meshes are already scaled by the meshmerizer
         _scale = new OMV.Vector3(1f, 1f, 1f);
+        DetailLog("{0},CreateGeomHull,done", LocalID);
         return;
     }
 
@@ -1129,7 +1234,6 @@ public sealed class BSPrim : PhysicsActor
         if (IsRootOfLinkset)
         {
             // Create a linkset around this object
-            // CreateLinksetWithCompoundHull();
             CreateLinksetWithConstraints();
         }
         else
@@ -1191,33 +1295,33 @@ public sealed class BSPrim : PhysicsActor
     // TODO: make this more effeicient: a large linkset gets rebuilt over and over and prims are added
     void CreateLinksetWithConstraints()
     {
-        // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, _childrenPrims.Count+1);
+        DebugLog("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, _childrenPrims.Count+1);
 
         // remove any constraints that might be in place
         foreach (BSPrim prim in _childrenPrims)
         {
-            // m_log.DebugFormat("{0}: CreateLinkset: RemoveConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
+            DebugLog("{0}: CreateLinkset: RemoveConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
             BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, prim.LocalID);
         }
         // create constraints between the root prim and each of the children
         foreach (BSPrim prim in _childrenPrims)
         {
-            // m_log.DebugFormat("{0}: CreateLinkset: AddConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
-
             // Zero motion for children so they don't interpolate
             prim.ZeroMotion();
 
             // relative position normalized to the root prim
-            OMV.Vector3 childRelativePosition = (prim._position - this._position) * OMV.Quaternion.Inverse(this._orientation);
+            OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(this._orientation);
+            OMV.Vector3 childRelativePosition = (prim._position - this._position) * invThisOrientation;
 
             // relative rotation of the child to the parent
-            OMV.Quaternion relativeRotation = OMV.Quaternion.Inverse(prim._orientation) * this._orientation;
+            OMV.Quaternion childRelativeRotation = invThisOrientation * prim._orientation;
 
             // this is a constraint that allows no freedom of movement between the two objects
             // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+            DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
             BulletSimAPI.AddConstraint(_scene.WorldID, LocalID, prim.LocalID, 
                 childRelativePosition,
-                relativeRotation,
+                childRelativeRotation,
                 OMV.Vector3.Zero,
                 OMV.Quaternion.Identity,
                 OMV.Vector3.Zero, OMV.Vector3.Zero,
@@ -1252,78 +1356,71 @@ public sealed class BSPrim : PhysicsActor
     const float POSITION_TOLERANCE = 0.05f;
     const float ACCELERATION_TOLERANCE = 0.01f;
     const float ROTATIONAL_VELOCITY_TOLERANCE = 0.01f;
-    const bool SHOULD_DAMP_UPDATES = false;
 
     public void UpdateProperties(EntityProperties entprop)
     {
+        /*
         UpdatedProperties changed = 0;
-        if (SHOULD_DAMP_UPDATES)
+        // assign to the local variables so the normal set action does not happen
+        // if (_position != entprop.Position)
+        if (!_position.ApproxEquals(entprop.Position, POSITION_TOLERANCE))
         {
-            // assign to the local variables so the normal set action does not happen
-            // if (_position != entprop.Position)
-            if (!_position.ApproxEquals(entprop.Position, POSITION_TOLERANCE))
-            {
-                _position = entprop.Position;
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, pos = {2}", LogHeader, LocalID, _position);
-                changed |= UpdatedProperties.Position;
-            }
-            // if (_orientation != entprop.Rotation)
-            if (!_orientation.ApproxEquals(entprop.Rotation, ROTATION_TOLERANCE))
-            {
-                _orientation = entprop.Rotation;
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, rot = {2}", LogHeader, LocalID, _orientation);
-                changed |= UpdatedProperties.Rotation;
-            }
-            // if (_velocity != entprop.Velocity)
-            if (!_velocity.ApproxEquals(entprop.Velocity, VELOCITY_TOLERANCE))
-            {
-                _velocity = entprop.Velocity;
-                // m_log.DebugFormat("{0}: UpdateProperties: velocity = {1}", LogHeader, _velocity);
-                changed |= UpdatedProperties.Velocity;
-            }
-            // if (_acceleration != entprop.Acceleration)
-            if (!_acceleration.ApproxEquals(entprop.Acceleration, ACCELERATION_TOLERANCE))
-            {
-                _acceleration = entprop.Acceleration;
-                // m_log.DebugFormat("{0}: UpdateProperties: acceleration = {1}", LogHeader, _acceleration);
-                changed |= UpdatedProperties.Acceleration;
-            }
-            // if (_rotationalVelocity != entprop.RotationalVelocity)
-            if (!_rotationalVelocity.ApproxEquals(entprop.RotationalVelocity, ROTATIONAL_VELOCITY_TOLERANCE))
-            {
-                _rotationalVelocity = entprop.RotationalVelocity;
-                // m_log.DebugFormat("{0}: UpdateProperties: rotationalVelocity = {1}", LogHeader, _rotationalVelocity);
-                changed |= UpdatedProperties.RotationalVel;
-            }
-            if (changed != 0)
-            {
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-                // Only update the position of single objects and linkset roots
-                if (this._parentPrim == null)
-                {
-                    // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-                    base.RequestPhysicsterseUpdate();
-                }
-            }
+            _position = entprop.Position;
+            changed |= UpdatedProperties.Position;
         }
-        else
+        // if (_orientation != entprop.Rotation)
+        if (!_orientation.ApproxEquals(entprop.Rotation, ROTATION_TOLERANCE))
         {
-            // Don't check for damping here -- it's done in BulletSim and SceneObjectPart.
-
-            // Only updates only for individual prims and for the root object of a linkset.
+            _orientation = entprop.Rotation;
+            changed |= UpdatedProperties.Rotation;
+        }
+        // if (_velocity != entprop.Velocity)
+        if (!_velocity.ApproxEquals(entprop.Velocity, VELOCITY_TOLERANCE))
+        {
+            _velocity = entprop.Velocity;
+            changed |= UpdatedProperties.Velocity;
+        }
+        // if (_acceleration != entprop.Acceleration)
+        if (!_acceleration.ApproxEquals(entprop.Acceleration, ACCELERATION_TOLERANCE))
+        {
+            _acceleration = entprop.Acceleration;
+            changed |= UpdatedProperties.Acceleration;
+        }
+        // if (_rotationalVelocity != entprop.RotationalVelocity)
+        if (!_rotationalVelocity.ApproxEquals(entprop.RotationalVelocity, ROTATIONAL_VELOCITY_TOLERANCE))
+        {
+            _rotationalVelocity = entprop.RotationalVelocity;
+            changed |= UpdatedProperties.RotationalVel;
+        }
+        if (changed != 0)
+        {
+            // Only update the position of single objects and linkset roots
             if (this._parentPrim == null)
             {
-                // Assign to the local variables so the normal set action does not happen
-                _position = entprop.Position;
-                _orientation = entprop.Rotation;
-                _velocity = entprop.Velocity;
-                _acceleration = entprop.Acceleration;
-                _rotationalVelocity = entprop.RotationalVelocity;
-                // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}, vel={5}, acc={6}, rvel={7}", 
-                //         LogHeader, LocalID, changed, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
                 base.RequestPhysicsterseUpdate();
             }
         }
+        */
+
+        // Don't check for damping here -- it's done in BulletSim and SceneObjectPart.
+
+        // Updates only for individual prims and for the root object of a linkset.
+        if (this._parentPrim == null)
+        {
+            // Assign to the local variables so the normal set action does not happen
+            _position = entprop.Position;
+            _orientation = entprop.Rotation;
+            _velocity = entprop.Velocity;
+            _acceleration = entprop.Acceleration;
+            _rotationalVelocity = entprop.RotationalVelocity;
+
+            // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}, vel={5}, acc={6}, rvel={7}", 
+            //         LogHeader, LocalID, changed, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+            DetailLog("{0},UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+                    LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+
+            base.RequestPhysicsterseUpdate();
+        }
     }
 
     // I've collided with something
@@ -1362,5 +1459,11 @@ public sealed class BSPrim : PhysicsActor
             collisionCollection.Clear();
         }
     }
+
+    // Invoke the detailed logger and output something if it's enabled.
+    private void DetailLog(string msg, params Object[] args)
+    {
+        Scene.PhysicsLogging.Write(msg, args);
+    }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 417cb5f..8773485 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -29,12 +29,14 @@ using System.Collections.Generic;
 using System.Runtime.InteropServices;
 using System.Text;
 using System.Threading;
-using Nini.Config;
-using log4net;
 using OpenSim.Framework;
+using OpenSim.Region.Framework;
+using OpenSim.Region.CoreModules;
+using Logging = OpenSim.Region.CoreModules.Framework.Statistics.Logging;
 using OpenSim.Region.Physics.Manager;
+using Nini.Config;
+using log4net;
 using OpenMetaverse;
-using OpenSim.Region.Framework;
 
 // TODOs for BulletSim (for BSScene, BSPrim, BSCharacter and BulletSim)
 // Debug linkset 
@@ -44,15 +46,17 @@ using OpenSim.Region.Framework;
 // Compute physics FPS reasonably
 // Based on material, set density and friction
 // More efficient memory usage when passing hull information from BSPrim to BulletSim
+// Move all logic out of the C++ code and into the C# code for easier future modifications.
 // Four states of prim: Physical, regular, phantom and selected. Are we modeling these correctly?
 //     In SL one can set both physical and phantom (gravity, does not effect others, makes collisions with ground)
 //     At the moment, physical and phantom causes object to drop through the terrain
 // Physical phantom objects and related typing (collision options )
+// Use collision masks for collision with terrain and phantom objects 
 // Check out llVolumeDetect. Must do something for that.
 // Should prim.link() and prim.delink() membership checking happen at taint time?
+// changing the position and orientation of a linked prim must rebuild the constraint with the root.
 // Mesh sharing. Use meshHash to tell if we already have a hull of that shape and only create once
 // Do attachments need to be handled separately? Need collision events. Do not collide with VolumeDetect
-// Use collision masks for collision with terrain and phantom objects 
 // Implement the genCollisions feature in BulletSim::SetObjectProperties (don't pass up unneeded collisions)
 // Implement LockAngularMotion
 // Decide if clearing forces is the right thing to do when setting position (BulletSim::SetObjectTranslation)
@@ -60,9 +64,6 @@ using OpenSim.Region.Framework;
 // Remove mesh and Hull stuff. Use mesh passed to bullet and use convexdecom from bullet.
 // Add PID movement operations. What does ScenePresence.MoveToTarget do?
 // Check terrain size. 128 or 127?
-// Multiple contact points on collision?
-//    See code in ode::near... calls to collision_accounting_events()
-//    (This might not be a problem. ODE collects all the collisions with one object in one tick.)
 // Raycast
 // 
 namespace OpenSim.Region.Physics.BulletSPlugin
@@ -72,6 +73,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
     private static readonly string LogHeader = "[BULLETS SCENE]";
 
+    private void DebugLog(string mm, params Object[] xx) { if (shouldDebugLog) m_log.DebugFormat(mm, xx); }
+
     public string BulletSimVersion = "?";
 
     private Dictionary<uint, BSCharacter> m_avatars = new Dictionary<uint, BSCharacter>();
@@ -105,6 +108,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private long m_simulationStep = 0;
     public long SimulationStep { get { return m_simulationStep; } }
 
+    public float LastSimulatedTimestep { get; private set; }
+
     // A value of the time now so all the collision and update routines do not have to get their own
     // Set to 'now' just before all the prims and actors are called for collisions and updates
     private int m_simulationNowTime;
@@ -121,6 +126,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private bool _meshSculptedPrim = true;         // cause scuplted prims to get meshed
     private bool _forceSimplePrimMeshing = false;   // if a cube or sphere, let Bullet do internal shapes
 
+    public float PID_D { get; private set; }    // derivative
+    public float PID_P { get; private set; }    // proportional
+
     public const uint TERRAIN_ID = 0;       // OpenSim senses terrain with a localID of zero
     public const uint GROUNDPLANE_ID = 1;
 
@@ -147,8 +155,20 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     ConfigurationParameters[] m_params;
     GCHandle m_paramsHandle;
 
+    public bool shouldDebugLog { get; private set; }
+
     private BulletSimAPI.DebugLogCallback m_DebugLogCallbackHandle;
 
+    // Sometimes you just have to log everything.
+    public Logging.LogWriter PhysicsLogging;
+    private bool m_physicsLoggingEnabled;
+    private string m_physicsLoggingDir;
+    private string m_physicsLoggingPrefix;
+    private int m_physicsLoggingFileMinutes;
+
+    private bool m_vehicleLoggingEnabled;
+    public bool VehicleLoggingEnabled { get { return m_vehicleLoggingEnabled; } }
+
     public BSScene(string identifier)
     {
         m_initialized = false;
@@ -169,17 +189,32 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         m_updateArray = new EntityProperties[m_maxUpdatesPerFrame];
         m_updateArrayPinnedHandle = GCHandle.Alloc(m_updateArray, GCHandleType.Pinned);
 
+        // Enable very detailed logging.
+        // By creating an empty logger when not logging, the log message invocation code
+        // can be left in and every call doesn't have to check for null.
+        if (m_physicsLoggingEnabled)
+        {
+            PhysicsLogging = new Logging.LogWriter(m_physicsLoggingDir, m_physicsLoggingPrefix, m_physicsLoggingFileMinutes);
+        }
+        else
+        {
+            PhysicsLogging = new Logging.LogWriter();
+        }
+
         // Get the version of the DLL
         // TODO: this doesn't work yet. Something wrong with marshaling the returned string.
         // BulletSimVersion = BulletSimAPI.GetVersion();
         // m_log.WarnFormat("{0}: BulletSim.dll version='{1}'", LogHeader, BulletSimVersion);
 
         // if Debug, enable logging from the unmanaged code
-        if (m_log.IsDebugEnabled)
+        if (m_log.IsDebugEnabled || PhysicsLogging.Enabled)
         {
             m_log.DebugFormat("{0}: Initialize: Setting debug callback for unmanaged code", LogHeader);
-            // the handle is saved to it doesn't get freed after this call
-            m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLogger);
+            if (PhysicsLogging.Enabled)
+                m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLoggerPhysLog);
+            else
+                m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLogger);
+            // the handle is saved in a variable to make sure it doesn't get freed after this call
             BulletSimAPI.SetDebugLogCallback(m_DebugLogCallbackHandle);
         }
 
@@ -209,6 +244,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         m_meshLOD = 8f;
         m_sculptLOD = 32f;
 
+        shouldDebugLog = false;
         m_detailedStatsStep = 0;            // disabled
 
         m_maxSubSteps = 10;
@@ -217,6 +253,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         m_maxUpdatesPerFrame = 2048;
         m_maximumObjectMass = 10000.01f;
 
+        PID_D = 2200f;
+        PID_P = 900f;
+
         parms.defaultFriction = 0.5f;
         parms.defaultDensity = 10.000006836f; // Aluminum g/cm3
         parms.defaultRestitution = 0f;
@@ -261,7 +300,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 _meshSculptedPrim = pConfig.GetBoolean("MeshSculptedPrim", _meshSculptedPrim);
                 _forceSimplePrimMeshing = pConfig.GetBoolean("ForceSimplePrimMeshing", _forceSimplePrimMeshing);
 
+                shouldDebugLog = pConfig.GetBoolean("ShouldDebugLog", shouldDebugLog);
                 m_detailedStatsStep = pConfig.GetInt("DetailedStatsStep", m_detailedStatsStep);
+
                 m_meshLOD = pConfig.GetFloat("MeshLevelOfDetail", m_meshLOD);
                 m_sculptLOD = pConfig.GetFloat("SculptLevelOfDetail", m_sculptLOD);
 
@@ -271,6 +312,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 m_maxUpdatesPerFrame = pConfig.GetInt("MaxUpdatesPerFrame", m_maxUpdatesPerFrame);
                 m_maximumObjectMass = pConfig.GetFloat("MaxObjectMass", m_maximumObjectMass);
 
+                PID_D = pConfig.GetFloat("PIDDerivative", PID_D);
+                PID_P = pConfig.GetFloat("PIDProportional", PID_P);
+
                 parms.defaultFriction = pConfig.GetFloat("DefaultFriction", parms.defaultFriction);
                 parms.defaultDensity = pConfig.GetFloat("DefaultDensity", parms.defaultDensity);
                 parms.defaultRestitution = pConfig.GetFloat("DefaultRestitution", parms.defaultRestitution);
@@ -303,6 +347,14 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                     parms.shouldSplitSimulationIslands = ParamBoolean(pConfig, "ShouldSplitSimulationIslands", parms.shouldSplitSimulationIslands);
                     parms.shouldEnableFrictionCaching = ParamBoolean(pConfig, "ShouldEnableFrictionCaching", parms.shouldEnableFrictionCaching);
                     parms.numberOfSolverIterations = pConfig.GetFloat("NumberOfSolverIterations", parms.numberOfSolverIterations);
+
+                // Very detailed logging for physics debugging
+                m_physicsLoggingEnabled = pConfig.GetBoolean("PhysicsLoggingEnabled", false);
+                m_physicsLoggingDir = pConfig.GetString("PhysicsLoggingDir", ".");
+                m_physicsLoggingPrefix = pConfig.GetString("PhysicsLoggingPrefix", "physics-");
+                m_physicsLoggingFileMinutes = pConfig.GetInt("PhysicsLoggingFileMinutes", 5);
+                // Very detailed logging for vehicle debugging
+                m_vehicleLoggingEnabled = pConfig.GetBoolean("VehicleLoggingEnabled", false);
             }
         }
         m_params[0] = parms;
@@ -323,12 +375,17 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         return ret;
     }
 
-
     // Called directly from unmanaged code so don't do much
     private void BulletLogger(string msg)
     {
         m_log.Debug("[BULLETS UNMANAGED]:" + msg);
     }
+    
+    // Called directly from unmanaged code so don't do much
+    private void BulletLoggerPhysLog(string msg)
+    {
+        PhysicsLogging.Write("[BULLETS UNMANAGED]:" + msg);
+    }
 
     public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
     {
@@ -347,34 +404,42 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     public override void RemoveAvatar(PhysicsActor actor)
     {
         // m_log.DebugFormat("{0}: RemoveAvatar", LogHeader);
-        if (actor is BSCharacter)
-        {
-            ((BSCharacter)actor).Destroy();
-        }
-        try
+        BSCharacter bsactor = actor as BSCharacter;
+        if (bsactor != null)
         {
-            lock (m_avatars) m_avatars.Remove(actor.LocalID);
-        }
-        catch (Exception e)
-        {
-            m_log.WarnFormat("{0}: Attempt to remove avatar that is not in physics scene: {1}", LogHeader, e);
+            try
+            {
+                lock (m_avatars) m_avatars.Remove(actor.LocalID);
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("{0}: Attempt to remove avatar that is not in physics scene: {1}", LogHeader, e);
+            }
+            bsactor.Destroy();
+            // bsactor.dispose();
         }
     }
 
     public override void RemovePrim(PhysicsActor prim)
     {
-        // m_log.DebugFormat("{0}: RemovePrim", LogHeader);
-        if (prim is BSPrim)
-        {
-            ((BSPrim)prim).Destroy();
-        }
-        try
+        BSPrim bsprim = prim as BSPrim;
+        if (bsprim != null)
         {
-            lock (m_prims) m_prims.Remove(prim.LocalID);
+            m_log.DebugFormat("{0}: RemovePrim. id={1}/{2}", LogHeader, bsprim.Name, bsprim.LocalID);
+            try
+            {
+                lock (m_prims) m_prims.Remove(bsprim.LocalID);
+            }
+            catch (Exception e)
+            {
+                m_log.ErrorFormat("{0}: Attempt to remove prim that is not in physics scene: {1}", LogHeader, e);
+            }
+            bsprim.Destroy();
+            // bsprim.dispose();
         }
-        catch (Exception e)
+        else
         {
-            m_log.WarnFormat("{0}: Attempt to remove prim that is not in physics scene: {1}", LogHeader, e);
+            m_log.ErrorFormat("{0}: Attempt to remove prim that is not a BSPrim type.", LogHeader);
         }
     }
 
@@ -400,6 +465,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         int collidersCount;
         IntPtr collidersPtr;
 
+        LastSimulatedTimestep = timeStep;
+
         // prevent simulation until we've been initialized
         if (!m_initialized) return 10.0f;
 
@@ -459,7 +526,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             for (int ii = 0; ii < updatedEntityCount; ii++)
             {
                 EntityProperties entprop = m_updateArray[ii];
-                // m_log.DebugFormat("{0}: entprop[{1}]: id={2}, pos={3}", LogHeader, ii, entprop.ID, entprop.Position);
                 BSPrim prim;
                 if (m_prims.TryGetValue(entprop.ID, out prim))
                 {
@@ -532,8 +598,17 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         });
     }
 
+    // Someday we will have complex terrain with caves and tunnels
+    // For the moment, it's flat and convex
+    public float GetTerrainHeightAtXYZ(Vector3 loc)
+    {
+        return GetTerrainHeightAtXY(loc.X, loc.Y);
+    }
+
     public float GetTerrainHeightAtXY(float tX, float tY)
     {
+        if (tX < 0 || tX >= Constants.RegionSize || tY < 0 || tY >= Constants.RegionSize)
+            return 30;
         return m_heightMap[((int)tX) * Constants.RegionSize + ((int)tY)];
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 086f0dc..babb707 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -146,6 +146,22 @@ public struct ConfigurationParameters
     public const float numericFalse = 0f;
 }
 
+// Values used by Bullet and BulletSim to control collisions
+public enum CollisionFlags : uint
+{
+    STATIC_OBJECT                 = 1 << 0,
+    KINEMATIC_OBJECT              = 1 << 1,
+    NO_CONTACT_RESPONSE           = 1 << 2,
+    CUSTOM_MATERIAL_CALLBACK      = 1 << 3,
+    CHARACTER_OBJECT              = 1 << 4,
+    DISABLE_VISUALIZE_OBJECT      = 1 << 5,
+    DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
+    // Following used by BulletSim to control collisions
+    VOLUME_DETECT_OBJECT          = 1 << 10,
+    PHANTOM_OBJECT                = 1 << 11,
+    PHYSICAL_OBJECT               = 1 << 12,
+};
+
 static class BulletSimAPI {
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -214,6 +230,9 @@ public static extern bool RemoveConstraint(uint worldID, uint id1, uint id2);
 public static extern Vector3 GetObjectPosition(uint WorldID, uint id);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Quaternion GetObjectOrientation(uint WorldID, uint id);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool SetObjectTranslation(uint worldID, uint id, Vector3 position, Quaternion rotation);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -268,5 +287,37 @@ public static extern void DumpBulletStatistics();
 public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void SetDebugLogCallback(DebugLogCallback callback);
+
+// ===============================================================================
+// ===============================================================================
+// ===============================================================================
+// A new version of the API that moves all the logic out of the C++ code and into
+//    the C# code. This will make modifications easier for the next person.
+// This interface passes the actual pointers to the objects in the unmanaged
+//    address space. All the management (calls for creation/destruction/lookup)
+//    is done in the C# code.
+// The names have a 2 tacked on. This will be removed as the code gets rebuilt
+//    and the old code is removed from the C# code.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetSimHandle2(uint worldID);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetBodyHandleWorldID2(uint worldID, uint id);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetBodyHandle2(IntPtr sim, uint id);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr ClearForces2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr SetCollisionFlags2(IntPtr obj, uint flags);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr AddToCollisionFlags2(IntPtr obj, uint flags);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr RemoveFromCollisionFlags2(IntPtr obj, uint flags);
+
 }
 }