10 files changed, 341 insertions, 156 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index b88ec3c..2a52e01 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -218,6 +218,18 @@ public class BSCharacter : BSPhysObject
             });
         }
     }
+    public override OMV.Vector3 ForcePosition {
+        get {
+            _position = BulletSimAPI.GetPosition2(BSBody.ptr);
+            return _position;
+        }
+        set {
+            _position = value;
+            PositionSanityCheck();
+            BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _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.
@@ -234,6 +246,15 @@ public class BSCharacter : BSPhysObject
             _position.Z = terrainHeight + 2.0f;
             ret = true;
         }
+        if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
+        {
+            float waterHeight = PhysicsScene.GetWaterLevelAtXYZ(_position);
+            if (Position.Z < waterHeight)
+            {
+                _position.Z = waterHeight;
+                ret = true;
+            }
+        }
 
         // TODO: check for out of bounds
         return ret;
@@ -242,18 +263,22 @@ public class BSCharacter : BSPhysObject
     // A version of the sanity check that also makes sure a new position value is
     //    pushed back to the physics engine. This routine would be used by anyone
     //    who is not already pushing the value.
-    private bool PositionSanityCheck2()
+    private bool PositionSanityCheck2(bool atTaintTime)
     {
         bool ret = false;
         if (PositionSanityCheck())
         {
             // The new position value must be pushed into the physics engine but we can't
             //    just assign to "Position" because of potential call loops.
-            PhysicsScene.TaintedObject("BSCharacter.PositionSanityCheck", delegate()
+            BSScene.TaintCallback sanityOperation = delegate()
             {
                 DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(PhysicsScene.WorldID, LocalID, _position, _orientation);
-            });
+            };
+            if (atTaintTime)
+                sanityOperation();
+            else
+                PhysicsScene.TaintedObject("BSCharacter.PositionSanityCheck", sanityOperation);
             ret = true;
         }
         return ret;
@@ -333,6 +358,20 @@ public class BSCharacter : BSPhysObject
             });
         }
     }
+    // Go directly to Bullet to get/set the value. 
+    public override OMV.Quaternion ForceOrientation
+    {
+        get
+        {
+            _orientation = BulletSimAPI.GetOrientation2(BSBody.ptr);
+            return _orientation;
+        }
+        set
+        {
+            _orientation = value;
+            BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+        }
+    }
     public override int PhysicsActorType {
         get { return _physicsActorType; }
         set { _physicsActorType = value;
@@ -378,7 +417,16 @@ public class BSCharacter : BSPhysObject
         set { _collidingObj = value; }
     }
     public override bool FloatOnWater {
-        set { _floatOnWater = value; }
+        set { 
+            _floatOnWater = value;
+            PhysicsScene.TaintedObject("BSCharacter.setFloatOnWater", delegate()
+            {
+                if (_floatOnWater)
+                    CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                else
+                    CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+            });
+        }
     }
     public override OMV.Vector3 RotationalVelocity {
         get { return _rotationalVelocity; }
@@ -493,15 +541,14 @@ public class BSCharacter : BSPhysObject
         _velocity = entprop.Velocity;
         _acceleration = entprop.Acceleration;
         _rotationalVelocity = entprop.RotationalVelocity;
+        // Do some sanity checking for the avatar. Make sure it's above ground and inbounds.
+        PositionSanityCheck2(true);
+
         // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
         // base.RequestPhysicsterseUpdate();
 
-        // Do some sanity checking for the avatar. Make sure it's above ground and inbounds.
-        PositionSanityCheck2();
-
-        float heightHere = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);   // only for debug
-        DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5},terrain={6}",
-                LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity, heightHere);
+        DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+                LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 65b38d6..4ba2f62 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -92,7 +92,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         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
- //       private Vector3 m_lastVertAttractor = Vector3.Zero;             // what VA was last applied to body
+        private Vector3 m_lastVertAttractor = Vector3.Zero;             // what VA was last applied to body
 
         //Deflection properties
         // private float m_angularDeflectionEfficiency = 0;
@@ -138,74 +138,55 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             switch (pParam)
             {
                 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    // m_angularDeflectionEfficiency = pValue;
+                    // m_angularDeflectionEfficiency = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    // m_angularDeflectionTimescale = pValue;
+                    // m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    m_angularMotorDecayTimescale = pValue;
+                    m_angularMotorDecayTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    m_angularMotorTimescale = pValue;
+                    m_angularMotorTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.BANKING_EFFICIENCY:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    // m_bankingEfficiency = pValue;
+                    // m_bankingEfficiency = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.BANKING_MIX:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    // m_bankingMix = pValue;
+                    // m_bankingMix = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.BANKING_TIMESCALE:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    // m_bankingTimescale = pValue;
+                    // m_bankingTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.BUOYANCY:
-                    if (pValue < -1f) pValue = -1f;
-                    if (pValue > 1f) pValue = 1f;
-                    m_VehicleBuoyancy = pValue;
+                    m_VehicleBuoyancy = Math.Max(-1f, Math.Min(pValue, 1f));
                     break;
 //                case Vehicle.HOVER_EFFICIENCY:
-//                    if (pValue < 0f) pValue = 0f;
-//                    if (pValue > 1f) pValue = 1f;
-//                    m_VhoverEfficiency = pValue;
+//                    m_VhoverEfficiency = Math.Max(0f, Math.Min(pValue, 1f));
 //                    break;
                 case Vehicle.HOVER_HEIGHT:
                     m_VhoverHeight = pValue;
                     break;
                 case Vehicle.HOVER_TIMESCALE:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    m_VhoverTimescale = pValue;
+                    m_VhoverTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    // m_linearDeflectionEfficiency = pValue;
+                    // m_linearDeflectionEfficiency = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    // m_linearDeflectionTimescale = pValue;
+                    // m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    m_linearMotorDecayTimescale = pValue;
+                    m_linearMotorDecayTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.LINEAR_MOTOR_TIMESCALE:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    m_linearMotorTimescale = pValue;
+                    m_linearMotorTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
-                    if (pValue < 0.1f) pValue = 0.1f;    // Less goes unstable
-                    if (pValue > 1.0f) pValue = 1.0f;
-                    m_verticalAttractionEfficiency = pValue;
+                    m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f));
                     break;
                 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
-                    if (pValue < 0.01f) pValue = 0.01f;
-                    m_verticalAttractionTimescale = pValue;
+                    m_verticalAttractionTimescale = Math.Max(pValue, 0.01f);
                     break;
 
                 // These are vector properties but the engine lets you use a single float value to
@@ -371,8 +352,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 1;
                     // m_bankingTimescale = 1;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
-                                VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP 
+                                | VehicleFlag.LIMIT_ROLL_ONLY
+                                | VehicleFlag.LIMIT_MOTOR_UP);
                     m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     m_flags |= (VehicleFlag.HOVER_UP_ONLY);
                     break;
@@ -399,12 +381,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.8f;
                     // m_bankingTimescale = 1;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
-                            VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
-                    m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
-                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
-                                VehicleFlag.LIMIT_MOTOR_UP);
-                    m_flags |= (VehicleFlag.HOVER_WATER_ONLY);
+                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY
+                                    | VehicleFlag.HOVER_GLOBAL_HEIGHT 
+                                    | VehicleFlag.LIMIT_ROLL_ONLY
+                                    | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP
+                                    | VehicleFlag.LIMIT_MOTOR_UP
+                                    | VehicleFlag.HOVER_WATER_ONLY);
                     break;
                 case Vehicle.TYPE_AIRPLANE:
                     m_linearFrictionTimescale = new Vector3(200, 10, 5);
@@ -429,9 +412,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.7f;
                     // m_bankingTimescale = 2;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
-                        VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
-                    m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
+                                    | VehicleFlag.HOVER_TERRAIN_ONLY
+                                    | VehicleFlag.HOVER_GLOBAL_HEIGHT
+                                    | VehicleFlag.HOVER_UP_ONLY
+                                    | VehicleFlag.NO_DEFLECTION_UP
+                                    | VehicleFlag.LIMIT_MOTOR_UP);
                     m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
                     break;
                 case Vehicle.TYPE_BALLOON:
@@ -457,11 +443,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.7f;
                     // m_bankingTimescale = 5;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
-                        VehicleFlag.HOVER_UP_ONLY);
-                    m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
-                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
-                    m_flags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
+                                    | VehicleFlag.HOVER_TERRAIN_ONLY
+                                    | VehicleFlag.HOVER_UP_ONLY
+                                    | VehicleFlag.NO_DEFLECTION_UP
+                                    | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY
+                                    | VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     break;
             }
         }//end SetDefaultsForType
@@ -470,7 +458,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // Do any updating needed for a vehicle
         public void Refresh()
         {
-            if (Type == Vehicle.TYPE_NONE) return;
+            if (!IsActive) 
+                return;
 
             // Set the prim's inertia to zero. The vehicle code handles that and this
             //    removes the torque action introduced by Bullet.
@@ -489,7 +478,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             LimitRotation(pTimestep);
 
             // remember the position so next step we can limit absolute movement effects
-            m_lastPositionVector = Prim.Position;
+            m_lastPositionVector = Prim.ForcePosition;
 
             VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
                     Prim.LocalID, Prim.Position, Prim.Force, Prim.Velocity, Prim.RotationalVelocity);
@@ -543,7 +532,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
 
             // convert requested object velocity to object relative vector
-            Quaternion rotq = Prim.Orientation;
+            Quaternion rotq = Prim.ForceOrientation;
             m_newVelocity = m_lastLinearVelocityVector * rotq;
 
             // Add the various forces into m_dir which will be our new direction vector (velocity)
@@ -560,19 +549,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
              */
 
-            Vector3 pos = Prim.Position;
+            Vector3 pos = Prim.ForcePosition;
 //            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);
 
             // If below the terrain, move us above the ground a little.
             float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
             // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
             //     Need to add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
-            // Vector3 rotatedSize = m_prim.Size * m_prim.Orientation;
+            // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
             // if (rotatedSize.Z < terrainHeight)
             if (pos.Z < terrainHeight)
             {
                 pos.Z = terrainHeight + 2;
-                Prim.Position = pos;
+                Prim.ForcePosition = pos;
                 VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
             }
 
@@ -602,7 +591,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 {
                     if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
                     {
-                        Prim.Position = pos;
+                        Prim.ForcePosition = pos;
                     }
                 }
                 else
@@ -654,12 +643,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
                 if (changed)
                 {
-                    Prim.Position = pos;
+                    Prim.ForcePosition = pos;
                     VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
                                 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
                 }
             }
 
+            // Limit absolute vertical change
             float Zchange = Math.Abs(posChange.Z);
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
@@ -678,6 +668,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     grav.Z = (float)(grav.Z * 1.037125);
                 VDetailLog("{0},MoveLinear,limitMotorUp,grav={1}", Prim.LocalID, grav);
             }
+
+            // If not changing some axis, reduce out velocity
             if ((m_flags & (VehicleFlag.NO_X)) != 0)
                 m_newVelocity.X = 0;
             if ((m_flags & (VehicleFlag.NO_Y)) != 0)
@@ -720,19 +712,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 //    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;
+                Vector3 origVel = m_angularMotorVelocity;
+                Vector3 origDir = m_angularMotorDirection;
+
                 // ramp up to new value
-                //   current velocity    +=                         error                          /    (  time to get there   / step interval)
+                //       new velocity    +=                         error                          /    (  time to get there   / step interval)
                 //                               requested speed       -       last motor speed
                 m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) /  (m_angularMotorTimescale / pTimestep);
                 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) /  (m_angularMotorTimescale / pTimestep);
                 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) /  (m_angularMotorTimescale / pTimestep);
 
-                VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},dir={5},vel={6}",
-                        Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
+                VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},origDir={5},vel={6}",
+                        Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
 
-                // This is done so that if script request rate is less than phys frame rate the expected
-                //    velocity may still be acheived.
                 m_angularMotorApply--;
             }
             else
@@ -746,25 +738,32 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
             // Vertical attractor section
             Vector3 vertattr = Vector3.Zero;
-            if (m_verticalAttractionTimescale < 300)
+            Vector3 deflection = Vector3.Zero;
+            Vector3 banking = Vector3.Zero;
+
+            if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
             {
                 float VAservo = 0.2f / (m_verticalAttractionTimescale / pTimestep);
+                VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
+
                 // get present body rotation
-                Quaternion rotq = Prim.Orientation;
-                // make a vector pointing up
+                Quaternion rotq = Prim.ForceOrientation;
+                // vector pointing up
                 Vector3 verterr = Vector3.Zero;
                 verterr.Z = 1.0f;
+
                 // rotate it to Body Angle
                 verterr = verterr * rotq;
-                // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
+                // verterr.X and .Y are the World error amounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
                 // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
                 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
+
+                // Error is 0 (no error) to +/- 2 (max error)
                 if (verterr.Z < 0.0f)
                 {
                     verterr.X = 2.0f - verterr.X;
                     verterr.Y = 2.0f - verterr.Y;
                 }
-                // Error is 0 (no error) to +/- 2 (max error)
                 // scale it by VAservo
                 verterr = verterr * VAservo;
 
@@ -784,7 +783,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
             } // else vertical attractor is off
 
-            // m_lastVertAttractor = vertattr;
+            m_lastVertAttractor = vertattr;
 
             // Bank section tba
 
@@ -818,7 +817,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void LimitRotation(float timestep)
         {
-            Quaternion rotq = Prim.Orientation;
+            Quaternion rotq = Prim.ForceOrientation;
             Quaternion m_rot = rotq;
             bool changed = false;
             if (m_RollreferenceFrame != Quaternion.Identity)
@@ -853,7 +852,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
             if (changed)
             {
-                Prim.Orientation = m_rot;
+                Prim.ForceOrientation = m_rot;
                 VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
             }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 4f225ae..3e82642 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -34,7 +34,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public class BSLinkset
 {
-    private static string LogHeader = "[BULLETSIM LINKSET]";
+    // private static string LogHeader = "[BULLETSIM LINKSET]";
 
     public BSPhysObject LinksetRoot { get; protected set; }
 
@@ -331,21 +331,21 @@ public class BSLinkset
             m_children.Add(child);
 
             BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
-            BulletBody rootBodyx = LinksetRoot.BSBody;
             BSPhysObject childx = child;
-            BulletBody childBodyx = child.BSBody;
 
             DetailLog("{0},AddChildToLinkset,call,rID={1},rBody={2},cID={3},cBody={4}", 
                             rootx.LocalID,
-                            rootx.LocalID, rootBodyx.ptr.ToString("X"),
-                            childx.LocalID, childBodyx.ptr.ToString("X"));
+                            rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
+                            childx.LocalID, childx.BSBody.ptr.ToString("X"));
 
             PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
             {
                 DetailLog("{0},AddChildToLinkset,taint,child={1}", LinksetRoot.LocalID, child.LocalID);
                 // build the physical binding between me and the child
                 m_taintChildren.Add(childx);
-                PhysicallyLinkAChildToRoot(rootx, rootBodyx, childx, childBodyx);
+
+                // Since this is taint-time, the body and shape could have changed for the child
+                PhysicallyLinkAChildToRoot(rootx, rootx.BSBody, childx, childx.BSBody);
             });
         }
         return;
@@ -369,21 +369,19 @@ public class BSLinkset
         if (m_children.Remove(child))
         {
             BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
-            BulletBody rootBodyx = LinksetRoot.BSBody;
             BSPhysObject childx = child;
-            BulletBody childBodyx = child.BSBody;
 
             DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}", 
                             childx.LocalID,
-                            rootx.LocalID, rootBodyx.ptr.ToString("X"),
-                            childx.LocalID, childBodyx.ptr.ToString("X"));
+                            rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
+                            childx.LocalID, childx.BSBody.ptr.ToString("X"));
 
             PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
             {
                 if (m_taintChildren.Contains(childx))
                     m_taintChildren.Remove(childx);
 
-                PhysicallyUnlinkAChildFromRoot(rootx, rootBodyx, childx, childBodyx);
+                PhysicallyUnlinkAChildFromRoot(rootx, rootx.BSBody, childx, childx.BSBody);
                 RecomputeLinksetConstraintVariables();
             });
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index d9b738b..1ac8c59 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -81,6 +81,10 @@ public abstract class BSPhysObject : PhysicsActor
     // Tell the object to clean up.
     public abstract void Destroy();
 
+    public abstract OMV.Vector3 ForcePosition { get; set; }
+
+    public abstract OMV.Quaternion ForceOrientation { get; set; }
+
     #region Collisions
 
     // Requested number of milliseconds between collision events. Zero means disabled.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index a0e627e..f7b68ba 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -46,19 +46,13 @@ public sealed class BSPrim : BSPhysObject
     private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
     private static readonly string LogHeader = "[BULLETS PRIM]";
 
-    private IMesh _mesh;
     private PrimitiveBaseShape _pbs;
-    private ShapeData.PhysicsShapeType _shapeType;
-    private ulong _meshKey;
-    private ulong _hullKey;
-    private List<ConvexResult> _hulls;
 
     // _size is what the user passed. _scale is what we pass to the physics engine with the mesh.
     // Often _scale is unity because the meshmerizer will apply _size when creating the mesh.
     private OMV.Vector3 _size;  // the multiplier for each mesh dimension as passed by the user
     private OMV.Vector3 _scale; // the multiplier for each mesh dimension for the mesh as created by the meshmerizer
 
-    private bool _stopped;
     private bool _grabbed;
     private bool _isSelected;
     private bool _isVolumeDetect;
@@ -109,8 +103,6 @@ public sealed class BSPrim : BSPhysObject
         _buoyancy = 1f;
         _velocity = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
-        _hullKey = 0;
-        _meshKey = 0;
         _pbs = pbs;
         _isPhysical = pisPhysical;
         _isVolumeDetect = false;
@@ -160,8 +152,9 @@ public sealed class BSPrim : BSPhysObject
         });
     }
 
+    // No one uses this property.
     public override bool Stopped {
-        get { return _stopped; }
+        get { return false; }
     }
     public override OMV.Vector3 Size {
         get { return _size; }
@@ -274,6 +267,7 @@ public sealed class BSPrim : BSPhysObject
         set {
             _position = value;
             // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
+            PositionSanityCheck();
             PhysicsScene.TaintedObject("BSPrim.setPosition", delegate()
             {
                 // DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
@@ -281,6 +275,74 @@ public sealed class BSPrim : BSPhysObject
             });
         }
     }
+    public override OMV.Vector3 ForcePosition {
+        get {
+            _position = BulletSimAPI.GetPosition2(BSBody.ptr);
+            return _position;
+        }
+        set {
+            _position = value;
+            PositionSanityCheck();
+            BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _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.
+    // Returns 'true' of the position was made sane by some action.
+    private bool PositionSanityCheck()
+    {
+        bool ret = false;
+      
+        // If totally below the ground, move the prim up
+        // TODO: figure out the right solution for this... only for dynamic objects?
+        /*
+        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
+        if (Position.Z < terrainHeight)
+        {
+            DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
+            _position.Z = terrainHeight + 2.0f;
+            ret = true;
+        }
+         */
+        if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
+        {
+            float waterHeight = PhysicsScene.GetWaterLevelAtXYZ(_position);
+            if (Position.Z < waterHeight)
+            {
+                _position.Z = waterHeight;
+                ret = true;
+            }
+        }
+
+        // TODO: check for out of bounds
+        return ret;
+    }
+
+    // A version of the sanity check that also makes sure a new position value is
+    //    pushed back to the physics engine. This routine would be used by anyone
+    //    who is not already pushing the value.
+    private bool PositionSanityCheck2(bool atTaintTime)
+    {
+        bool ret = false;
+        if (PositionSanityCheck())
+        {
+            // The new position value must be pushed into the physics engine but we can't
+            //    just assign to "Position" because of potential call loops.
+            BSScene.TaintCallback sanityOperation = delegate()
+            {
+                DetailLog("{0},BSPrim.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                BulletSimAPI.SetObjectTranslation(PhysicsScene.WorldID, LocalID, _position, _orientation);
+            };
+            if (atTaintTime)
+                sanityOperation();
+            else
+                PhysicsScene.TaintedObject("BSPrim.PositionSanityCheck", sanityOperation);
+
+            ret = true;
+        }
+        return ret;
+    }
 
     // Return the effective mass of the object.
     // If there are multiple items in the linkset, add them together for the root
@@ -326,14 +388,15 @@ public sealed class BSPrim : BSPhysObject
         }
         set {
             Vehicle type = (Vehicle)value;
-            BSPrim vehiclePrim = this;
+
+            // Tell the scene about the vehicle so it will get processing each frame.
+            PhysicsScene.VehicleInSceneTypeChanged(this, type);
+
             PhysicsScene.TaintedObject("setVehicleType", delegate()
             {
                 // Done at taint time so we're sure the physics engine is not using the variables
                 // Vehicle code changes the parameters for this vehicle type.
-                _vehicle.ProcessTypeChange(type);
-                // Tell the scene about the vehicle so it will get processing each frame.
-                PhysicsScene.VehicleInSceneTypeChanged(this, type);
+                this._vehicle.ProcessTypeChange(type);
             });
         }
     }
@@ -371,7 +434,9 @@ public sealed class BSPrim : BSPhysObject
     public override void StepVehicle(float timeStep)
     {
         if (IsPhysical)
+        {
             _vehicle.Step(timeStep);
+        }
     }
 
     // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more
@@ -435,6 +500,20 @@ public sealed class BSPrim : BSPhysObject
             });
         }
     }
+    // Go directly to Bullet to get/set the value. 
+    public override OMV.Quaternion ForceOrientation
+    {
+        get
+        {
+            _orientation = BulletSimAPI.GetOrientation2(BSBody.ptr);
+            return _orientation;
+        }
+        set
+        {
+            _orientation = value;
+            BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+        }
+    }
     public override int PhysicsActorType {
         get { return _physicsActorType; }
         set { _physicsActorType = value; }
@@ -488,11 +567,10 @@ public sealed class BSPrim : BSPhysObject
         // This is a NOOP if the object is not in the world (BulletSim and Bullet ignore objects not found).
         BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, BSBody.ptr);
 
-
         // Set up the object physicalness (does gravity and collisions move this object)
         MakeDynamic(IsStatic);
 
-        // Do any vehicle stuff
+        // Update vehicle specific parameters
         _vehicle.Refresh();
 
         // Arrange for collision events if the simulator wants them
@@ -563,7 +641,6 @@ public sealed class BSPrim : BSPhysObject
             // A dynamic object has mass
             IntPtr collisionShapePtr = BulletSimAPI.GetCollisionShape2(BSBody.ptr);
             OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(collisionShapePtr, Mass);
-            // OMV.Vector3 inertia = OMV.Vector3.Zero;
             BulletSimAPI.SetMassProps2(BSBody.ptr, _mass, inertia);
             BulletSimAPI.UpdateInertiaTensor2(BSBody.ptr);
 
@@ -573,7 +650,7 @@ public sealed class BSPrim : BSPhysObject
             BulletSimAPI.SetSleepingThresholds2(BSBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
             BulletSimAPI.SetContactProcessingThreshold2(BSBody.ptr, PhysicsScene.Params.contactProcessingThreshold);
 
-            // There can be special things needed for implementing linksets.
+            // There might be special things needed for implementing linksets.
             Linkset.MakeDynamic(this);
 
             // Force activation of the object so Bullet will act on it.
@@ -663,7 +740,16 @@ public sealed class BSPrim : BSPhysObject
         }
     }
     public override bool FloatOnWater {
-        set { _floatOnWater = value; }
+        set { 
+            _floatOnWater = value;
+            PhysicsScene.TaintedObject("BSPrim.setFloatOnWater", delegate()
+            {
+                if (_floatOnWater)
+                    CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                else
+                    CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+            });
+        }
     }
     public override OMV.Vector3 RotationalVelocity {
         get {
@@ -1082,15 +1168,15 @@ public sealed class BSPrim : BSPhysObject
     public void FillShapeInfo(out ShapeData shape)
     {
         shape.ID = LocalID;
-        shape.Type = _shapeType;
+        shape.Type = ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;
         shape.Position = _position;
         shape.Rotation = _orientation;
         shape.Velocity = _velocity;
         shape.Scale = _scale;
         shape.Mass = _isPhysical ? _mass : 0f;
         shape.Buoyancy = _buoyancy;
-        shape.HullKey = _hullKey;
-        shape.MeshKey = _meshKey;
+        shape.HullKey = 0;
+        shape.MeshKey = 0;
         shape.Friction = _friction;
         shape.Restitution = _restitution;
         shape.Collidable = (!IsPhantom) ? ShapeData.numericTrue : ShapeData.numericFalse;
@@ -1112,7 +1198,8 @@ public sealed class BSPrim : BSPhysObject
 
         // Create the correct physical representation for this type of object.
         // Updates BSBody and BSShape with the new information.
-        PhysicsScene.Shapes.GetBodyAndShape(forceRebuild, PhysicsScene.World, this, shapeData, _pbs, 
+        // Ignore 'forceRebuild'. This routine makes the right choices and changes of necessary.
+        PhysicsScene.Shapes.GetBodyAndShape(false, PhysicsScene.World, this, shapeData, _pbs, 
                         null, delegate(BulletBody dBody)
         {
             // Called if the current prim body is about to be destroyed.
@@ -1205,6 +1292,8 @@ public sealed class BSPrim : BSPhysObject
             _acceleration = entprop.Acceleration;
             _rotationalVelocity = entprop.RotationalVelocity;
 
+            PositionSanityCheck2(true);
+
             DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
                     LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 0cf8c91..aaed7de 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -493,6 +493,10 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         // step the physical world one interval
         m_simulationStep++;
         int numSubSteps = 0;
+
+        // Sometimes needed for debugging to find out what happened before the step
+        // PhysicsLogging.Flush();
+
         try
         {
             if (PhysicsLogging.Enabled) beforeTime = Util.EnvironmentTickCount();
@@ -536,7 +540,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         }
 
         // This is a kludge to get avatar movement updates.
-        //   ODE sends collisions for avatars even if there are have been no collisions. This updates
+        //   the simulator expects collisions for avatars even if there are have been no collisions. This updates
         //   avatar animations and stuff.
         // If you fix avatar animation updates, remove this overhead and let normal collision processing happen.
         foreach (BSPhysObject bsp in m_avatars)
@@ -556,7 +560,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         }
 
         // Objects that are done colliding are removed from the ObjectsWithCollisions list.
-        // This can't be done by SendCollisions because it is inside an iteration of ObjectWithCollisions.
+        // Not done above because it is inside an iteration of ObjectWithCollisions.
         if (ObjectsWithNoMoreCollisions.Count > 0)
         {
             foreach (BSPhysObject po in ObjectsWithNoMoreCollisions)
@@ -726,13 +730,10 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     public void VehicleInSceneTypeChanged(BSPrim vehic, Vehicle newType)
     {
-        if (newType == Vehicle.TYPE_NONE)
-        {
-            RemoveVehiclePrim(vehic);
-        }
-        else
+        RemoveVehiclePrim(vehic);
+        if (newType != Vehicle.TYPE_NONE)
         {
-            // make it so the scene will call us each tick to do vehicle things
+           // make it so the scene will call us each tick to do vehicle things
            AddVehiclePrim(vehic);
         }
     }
@@ -764,7 +765,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     }
 
     // Some prims have extra vehicle actions
-    // no locking because only called when physics engine is not busy
+    // Called at taint time!
     private void ProcessVehicles(float timeStep)
     {
         foreach (BSPhysObject pobj in m_vehicles)
@@ -1008,12 +1009,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
 
             new ParameterDefn("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)",
-            0f,     // zero to disable
+            0f,
             (s,cf,p,v) => { s.m_params[0].maxPersistantManifoldPoolSize = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].maxPersistantManifoldPoolSize; },
             (s,p,l,v) => { s.m_params[0].maxPersistantManifoldPoolSize = v; } ),
             new ParameterDefn("MaxCollisionAlgorithmPoolSize", "Number of collisions pooled (0 means default of 4096)",
-            0f,     // zero to disable
+            0f,
             (s,cf,p,v) => { s.m_params[0].maxCollisionAlgorithmPoolSize = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].maxCollisionAlgorithmPoolSize; },
             (s,p,l,v) => { s.m_params[0].maxCollisionAlgorithmPoolSize = v; } ),
@@ -1028,7 +1029,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s) => { return s.m_params[0].shouldForceUpdateAllAabbs; },
             (s,p,l,v) => { s.m_params[0].shouldForceUpdateAllAabbs = v; } ),
             new ParameterDefn("ShouldRandomizeSolverOrder", "Enable for slightly better stacking interaction",
-            ConfigurationParameters.numericFalse,
+            ConfigurationParameters.numericTrue,
             (s,cf,p,v) => { s.m_params[0].shouldRandomizeSolverOrder = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
             (s) => { return s.m_params[0].shouldRandomizeSolverOrder; },
             (s,p,l,v) => { s.m_params[0].shouldRandomizeSolverOrder = v; } ),
@@ -1152,7 +1153,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     {
         if (SettableParameters.Length < ParameterDefinitions.Length)
         {
-
             List<PhysParameterEntry> entries = new List<PhysParameterEntry>();
             for (int ii = 0; ii < ParameterDefinitions.Length; ii++)
             {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index dee6243..399a133 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -36,7 +36,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public class BSShapeCollection : IDisposable
 {
-    private static string LogHeader = "[BULLETSIM SHAPE COLLECTION]";
+    // private static string LogHeader = "[BULLETSIM SHAPE COLLECTION]";
 
     protected BSScene PhysicsScene { get; set; }
 
@@ -108,7 +108,8 @@ public class BSShapeCollection : IDisposable
             // If we had to select a new shape geometry for the object,
             //    rebuild the body around it.
             // Updates prim.BSBody with information/pointers to requested body
-            bool newBody = CreateBody((newGeom || forceRebuild), prim, PhysicsScene.World, prim.BSShape, shapeData, bodyCallback);
+            bool newBody = CreateBody((newGeom || forceRebuild), prim, PhysicsScene.World, 
+                                    prim.BSShape, shapeData, bodyCallback);
             ret = newGeom || newBody;
         }
         DetailLog("{0},BSShapeCollection.GetBodyAndShape,force={1},ret={2},body={3},shape={4}",
@@ -140,7 +141,7 @@ public class BSShapeCollection : IDisposable
             bodyDesc.lastReferenced = System.DateTime.Now;
             Bodies[body.ID] = bodyDesc;
         }
-}
+    }
 
     // Release the usage of a body.
     // Called when releasing use of a BSBody. BSShape is handled separately.
@@ -167,7 +168,7 @@ public class BSShapeCollection : IDisposable
                     {
                         DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody. ptr={1}",
                                                     body.ID, body.ptr.ToString("X"));
-                        // If the caller needs to know, pass the event up.
+                        // If the caller needs to know the old body is going away, pass the event up.
                         if (bodyCallback != null) bodyCallback(body);
 
                         // Zero any reference to the shape so it is not freed when the body is deleted.
@@ -448,7 +449,8 @@ public class BSShapeCollection : IDisposable
         ulong newMeshKey = ComputeShapeKey(shapeData, pbs, out lod);
 
         // if this new shape is the same as last time, don't recreate the mesh
-        if (prim.BSShape.shapeKey == newMeshKey) return false;
+        if (newMeshKey == prim.BSShape.shapeKey && prim.BSShape.type == ShapeData.PhysicsShapeType.SHAPE_MESH)
+            return false;
 
         DetailLog("{0},BSShapeCollection.CreateGeomMesh,create,oldKey={1},newKey={2}",
                                 prim.LocalID, prim.BSShape.shapeKey.ToString("X"), newMeshKey.ToString("X"));
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 1125d7e..a43880d 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -71,7 +71,7 @@ public struct BulletBody
         buff.Append(ID.ToString());
         buff.Append(",p=");
         buff.Append(ptr.ToString("X"));
-        if (collisionFilter != 0 && collisionMask != 0)
+        if (collisionFilter != 0 || collisionMask != 0)
         {
             buff.Append(",f=");
             buff.Append(collisionFilter.ToString("X"));
@@ -344,10 +344,7 @@ public enum CollisionFlags : uint
     CF_DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
     // Following used by BulletSim to control collisions
     BS_SUBSCRIBE_COLLISION_EVENTS    = 1 << 10,
-    // BS_VOLUME_DETECT_OBJECT          = 1 << 11,
-    // BS_PHANTOM_OBJECT                = 1 << 12,
-    // BS_PHYSICAL_OBJECT               = 1 << 13,
-    // BS_TERRAIN_OBJECT                = 1 << 14,
+    BS_FLOATS_ON_WATER               = 1 << 11,
     BS_NONE                          = 0,
     BS_ALL                           = 0xFFFFFFFF,
 
@@ -356,9 +353,6 @@ public enum CollisionFlags : uint
     BS_ACTIVE = CF_STATIC_OBJECT
                 | CF_KINEMATIC_OBJECT
                 | CF_NO_CONTACT_RESPONSE
-    //            | BS_VOLUME_DETECT_OBJECT
-    //            | BS_PHANTOM_OBJECT
-    //            | BS_PHYSICAL_OBJECT,
 };
 
 // Values for collisions groups and masks
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 0716214..e7b3b2b 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -63,6 +63,9 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private bool m_isphysical;
 
+        public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } }
+        private int m_expectedCollisionContacts = 0;
+
         /// <summary>
         /// Is this prim subject to physics?  Even if not, it's still solid for collision purposes.
         /// </summary>
@@ -150,7 +153,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private PrimitiveBaseShape _pbs;
         private OdeScene _parent_scene;
-
+        
         /// <summary>
         /// The physics space which contains prim geometries
         /// </summary>
@@ -840,7 +843,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             int vertexStride, triStride;
             mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
             mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
-
+            m_expectedCollisionContacts = indexCount;
             mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
 
             // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at
@@ -1377,6 +1380,7 @@ Console.WriteLine("CreateGeom:");
                             {
 //Console.WriteLine(" CreateGeom 1");
                                 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
+                                m_expectedCollisionContacts = 3;
                             }
                             catch (AccessViolationException)
                             {
@@ -1391,6 +1395,7 @@ Console.WriteLine("CreateGeom:");
                             {
 //Console.WriteLine(" CreateGeom 2");
                                 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                                m_expectedCollisionContacts = 4;
                             }
                             catch (AccessViolationException)
                             {
@@ -1406,6 +1411,7 @@ Console.WriteLine("CreateGeom:");
                         {
 //Console.WriteLine("  CreateGeom 3");
                             SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                            m_expectedCollisionContacts = 4;
                         }
                         catch (AccessViolationException)
                         {
@@ -1421,6 +1427,7 @@ Console.WriteLine("CreateGeom:");
                     {
 //Console.WriteLine("  CreateGeom 4");
                         SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                        m_expectedCollisionContacts = 4;
                     }
                     catch (AccessViolationException)
                     {
@@ -1446,11 +1453,13 @@ Console.WriteLine("CreateGeom:");
                     _parent_scene.geom_name_map.Remove(prim_geom);
                     _parent_scene.actor_name_map.Remove(prim_geom);
                     d.GeomDestroy(prim_geom);
+                    m_expectedCollisionContacts = 0;
                     prim_geom = IntPtr.Zero;
                 }
                 catch (System.AccessViolationException)
                 {
                     prim_geom = IntPtr.Zero;
+                    m_expectedCollisionContacts = 0;
                     m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name);
 
                     return false;
@@ -2395,15 +2404,15 @@ Console.WriteLine(" JointCreateFixed");
         {
             get
             {
-                // Averate previous velocity with the new one so
+                // Average previous velocity with the new one so
                 // client object interpolation works a 'little' better
                 if (_zeroFlag)
                     return Vector3.Zero;
 
                 Vector3 returnVelocity = Vector3.Zero;
-                returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
-                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
-                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
+                returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2'
+                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f;
+                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f;
                 return returnVelocity;
             }
             set
@@ -2600,6 +2609,7 @@ Console.WriteLine(" JointCreateFixed");
             {
                 Vector3 pv = Vector3.Zero;
                 bool lastZeroFlag = _zeroFlag;
+                float m_minvelocity = 0;
                 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
                 {
                     d.Vector3 vec = d.BodyGetPosition(Body);
@@ -2752,8 +2762,21 @@ Console.WriteLine(" JointCreateFixed");
                         _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
                         _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
                         //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
+                       
+                        // Note here that linearvelocity is affecting angular velocity...  so I'm guessing this is a vehicle specific thing... 
+                        // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. 
+                        // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles
+                        // adding these logical exclusion situations to maintain this where I think it was intended to be.
+                        if (m_throttleUpdates || m_usePID || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) 
+                        {
+                            m_minvelocity = 0.5f;
+                        }
+                        else
+                        {
+                            m_minvelocity = 0.02f;
+                        }
 
-                        if (_velocity.ApproxEquals(pv, 0.5f))
+                        if (_velocity.ApproxEquals(pv, m_minvelocity))
                         {
                             m_rotationalVelocity = pv;
                         }
diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
index 929b019..7a50c4c 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
@@ -336,6 +336,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public int geomContactPointsStartthrottle = 3;
         public int geomUpdatesPerThrottledUpdate = 15;
+        private const int avatarExpectedContacts = 3;
 
         public float bodyPIDD = 35f;
         public float bodyPIDG = 25;
@@ -474,6 +475,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private OdePrim cp1;
         private OdeCharacter cc2;
         private OdePrim cp2;
+        private int p1ExpectedPoints = 0;
+        private int p2ExpectedPoints = 0;
         //private int cStartStop = 0;
         //private string cDictKey = "";
 
@@ -498,6 +501,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         public int physics_logging_interval = 0;
         public bool physics_logging_append_existing_logfile = false;
 
+
         public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
         public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
 
@@ -644,7 +648,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
 
-                    geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
+                    geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5);
                     geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
                     geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
 
@@ -1064,7 +1068,10 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             PhysicsActor p1;
             PhysicsActor p2;
-
+            
+            p1ExpectedPoints = 0;
+            p2ExpectedPoints = 0;
+            
             if (!actor_name_map.TryGetValue(g1, out p1))
             {
                 p1 = PANull;
@@ -1121,9 +1128,13 @@ namespace OpenSim.Region.Physics.OdePlugin
                 switch (p1.PhysicsActorType)
                 {
                     case (int)ActorTypes.Agent:
+                        p1ExpectedPoints = avatarExpectedContacts;
                         p2.CollidingObj = true;
                         break;
                     case (int)ActorTypes.Prim:
+                        if (p1 != null && p1 is OdePrim)
+                            p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts;
+
                         if (p2.Velocity.LengthSquared() > 0.0f)
                             p2.CollidingObj = true;
                         break;
@@ -1319,6 +1330,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
                             (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
                         {
+                            p2ExpectedPoints = avatarExpectedContacts;
                             // Avatar is moving on terrain, use the movement terrain contact
                             AvatarMovementTerrainContact.geom = curContact;
 
@@ -1332,6 +1344,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         {
                             if (p2.PhysicsActorType == (int)ActorTypes.Agent)
                             {
+                                p2ExpectedPoints = avatarExpectedContacts;
                                 // Avatar is standing on terrain, use the non moving terrain contact
                                 TerrainContact.geom = curContact;
 
@@ -1356,9 +1369,18 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     }
 
                                     if (p2 is OdePrim)
-                                        material = ((OdePrim)p2).m_material;
-
+                                    {
+                                        material = ((OdePrim) p2).m_material;
+                                        p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
+                                    }
+                                   
+                                    // Unnessesary because p1 is defined above
+                                    //if (p1 is OdePrim)
+                                    // { 
+                                    //     p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts;
+                                    // }
                                     //m_log.DebugFormat("Material: {0}", material);
+
                                     m_materialContacts[material, movintYN].geom = curContact;
 
                                     if (m_global_contactcount < maxContactsbeforedeath)
@@ -1379,7 +1401,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     int material = (int)Material.Wood;
 
                                     if (p2 is OdePrim)
+                                    {
                                         material = ((OdePrim)p2).m_material;
+                                        p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
+                                    }
 
                                     //m_log.DebugFormat("Material: {0}", material);
                                     m_materialContacts[material, movintYN].geom = curContact;
@@ -1429,6 +1454,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {
                         if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
                         {
+                            p2ExpectedPoints = avatarExpectedContacts;
                             if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
                             {
                                 // Avatar is moving on a prim, use the Movement prim contact
@@ -1458,7 +1484,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                             int material = (int)Material.Wood;
 
                             if (p2 is OdePrim)
+                            {
                                 material = ((OdePrim)p2).m_material;
+                                p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
+                            }
                             
                             //m_log.DebugFormat("Material: {0}", material);
                             m_materialContacts[material, 0].geom = curContact;
@@ -1479,8 +1508,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
 
                 collision_accounting_events(p1, p2, maxDepthContact);
-
-                if (count > geomContactPointsStartthrottle)
+                
+                if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle))
                 {
                     // If there are more then 3 contact points, it's likely
                     // that we've got a pile of objects, so ...