9 files changed, 351 insertions, 286 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index fa22c78..a9b1365 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -131,8 +131,6 @@ public class BSCharacter : BSPhysObject
             BulletSimAPI.SetObjectBuoyancy(Scene.WorldID, LocalID, _buoyancy);
 
             BSBody = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(Scene.World.Ptr, LocalID));
-            // avatars get all collisions no matter what (makes walking on ground and such work)
-            BulletSimAPI.AddToCollisionFlags2(BSBody.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
         });
             
         return;
@@ -480,11 +478,10 @@ public class BSCharacter : BSPhysObject
     // Stop collision events
     public override void UnSubscribeEvents() { 
         _subscribedEventsMs = 0;
-        // Avatars get all their collision events
-        // Scene.TaintedObject("BSCharacter.UnSubscribeEvents", delegate()
-        // {
-        //     BulletSimAPI.RemoveFromCollisionFlags2(Body.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
-        // });
+        Scene.TaintedObject("BSCharacter.UnSubscribeEvents", delegate()
+        {
+            BulletSimAPI.RemoveFromCollisionFlags2(BSBody.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+        });
     }
     // Return 'true' if someone has subscribed to events
     public override bool SubscribedEvents() {
@@ -532,10 +529,12 @@ public class BSCharacter : BSPhysObject
     // The collision, if it should be reported to the character, is placed in a collection
     //   that will later be sent to the simulator when SendCollisions() is called.
     CollisionEventUpdate collisionCollection = null;
-    public override void Collide(uint collidingWith, BSPhysObject collidee, ActorTypes type, Vector3 contactPoint, Vector3 contactNormal, float pentrationDepth)
+    public override bool Collide(uint collidingWith, BSPhysObject collidee, ActorTypes type, Vector3 contactPoint, Vector3 contactNormal, float pentrationDepth)
     {
         // m_log.DebugFormat("{0}: Collide: ms={1}, id={2}, with={3}", LogHeader, _subscribedEventsMs, LocalID, collidingWith);
 
+        bool ret = false;
+
         // The following makes IsColliding() and IsCollidingGround() work
         _collidingStep = Scene.SimulationStep;
         if (collidingWith == BSScene.TERRAIN_ID || collidingWith == BSScene.GROUNDPLANE_ID)
@@ -553,8 +552,10 @@ public class BSCharacter : BSPhysObject
                 if (collisionCollection == null)
                     collisionCollection = new CollisionEventUpdate();
                 collisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
+                ret = true;
             }
         }
+        return ret;
     }
 
     public override void SendCollisions()
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index 2e15ced..1376a29 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -74,6 +74,17 @@ public abstract class BSConstraint : IDisposable
         return ret;
     }
 
+    public virtual bool SetSolverIterations(float cnt)
+    {
+        bool ret = false;
+        if (m_enabled)
+        {
+            BulletSimAPI.SetConstraintNumSolverIterations2(m_constraint.Ptr, cnt);
+            ret = true;
+        }
+        return ret;
+    }
+
     public virtual bool CalculateTransforms()
     {
         bool ret = false;
@@ -96,12 +107,9 @@ public abstract class BSConstraint : IDisposable
             ret = CalculateTransforms();
             if (ret)
             {
-                // m_world.scene.PhysicsLogging.Write("{0},BSConstraint.RecomputeConstraintVariables,taint,enabling,A={1},B={2}",
-                //                 BSScene.DetailLogZero, Body1.ID, Body2.ID);
-
                 // Setting an object's mass to zero (making it static like when it's selected)
                 //     automatically disables the constraints.
-                // If enabled, be sure to set the constraint itself to enabled.
+                // If the link is enabled, be sure to set the constraint itself to enabled.
                 BulletSimAPI.SetConstraintEnable2(m_constraint.Ptr, m_world.scene.NumericBool(true));
             }
             else
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 8169e99..098fea7 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -80,7 +80,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // Linear properties
         private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
         private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;   // velocity requested by LSL
-        private Vector3 m_dir = Vector3.Zero;                           // velocity applied to body
+        private Vector3 m_newVelocity = Vector3.Zero;                   // velocity computed to be applied to body
         private Vector3 m_linearFrictionTimescale = Vector3.Zero;
         private float m_linearMotorDecayTimescale = 0;
         private float m_linearMotorTimescale = 0;
@@ -475,32 +475,33 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 frcount = 0;
 
             MoveLinear(pTimestep);
-            MoveAngular(pTimestep);
+            // MoveAngular(pTimestep);
             LimitRotation(pTimestep);
 
+            // remember the position so next step we can limit absolute movement effects
+            m_lastPositionVector = m_prim.Position;
+
             VDetailLog("{0},BSDynamics.Step,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)
         {
-            // requested m_linearMotorDirection is significant
-            // if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
-            if (m_linearMotorDirection.LengthSquared() > 0.0001f)
+            // m_linearMotorDirection is the direction we are moving relative to the vehicle coordinates
+            // m_lastLinearVelocityVector is the speed we are moving in that direction
+            if (m_linearMotorDirection.LengthSquared() > 0.001f)
             {
                 Vector3 origDir = m_linearMotorDirection;
                 Vector3 origVel = m_lastLinearVelocityVector;
 
                 // add drive to body
-                // Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
-                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale);
-                // lastLinearVelocityVector is the current body velocity vector?
+                // Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale / pTimestep);
+                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale / pTimestep);
+                // 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;
@@ -509,34 +510,29 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
                     m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
 
+                /*
                 // decay applied velocity
-                Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
+                Vector3 decayfraction = Vector3.One/(m_linearMotorDecayTimescale / pTimestep);
+                // (RA: do not know where the 0.5f comes from)
                 m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
-
-                /*
-                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/m_linearMotorTimescale;
-                m_lastLinearVelocityVector += addAmount;
-
-                float decayfraction = (1.0f - 1.0f / m_linearMotorDecayTimescale);
-                m_linearMotorDirection *= decayfraction;
-
                  */
+                float keepfraction = 1.0f - (1.0f / (m_linearMotorDecayTimescale / pTimestep));
+                m_linearMotorDirection *= keepfraction;
 
-                VDetailLog("{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);
+                VDetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},add={3},notDecay={4},dir={5},vel={6}",
+                    m_prim.LocalID, origDir, origVel, addAmount, keepfraction, m_linearMotorDirection, m_lastLinearVelocityVector);
             }
             else
             {
-                // 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 direction is very small, zero it.
                 m_linearMotorDirection = Vector3.Zero;
                 m_lastLinearVelocityVector = Vector3.Zero;
+                VDetailLog("{0},MoveLinear,zeroed", m_prim.LocalID);
             }
 
             // convert requested object velocity to object relative vector
             Quaternion rotq = m_prim.Orientation;
-            m_dir = m_lastLinearVelocityVector * rotq;
+            m_newVelocity = m_lastLinearVelocityVector * rotq;
 
             // Add the various forces into m_dir which will be our new direction vector (velocity)
 
@@ -544,60 +540,31 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // 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.
             // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            grav.Z = m_prim.Scene.DefaultGravity.Z * m_prim.Mass * (1f - m_VehicleBuoyancy);
+            Vector3 grav = m_prim.Scene.DefaultGravity * (m_prim.Mass * (1f - m_VehicleBuoyancy));
+
+            /*
+             * RA: Not sure why one would do this
             // 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);
-            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;
-                    changed = true;
-                }
-                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
-                {
-                    pos.Y -= posChange.Y + 1;
-                    changed = true;
-                }
-                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
-                {
-                    pos.Z -= posChange.Z + 1;
-                    changed = true;
-                }
-                if (pos.X <= 0)
-                {
-                    pos.X += posChange.X + 1;
-                    changed = true;
-                }
-                if (pos.Y <= 0)
-                {
-                    pos.Y += posChange.Y + 1;
-                    changed = true;
-                }
-                if (changed)
-                {
-                    m_prim.Position = pos;
-                    VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
-                                m_prim.LocalID, m_BlockingEndPoint, posChange, pos);
-                }
-            }
 
             // If below the terrain, move us above the ground a little.
-            if (pos.Z < m_prim.Scene.TerrainManager.GetTerrainHeightAtXYZ(pos))
+            float terrainHeight = m_prim.Scene.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;
+            // if (rotatedSize.Z < terrainHeight)
+            if (pos.Z < terrainHeight)
             {
-                pos.Z = m_prim.Scene.TerrainManager.GetTerrainHeightAtXYZ(pos) + 2;
+                pos.Z = terrainHeight + 2;
                 m_prim.Position = pos;
-                VDetailLog("{0},MoveLinear,terrainHeight,pos={1}", m_prim.LocalID, pos);
+                VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", m_prim.LocalID, terrainHeight, pos);
             }
 
             // Check if hovering
@@ -606,11 +573,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // We should hover, get the target height
                 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = m_prim.Scene.GetWaterLevel() + m_VhoverHeight;
+                    m_VhoverTargetHeight = m_prim.Scene.GetWaterLevelAtXYZ(pos) + m_VhoverHeight;
                 }
                 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = m_prim.Scene.TerrainManager.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
+                    m_VhoverTargetHeight = terrainHeight + m_VhoverHeight;
                 }
                 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                 {
@@ -635,82 +602,92 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // Replace Vertical speed with correction figure if significant
                     if (Math.Abs(herr0) > 0.01f)
                     {
-                        m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
+                        m_newVelocity.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
                         //KF: m_VhoverEfficiency is not yet implemented
                     }
                     else
                     {
-                        m_dir.Z = 0f;
+                        m_newVelocity.Z = 0f;
                     }
                 }
 
-                VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", m_prim.LocalID, pos, m_dir, m_VhoverHeight, m_VhoverTargetHeight);
+                VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", m_prim.LocalID, pos, m_newVelocity, 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
+            Vector3 posChange = pos - m_lastPositionVector;
+            if (m_BlockingEndPoint != Vector3.Zero)
+            {
+                bool changed = false;
+                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
+                {
+                    pos.X -= posChange.X + 1;
+                    changed = true;
+                }
+                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
+                {
+                    pos.Y -= posChange.Y + 1;
+                    changed = true;
+                }
+                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
+                {
+                    pos.Z -= posChange.Z + 1;
+                    changed = true;
+                }
+                if (pos.X <= 0)
+                {
+                    pos.X += posChange.X + 1;
+                    changed = true;
+                }
+                if (pos.Y <= 0)
+                {
+                    pos.Y += posChange.Y + 1;
+                    changed = true;
+                }
+                if (changed)
+                {
+                    m_prim.Position = pos;
+                    VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
+                                m_prim.LocalID, m_BlockingEndPoint, posChange, pos);
+                }
             }
 
+            float Zchange = Math.Abs(posChange.Z);
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
-                //Start Experimental Values
                 if (Zchange > .3)
-                {
                     grav.Z = (float)(grav.Z * 3);
-                }
                 if (Zchange > .15)
-                {
                     grav.Z = (float)(grav.Z * 2);
-                }
                 if (Zchange > .75)
-                {
                     grav.Z = (float)(grav.Z * 1.5);
-                }
                 if (Zchange > .05)
-                {
                     grav.Z = (float)(grav.Z * 1.25);
-                }
                 if (Zchange > .025)
-                {
                     grav.Z = (float)(grav.Z * 1.125);
-                }
-                float terraintemp = m_prim.Scene.TerrainManager.GetTerrainHeightAtXYZ(pos);
-                float postemp = (pos.Z - terraintemp);
+                float postemp = (pos.Z - terrainHeight);
                 if (postemp > 2.5f)
-                {
                     grav.Z = (float)(grav.Z * 1.037125);
-                }
                 VDetailLog("{0},MoveLinear,limitMotorUp,grav={1}", m_prim.LocalID, grav);
-                //End Experimental Values
             }
             if ((m_flags & (VehicleFlag.NO_X)) != 0)
-            {
-                m_dir.X = 0;
-            }
+                m_newVelocity.X = 0;
             if ((m_flags & (VehicleFlag.NO_Y)) != 0)
-            {
-                m_dir.Y = 0;
-            }
+                m_newVelocity.Y = 0;
             if ((m_flags & (VehicleFlag.NO_Z)) != 0)
-            {
-                m_dir.Z = 0;
-            }
-
-            m_lastPositionVector = m_prim.Position;
+                m_newVelocity.Z = 0;
 
             // Apply velocity
-            m_prim.Velocity = m_dir;
+            m_prim.Velocity = m_newVelocity;
             // apply gravity force
             // Why is this set here? The physics engine already does gravity.
             // m_prim.AddForce(grav, false);
-            // m_prim.Force = grav;
 
             // Apply friction
-            Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
-            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
+            Vector3 keepFraction = Vector3.One - (Vector3.One / (m_linearFrictionTimescale / pTimestep));
+            m_lastLinearVelocityVector *= keepFraction;
 
-            VDetailLog("{0},MoveLinear,done,pos={1},vel={2},force={3},decay={4}", 
-                        m_prim.LocalID, m_lastPositionVector, m_dir, grav, decayamount);
+            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},grav={4},1Mdecay={5}", 
+                    m_prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, grav, keepFraction);
 
         } // end MoveLinear()
 
@@ -735,17 +712,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // 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
+                //   current 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},origvel={2},dir={3},vel={4}", 
-                        m_prim.LocalID,m_angularMotorApply,origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
+                VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},dir={5},vel={6}", 
+                        m_prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, 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.
+                // 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
             {
@@ -760,7 +738,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             Vector3 vertattr = Vector3.Zero;
             if (m_verticalAttractionTimescale < 300)
             {
-                float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
+                float VAservo = 0.2f / (m_verticalAttractionTimescale / pTimestep);
                 // get present body rotation
                 Quaternion rotq = m_prim.Orientation;
                 // make a vector pointing up
@@ -863,16 +841,12 @@ 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;
+                VDetailLog("{0},LimitRotation,done,orig={1},new={2}", m_prim.LocalID, rotq, m_rot);
+            }
 
-            VDetailLog("{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.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 5f6601d..dc1de6c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -206,7 +206,7 @@ public class BSLinkset
     //   its internal properties.
     public void Refresh(BSPhysObject requestor)
     {
-        // If there are no children, there aren't any constraints to recompute
+        // If there are no children, there can't be any constraints to recompute
         if (!HasAnyChildren)
             return;
 
@@ -225,11 +225,12 @@ public class BSLinkset
     //    from a linkset to make sure the constraints know about the new mass and
     //    geometry.
     // Must only be called at taint time!!
-    private bool RecomputeLinksetConstraintVariables()
+    private void RecomputeLinksetConstraintVariables()
     {
         float linksetMass = LinksetMass;
         lock (m_linksetActivityLock)
         {
+            bool somethingMissing = false;
             foreach (BSPhysObject child in m_children)
             {
                 BSConstraint constrain;
@@ -241,16 +242,29 @@ public class BSLinkset
                 }
                 else
                 {
-                    // Non-fatal error that can happen when children are being added to the linkset but
+                    // Non-fatal error that happens when children are being added to the linkset but
                     //    their constraints have not been created yet.
                     // Caused by the fact that m_children is built at run time but building constraints
                     //    happens at taint time.
-                    // m_physicsScene.Logger.ErrorFormat("{0} RecomputeLinksetConstraintVariables: constraint not found for root={1}, child={2}",
-                    //                                 LogHeader, m_linksetRoot.Body.ID, child.Body.ID);
+                    somethingMissing = true;
+                    break;
                 }
             }
+
+            // If the whole linkset is not here, doesn't make sense to recompute the root prim now.
+            if (!somethingMissing)
+            {
+                // The root prim takes on the weight of the whole linkset
+                /*
+                OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(LinksetRoot.BSShape.Ptr, linksetMass);
+                BulletSimAPI.SetMassProps2(LinksetRoot.BSBody.Ptr, linksetMass, inertia);
+                OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
+                BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.Ptr, centerOfMass, OMV.Quaternion.Identity);
+                BulletSimAPI.UpdateInertiaTensor2(LinksetRoot.BSBody.Ptr);
+                 */
+            }
         }
-        return false;
+        return;
     }
 
     // I am the root of a linkset and a new child is being added
@@ -296,9 +310,9 @@ public class BSLinkset
                 DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
 
                 PhysicallyUnlinkAChildFromRoot(rootx, childx);
+                RecomputeLinksetConstraintVariables();
             });
 
-            RecomputeLinksetConstraintVariables();
         }
         else
         {
@@ -377,6 +391,10 @@ public class BSLinkset
                         PhysicsScene.Params.linkConstraintTransMotorMaxVel,
                         PhysicsScene.Params.linkConstraintTransMotorMaxForce);
         constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
+        if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
+        {
+            constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
+        }
 
         RecomputeLinksetConstraintVariables();
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index e411fcb..969c53e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -41,7 +41,7 @@ public abstract class BSPhysObject : PhysicsActor
 {
     public abstract BSLinkset Linkset { get; set; }
 
-    public abstract void Collide(uint collidingWith, BSPhysObject collidee, ActorTypes type,
+    public abstract bool Collide(uint collidingWith, BSPhysObject collidee, ActorTypes type,
             OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth);
     public abstract void SendCollisions();
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 6d0af63..481a8db 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -136,11 +136,11 @@ public sealed class BSPrim : BSPhysObject
         Linkset = new BSLinkset(Scene, this);     // a linkset of one
         _vehicle = new BSDynamics(Scene, this);            // add vehicleness
         _mass = CalculateMass();
-        // do the actual object creation at taint time
         DetailLog("{0},BSPrim.constructor,call", LocalID);
+        // do the actual object creation at taint time
         _scene.TaintedObject("BSPrim.create", delegate()
         {
-            RecreateGeomAndObject();
+            CreateGeomAndObject(true);
 
             // Get the pointer to the physical body for this object.
             // At the moment, we're still letting BulletSim manage the creation and destruction
@@ -186,9 +186,10 @@ public sealed class BSPrim : BSPhysObject
             _scene.TaintedObject("BSPrim.setSize", delegate()
             {
                 _mass = CalculateMass();   // changing size changes the mass
-                BulletSimAPI.SetObjectScaleMass(_scene.WorldID, _localID, _scale, (IsPhysical ? _mass : 0f), IsPhysical);
-                DetailLog("{0}: BSPrim.setSize: size={1}, mass={2}, physical={3}", LocalID, _size, _mass, IsPhysical);
-                RecreateGeomAndObject();
+                // Since _size changed, the mesh needs to be rebuilt. If rebuilt, all the correct
+                //   scale and margins are set.
+                CreateGeomAndObject(true);
+                DetailLog("{0}: BSPrim.setSize: size={1}, scale={2}, mass={3}, physical={4}", LocalID, _size, _scale, _mass, IsPhysical);
             });
         } 
     }
@@ -198,7 +199,7 @@ public sealed class BSPrim : BSPhysObject
             _scene.TaintedObject("BSPrim.setShape", delegate()
             {
                 _mass = CalculateMass();   // changing the shape changes the mass
-                RecreateGeomAndObject();
+                CreateGeomAndObject(false);
             });
         } 
     }
@@ -279,7 +280,7 @@ public sealed class BSPrim : BSPhysObject
         get { 
             if (!Linkset.IsRoot(this))
                 // child prims move around based on their parent. Need to get the latest location
-                _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
+                _position = BulletSimAPI.GetPosition2(BSBody.Ptr);
 
             // don't do the GetObjectPosition for root elements because this function is called a zillion times
             // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
@@ -291,7 +292,7 @@ public sealed class BSPrim : BSPhysObject
             _scene.TaintedObject("BSPrim.setPosition", delegate()
             {
                 DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
+                BulletSimAPI.SetTranslation2(BSBody.Ptr, _position, _orientation);
             });
         } 
     }
@@ -302,7 +303,8 @@ public sealed class BSPrim : BSPhysObject
     { 
         get
         {
-            return Linkset.LinksetMass;
+            // return Linkset.LinksetMass;
+            return _mass;
         }
     }
 
@@ -328,7 +330,6 @@ public sealed class BSPrim : BSPhysObject
             _scene.TaintedObject("BSPrim.setForce", delegate()
             {
                 DetailLog("{0},BSPrim.setForce,taint,force={1}", LocalID, _force);
-                // BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
                 BulletSimAPI.SetObjectForce2(BSBody.Ptr, _force);
             });
         } 
@@ -406,7 +407,7 @@ public sealed class BSPrim : BSPhysObject
             _scene.TaintedObject("BSPrim.setVelocity", delegate()
             {
                 DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, _velocity);
-                BulletSimAPI.SetObjectVelocity(_scene.WorldID, LocalID, _velocity);
+                BulletSimAPI.SetLinearVelocity2(BSBody.Ptr, _velocity);
             });
         } 
     }
@@ -430,7 +431,7 @@ public sealed class BSPrim : BSPhysObject
             if (!Linkset.IsRoot(this))
             {
                 // Children move around because tied to parent. Get a fresh value.
-                _orientation = BulletSimAPI.GetObjectOrientation(_scene.WorldID, LocalID);
+                _orientation = BulletSimAPI.GetOrientation2(BSBody.Ptr);
             }
             return _orientation;
         } 
@@ -441,7 +442,7 @@ public sealed class BSPrim : BSPhysObject
             {
                 // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
                 DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
+                BulletSimAPI.SetTranslation2(BSBody.Ptr, _position, _orientation);
             });
         } 
     }
@@ -483,31 +484,37 @@ public sealed class BSPrim : BSPhysObject
     {
         // If it's becoming dynamic, it will need hullness
         VerifyCorrectPhysicalShape();
+        UpdatePhysicalParameters();
+    }
 
+    private void UpdatePhysicalParameters()
+    {
+        /*
         // Bullet wants static objects to have a mass of zero
         float mass = IsStatic ? 0f : _mass;
 
         BulletSimAPI.SetObjectProperties(_scene.WorldID, LocalID, IsStatic, IsSolid, SubscribedEvents(), mass);
-        /*
+         */
         BulletSimAPI.RemoveObjectFromWorld2(Scene.World.Ptr, BSBody.Ptr);
 
-        // Set up the object physicalness (static or dynamic)
-        MakeDynamic();
+        // Set up the object physicalness (does gravity and collisions move this object)
+        MakeDynamic(IsStatic);
 
-        // Make solid or not and arrange for collisions, etc
-        MakeSolid();
+        // Make solid or not (do things bounce off or pass through this object)
+        MakeSolid(IsSolid);
 
-        m_currentCollisionFlags = BulletSimAPI.GetCollisionFlags2(BSBody.Ptr);
+        // Arrange for collisions events if the simulator wants them
+        EnableCollisions(SubscribedEvents());
 
         BulletSimAPI.AddObjectToWorld2(Scene.World.Ptr, BSBody.Ptr);
-         */
 
         // Recompute any linkset parameters.
         // When going from non-physical to physical, this re-enables the constraints that
         //     had been automatically disabled when the mass was set to zero.
         Linkset.Refresh(this);
 
-        DetailLog("{0},BSPrim.SetObjectDynamic,taint,static={1},solid={2},mass={3}, cf={4}", LocalID, IsStatic, IsSolid, mass, m_currentCollisionFlags);
+        DetailLog("{0},BSPrim.UpdatePhysicalParameters,taint,static={1},solid={2},mass={3}, cf={4}", 
+                        LocalID, IsStatic, IsSolid, _mass, m_currentCollisionFlags);
     }
 
     // "Making dynamic" means changing to and from static.
@@ -515,12 +522,12 @@ public sealed class BSPrim : BSPhysObject
     // When dynamic, the object can fall and be pushed by others.
     // This is independent of its 'solidness' which controls what passes through
     //    this object and what interacts with it.
-    private void MakeDynamic()
+    private void MakeDynamic(bool makeStatic)
     {
-        if (IsStatic)
+        if (makeStatic)
         {
             // Become a Bullet 'static' object type
-            BulletSimAPI.AddToCollisionFlags2(BSBody.Ptr, CollisionFlags.CF_STATIC_OBJECT);
+            m_currentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.Ptr, CollisionFlags.CF_STATIC_OBJECT);
             // Stop all movement
             BulletSimAPI.ClearAllForces2(BSBody.Ptr);
             // Mass is zero which disables a bunch of physics stuff in Bullet
@@ -533,12 +540,11 @@ public sealed class BSPrim : BSPhysObject
         else
         {
             // Not a Bullet static object
-            BulletSimAPI.RemoveFromCollisionFlags2(BSBody.Ptr, CollisionFlags.CF_STATIC_OBJECT);
+            m_currentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.Ptr, CollisionFlags.CF_STATIC_OBJECT);
             // A dynamic object has mass
-            BulletSimAPI.SetMassProps2(BSBody.Ptr, _mass, OMV.Vector3.Zero);
-            // The shape is interesting and has mass and a center of gravity
             IntPtr collisionShapePtr = BulletSimAPI.GetCollisionShape2(BSBody.Ptr);
-            BulletSimAPI.CalculateLocalInertia2(collisionShapePtr, _mass, OMV.Vector3.Zero);
+            OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(collisionShapePtr, _mass);
+            BulletSimAPI.SetMassProps2(BSBody.Ptr, _mass, inertia);
             // Inertia is based on our new mass
             BulletSimAPI.UpdateInertiaTensor2(BSBody.Ptr);
             // Force activation of the object so Bullet will act on it.
@@ -546,8 +552,31 @@ public sealed class BSPrim : BSPhysObject
         }
     }
 
-    private void MakeSolid()
+    // "Making solid" means that other object will not pass through this object.
+    private void MakeSolid(bool makeSolid)
     {
+        if (makeSolid)
+        {
+            // Easy in Bullet -- just remove the object flag that controls collision response
+            m_currentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.Ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+        }
+        else
+        {
+            m_currentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.Ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+        }
+    }
+
+    // Turn on or off the flag controlling whether collision events are returned to the simulator.
+    private void EnableCollisions(bool wantsCollisionEvents)
+    {
+        if (wantsCollisionEvents)
+        {
+            m_currentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+        }
+        else
+        {
+            m_currentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+        }
     }
 
     // prims don't fly
@@ -607,7 +636,7 @@ public sealed class BSPrim : BSPhysObject
             _scene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
             {
                 DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
-                BulletSimAPI.SetObjectAngularVelocity(_scene.WorldID, LocalID, _rotationalVelocity);
+                BulletSimAPI.SetAngularVelocity2(BSBody.Ptr, _rotationalVelocity);
             });
         } 
     }
@@ -624,7 +653,10 @@ public sealed class BSPrim : BSPhysObject
             _scene.TaintedObject("BSPrim.setBuoyancy", delegate()
             {
                 DetailLog("{0},BSPrim.SetBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
-                BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
+                // Buoyancy is faked by changing the gravity applied to the object
+                float grav = Scene.Params.gravity * (1f - _buoyancy);
+                BulletSimAPI.SetGravity2(BSBody.Ptr, new OMV.Vector3(0f, 0f, grav));
+                // BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
             });
         } 
     }
@@ -686,8 +718,8 @@ public sealed class BSPrim : BSPhysObject
                 }
                 m_accumulatedForces.Clear();
             }
-            DetailLog("{0},BSPrim.AddObjectForce,taint,force={1}", LocalID, _force);
-            // For unknown reason, "ApplyCentralForce" is really additive.
+            DetailLog("{0},BSPrim.AddObjectForce,taint,force={1}", LocalID, fSum);
+            // For unknown reasons, "ApplyCentralForce" adds this force to the total force on the object.
             BulletSimAPI.ApplyCentralForce2(BSBody.Ptr, fSum);
         });
     }
@@ -1030,29 +1062,36 @@ public sealed class BSPrim : BSPhysObject
     // Returns 'true' if the geometry was rebuilt
     private bool CreateGeom(bool forceRebuild)
     {
-        // the mesher thought this was too simple to mesh. Use a native Bullet collision shape.
         bool ret = false;
-        if (!_scene.NeedsMeshing(_pbs))
+        bool haveShape = false;
+
+        // If the prim attributes are simple, this could be a simple Bullet native shape
+        if ((_pbs.SculptEntry && !Scene.ShouldMeshSculptedPrim)
+                || (_pbs.ProfileBegin == 0 && _pbs.ProfileEnd == 0
+                    && _pbs.ProfileHollow == 0
+                    && _pbs.PathTwist == 0 && _pbs.PathTwistBegin == 0
+                    && _pbs.PathBegin == 0 && _pbs.PathEnd == 0
+                    && _pbs.PathTaperX == 0 && _pbs.PathTaperY == 0
+                    && _pbs.PathScaleX == 100 && _pbs.PathScaleY == 100
+                    && _pbs.PathShearX == 0 && _pbs.PathShearY == 0) )
         {
             if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
             {
-                // if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
-                // {
-                    // m_log.DebugFormat("{0}: CreateGeom: Defaulting to sphere of size {1}", LogHeader, _size);
-                    if (forceRebuild || (_shapeType != ShapeData.PhysicsShapeType.SHAPE_SPHERE))
-                    {
-                        DetailLog("{0},BSPrim.CreateGeom,sphere (force={1}", LocalID, forceRebuild);
-                        _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE;
-                        // Bullet native objects are scaled by the Bullet engine so pass the size in
-                        _scale = _size;
-                        // TODO: do we need to check for and destroy a mesh or hull that might have been left from before?
-                        ret = true;
-                    }
-                // }
+                haveShape = true;
+                if (forceRebuild || (_shapeType != ShapeData.PhysicsShapeType.SHAPE_SPHERE))
+                {
+                    DetailLog("{0},BSPrim.CreateGeom,sphere (force={1}", LocalID, forceRebuild);
+                    _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE;
+                    // Bullet native objects are scaled by the Bullet engine so pass the size in
+                    _scale = _size;
+                    // TODO: do we need to check for and destroy a mesh or hull that might have been left from before?
+                    ret = true;
+                }
             }
             else
             {
                 // m_log.DebugFormat("{0}: CreateGeom: Defaulting to box. lid={1}, type={2}, size={3}", LogHeader, LocalID, _shapeType, _size);
+                haveShape = true;
                 if (forceRebuild || (_shapeType != ShapeData.PhysicsShapeType.SHAPE_BOX))
                 {
                     DetailLog("{0},BSPrim.CreateGeom,box (force={1})", LocalID, forceRebuild);
@@ -1063,16 +1102,16 @@ public sealed class BSPrim : BSPhysObject
                 }
             }
         }
-        else
+        // If a simple shape isn't happening, create a mesh and possibly a hull
+        if (!haveShape)
         {
             if (IsPhysical)
             {
                 if (forceRebuild || _hullKey == 0)
                 {
                     // physical objects require a hull for interaction.
-                    // This will create the mesh if it doesn't already exist
-                    CreateGeomHull();
-                    ret = true;
+                    // This also creates the mesh if it doesn't already exist
+                    ret = CreateGeomHull();
                 }
             }
             else
@@ -1080,8 +1119,7 @@ public sealed class BSPrim : BSPhysObject
                 if (forceRebuild || _meshKey == 0)
                 {
                     // Static (non-physical) objects only need a mesh for bumping into
-                    CreateGeomMesh();
-                    ret = true;
+                    ret = CreateGeomMesh();
                 }
             }
         }
@@ -1089,7 +1127,8 @@ public sealed class BSPrim : BSPhysObject
     }
 
     // No locking here because this is done when we know physics is not simulating
-    private void CreateGeomMesh()
+    // Returns 'true' of a mesh was actually rebuild (we could also have one of these specs).
+    private bool CreateGeomMesh()
     {
         // level of detail based on size and type of the object
         float lod = _scene.MeshLOD;
@@ -1103,7 +1142,7 @@ public sealed class BSPrim : BSPhysObject
         // m_log.DebugFormat("{0}: CreateGeomMesh: lID={1}, oldKey={2}, newKey={3}", LogHeader, _localID, _meshKey, newMeshKey);
 
         // if this new shape is the same as last time, don't recreate the mesh
-        if (_meshKey == newMeshKey) return;
+        if (_meshKey == newMeshKey) return false;
 
         DetailLog("{0},BSPrim.CreateGeomMesh,create,key={1}", LocalID, newMeshKey);
         // Since we're recreating new, get rid of any previously generated shape
@@ -1140,19 +1179,19 @@ public sealed class BSPrim : BSPhysObject
         _shapeType = ShapeData.PhysicsShapeType.SHAPE_MESH;
         // meshes are already scaled by the meshmerizer
         _scale = new OMV.Vector3(1f, 1f, 1f);
-        DetailLog("{0},BSPrim.CreateGeomMesh,done", LocalID);
-        return;
+        return true;
     }
 
     // No locking here because this is done when we know physics is not simulating
-    private void CreateGeomHull()
+    // Returns 'true' of a mesh was actually rebuild (we could also have one of these specs).
+    private bool CreateGeomHull()
     {
         float lod = _pbs.SculptEntry ? _scene.SculptLOD : _scene.MeshLOD;
         ulong newHullKey = (ulong)_pbs.GetMeshKey(_size, lod);
         // m_log.DebugFormat("{0}: CreateGeomHull: lID={1}, oldKey={2}, newKey={3}", LogHeader, _localID, _hullKey, newHullKey);
 
         // if the hull hasn't changed, don't rebuild it
-        if (newHullKey == _hullKey) return;
+        if (newHullKey == _hullKey) return false;
 
         DetailLog("{0},BSPrim.CreateGeomHull,create,oldKey={1},newKey={2}", LocalID, _hullKey, newHullKey);
         
@@ -1255,7 +1294,7 @@ public sealed class BSPrim : BSPhysObject
         // meshes are already scaled by the meshmerizer
         _scale = new OMV.Vector3(1f, 1f, 1f);
         DetailLog("{0},BSPrim.CreateGeomHull,done", LocalID);
-        return;
+        return true;
     }
 
     // Callback from convex hull creater with a newly created hull.
@@ -1268,20 +1307,12 @@ public sealed class BSPrim : BSPhysObject
 
     private void VerifyCorrectPhysicalShape()
     {
-        if (IsStatic)
-        {
-            // if static, we don't need a hull so, if there is one, rebuild without it
-            if (_hullKey != 0)
-            {
-                RecreateGeomAndObject();
-            }
-        }
-        else
+        if (!IsStatic)
         {
             // if not static, it will need a hull to efficiently collide with things
             if (_hullKey == 0)
             {
-                RecreateGeomAndObject();
+                CreateGeomAndObject(false);
             }
 
         }
@@ -1300,8 +1331,9 @@ public sealed class BSPrim : BSPhysObject
         // m_log.DebugFormat("{0}: CreateObject: lID={1}, shape={2}", LogHeader, _localID, shape.Type);
         bool ret = BulletSimAPI.CreateObject(_scene.WorldID, shape);
 
-        // the CreateObject() may have recreated the rigid body. Make sure we have the latest.
+        // the CreateObject() may have recreated the rigid body. Make sure we have the latest address.
         BSBody = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID));
+        BSShape = new BulletShape(BulletSimAPI.GetCollisionShape2(BSBody.Ptr));
 
         return ret;
     }
@@ -1325,15 +1357,20 @@ public sealed class BSPrim : BSPhysObject
         shape.Static = _isPhysical ? ShapeData.numericFalse : ShapeData.numericTrue;
     }
 
-
     // Rebuild the geometry and object.
     // This is called when the shape changes so we need to recreate the mesh/hull.
     // No locking here because this is done when the physics engine is not simulating
-    private void RecreateGeomAndObject()
+    private void CreateGeomAndObject(bool forceRebuild)
     {
-        // m_log.DebugFormat("{0}: RecreateGeomAndObject. lID={1}", LogHeader, _localID);
-        if (CreateGeom(true))
+        // m_log.DebugFormat("{0}: CreateGeomAndObject. lID={1}, force={2}", LogHeader, _localID, forceRebuild);
+        // Create the geometry that will make up the object
+        if (CreateGeom(forceRebuild))
+        {
+            // Create the object and place it into the world
             CreateObject();
+            // Make sure the properties are set on the new object
+            UpdatePhysicalParameters();
+        }
         return;
     }
 
@@ -1430,9 +1467,10 @@ public sealed class BSPrim : BSPhysObject
     // I've collided with something
     // Called at taint time from within the Step() function
     CollisionEventUpdate collisionCollection;
-    public override void Collide(uint collidingWith, BSPhysObject collidee, ActorTypes type, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
+    public override bool Collide(uint collidingWith, BSPhysObject collidee, ActorTypes type, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
     {
         // m_log.DebugFormat("{0}: Collide: ms={1}, id={2}, with={3}", LogHeader, _subscribedEventsMs, LocalID, collidingWith);
+        bool ret = false;
 
         // The following lines make IsColliding() and IsCollidingGround() work
         _collidingStep = _scene.SimulationStep;
@@ -1446,7 +1484,7 @@ public sealed class BSPrim : BSPhysObject
         // prims in the same linkset cannot collide with each other
         if (collidee != null && (this.Linkset.LinksetID == collidee.Linkset.LinksetID))
         {
-            return;
+            return ret;
         }
 
         // if someone has subscribed for collision events....
@@ -1459,8 +1497,10 @@ public sealed class BSPrim : BSPhysObject
                 if (collisionCollection == null)
                     collisionCollection = new CollisionEventUpdate();
                 collisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
+                ret = true;
             }
         }
+        return ret;
     }
 
     // The scene is telling us it's time to pass our collected collisions into the simulator
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 4a468af..eea899f 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -79,7 +79,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private HashSet<BSPhysObject> m_objectsWithCollisions = new HashSet<BSPhysObject>();
     // Following is a kludge and can  be removed when avatar animation updating is
     //    moved to a better place.
-    private HashSet<BSCharacter> m_avatarsWithCollisions = new HashSet<BSCharacter>();
+    private HashSet<BSPhysObject> m_avatarsWithCollisions = new HashSet<BSPhysObject>();
 
     // List of all the objects that have vehicle properties and should be called
     //    to update each physics step.
@@ -132,8 +132,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private EntityProperties[] m_updateArray;
     private GCHandle m_updateArrayPinnedHandle;
 
-    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 bool ShouldMeshSculptedPrim { get; private set; }   // cause scuplted prims to get meshed
+    public bool ShouldForceSimplePrimMeshing { get; private set; }   // 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
@@ -153,6 +153,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     {
         get { return new Vector3(0f, 0f, Params.gravity); }
     }
+    // Just the Z value of the gravity
+    public float DefaultGravityZ
+    {
+        get { return Params.gravity; }
+    }
 
     public float MaximumObjectMass { get; private set; }
 
@@ -171,8 +176,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             callback = c;
         }
     }
+    private Object _taintLock = new Object();   // lock for using the next object
     private List<TaintCallbackEntry> _taintedObjects;
-    private Object _taintLock = new Object();
 
     // A pointer to an instance if this structure is passed to the C++ code
     // Used to pass basic configuration values to the unmanaged code.
@@ -478,6 +483,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         // Some of the prims operate with special vehicle properties
         ProcessVehicles(timeStep);
+        numTaints += _taintedObjects.Count;
         ProcessTaints();    // the vehicles might have added taints
 
         // step the physical world one interval
@@ -506,6 +512,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         // Get a value for 'now' so all the collision and update routines don't have to get their own
         SimulationNowTime = Util.EnvironmentTickCount();
 
+        // This is a kludge to get avatar movement updates. 
+        //   ODE sends 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 collisions happen.
+        m_objectsWithCollisions = new HashSet<BSPhysObject>(m_avatarsWithCollisions);
+
         // If there were collisions, process them by sending the event to the prim.
         // Collisions must be processed before updates.
         if (collidersCount > 0)
@@ -527,13 +539,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             bsp.SendCollisions();
         m_objectsWithCollisions.Clear();
 
-        // This is a kludge to get avatar movement updated. 
-        //   ODE sends collisions even if there are none and this is used to update
-        //   avatar animations and stuff.
-        foreach (BSPhysObject bpo in m_avatarsWithCollisions)
-            bpo.SendCollisions();
-        // m_avatarsWithCollisions.Clear();
-
         // If any of the objects had updated properties, tell the object it has been changed by the physics engine
         if (updatedEntityCount > 0)
         {
@@ -544,7 +549,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 if (PhysObjects.TryGetValue(entprop.ID, out pobj))
                 {
                     pobj.UpdateProperties(entprop);
-                    continue;
                 }
             }
         }
@@ -600,8 +604,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         // DetailLog("{0},BSScene.SendCollision,collide,id={1},with={2}", DetailLogZero, localID, collidingWith);
 
-        collider.Collide(collidingWith, collidee, type, collidePoint, collideNormal, penetration);
-        m_objectsWithCollisions.Add(collider);
+        if (collider.Collide(collidingWith, collidee, type, collidePoint, collideNormal, penetration))
+        {
+            // If a collision was posted, remember to send it to the simulator
+            m_objectsWithCollisions.Add(collider);
+        }
 
         return;
     }
@@ -619,9 +626,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     public override void SetWaterLevel(float baseheight) 
     {
         m_waterLevel = baseheight;
-        // TODO: pass to physics engine so things will float?
     }
-    public float GetWaterLevel()
+    // Someday....
+    public float GetWaterLevelAtXYZ(Vector3 loc)
     {
         return m_waterLevel;
     }
@@ -672,7 +679,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         // int iPropertiesNotSupportedDefault = 0;
 
-        if (pbs.SculptEntry && !_meshSculptedPrim)
+        if (pbs.SculptEntry && !ShouldMeshSculptedPrim)
         {
             // Render sculpties as boxes
             return false;
@@ -680,7 +687,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since Bullet 
         // can use an internal representation for the prim
-        if (!_forceSimplePrimMeshing)
+        if (!ShouldForceSimplePrimMeshing)
         {
             if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
                 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
@@ -782,7 +789,10 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         if (!m_initialized) return;
 
         lock (_taintLock)
+        {
             _taintedObjects.Add(new TaintCallbackEntry(ident, callback));
+        }
+
         return;
     }
 
@@ -919,14 +929,14 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     {
         new ParameterDefn("MeshSculptedPrim", "Whether to create meshes for sculpties",
             ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { s._meshSculptedPrim = cf.GetBoolean(p, s.BoolNumeric(v)); },
-            (s) => { return s.NumericBool(s._meshSculptedPrim); },
-            (s,p,l,v) => { s._meshSculptedPrim = s.BoolNumeric(v); } ),
+            (s,cf,p,v) => { s.ShouldMeshSculptedPrim = cf.GetBoolean(p, s.BoolNumeric(v)); },
+            (s) => { return s.NumericBool(s.ShouldMeshSculptedPrim); },
+            (s,p,l,v) => { s.ShouldMeshSculptedPrim = s.BoolNumeric(v); } ),
         new ParameterDefn("ForceSimplePrimMeshing", "If true, only use primitive meshes for objects",
             ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { s._forceSimplePrimMeshing = cf.GetBoolean(p, s.BoolNumeric(v)); },
-            (s) => { return s.NumericBool(s._forceSimplePrimMeshing); },
-            (s,p,l,v) => { s._forceSimplePrimMeshing = s.BoolNumeric(v); } ),
+            (s,cf,p,v) => { s.ShouldForceSimplePrimMeshing = cf.GetBoolean(p, s.BoolNumeric(v)); },
+            (s) => { return s.NumericBool(s.ShouldForceSimplePrimMeshing); },
+            (s,p,l,v) => { s.ShouldForceSimplePrimMeshing = s.BoolNumeric(v); } ),
 
         new ParameterDefn("MeshLevelOfDetail", "Level of detail to render meshes (32, 16, 8 or 4. 32=most detailed)",
             8f,
@@ -1162,8 +1172,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.m_params[0].linkConstraintTransMotorMaxForce = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linkConstraintTransMotorMaxForce; },
             (s,p,l,v) => { s.m_params[0].linkConstraintTransMotorMaxForce = v; } ),
-            new ParameterDefn("LinkConstraintCFM", "Amount constraint can be violated. 0=none, 1=all. Default=0",
-            0.0f,
+            new ParameterDefn("LinkConstraintCFM", "Amount constraint can be violated. 0=no violation, 1=infinite. Default=0.1",
+            0.1f,
             (s,cf,p,v) => { s.m_params[0].linkConstraintCFM = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linkConstraintCFM; },
             (s,p,l,v) => { s.m_params[0].linkConstraintCFM = v; } ),
@@ -1172,6 +1182,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.m_params[0].linkConstraintERP = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linkConstraintERP; },
             (s,p,l,v) => { s.m_params[0].linkConstraintERP = v; } ),
+            new ParameterDefn("LinkConstraintSolverIterations", "Number of solver iterations when computing constraint. (0 = Bullet default)",
+            40,
+            (s,cf,p,v) => { s.m_params[0].linkConstraintSolverIterations = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].linkConstraintSolverIterations; },
+            (s,p,l,v) => { s.m_params[0].linkConstraintSolverIterations = v; } ),
 
         new ParameterDefn("DetailedStats", "Frames between outputting detailed phys stats. (0 is off)",
             0f,
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
index 47d7199..d48462e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -154,27 +154,31 @@ public class BSTerrainManager
 
     // The simulator wants to set a new heightmap for the terrain.
     public void SetTerrain(float[] heightMap) {
-        if (m_worldOffset != Vector3.Zero && m_parentScene != null)
+        float[] localHeightMap = heightMap;
+        m_physicsScene.TaintedObject("TerrainManager.SetTerrain", delegate()
         {
-            // If a child of a mega-region, we shouldn't have any terrain allocated for us
-            ReleaseGroundPlaneAndTerrain();   
-            // If doing the mega-prim stuff and we are the child of the zero region,
-            //    the terrain is added to our parent
-            if (m_parentScene is BSScene)
+            if (m_worldOffset != Vector3.Zero && m_parentScene != null)
             {
-                DetailLog("{0},SetTerrain.ToParent,offset={1},worldMax={2}", 
-                                BSScene.DetailLogZero, m_worldOffset, m_worldMax);
-                ((BSScene)m_parentScene).TerrainManager.UpdateOrCreateTerrain(BSScene.CHILDTERRAIN_ID,
-                                heightMap, m_worldOffset, m_worldOffset+DefaultRegionSize, false);
+                // If a child of a mega-region, we shouldn't have any terrain allocated for us
+                ReleaseGroundPlaneAndTerrain();
+                // If doing the mega-prim stuff and we are the child of the zero region,
+                //    the terrain is added to our parent
+                if (m_parentScene is BSScene)
+                {
+                    DetailLog("{0},SetTerrain.ToParent,offset={1},worldMax={2}",
+                                    BSScene.DetailLogZero, m_worldOffset, m_worldMax);
+                    ((BSScene)m_parentScene).TerrainManager.UpdateOrCreateTerrain(BSScene.CHILDTERRAIN_ID,
+                                    localHeightMap, m_worldOffset, m_worldOffset + DefaultRegionSize, true);
+                }
             }
-        }
-        else
-        {
-            // If not doing the mega-prim thing, just change the terrain
-            DetailLog("{0},SetTerrain.Existing", BSScene.DetailLogZero);
+            else
+            {
+                // If not doing the mega-prim thing, just change the terrain
+                DetailLog("{0},SetTerrain.Existing", BSScene.DetailLogZero);
 
-            UpdateOrCreateTerrain(BSScene.TERRAIN_ID, heightMap, m_worldOffset, m_worldOffset+DefaultRegionSize, false);
-        }
+                UpdateOrCreateTerrain(BSScene.TERRAIN_ID, localHeightMap, m_worldOffset, m_worldOffset + DefaultRegionSize, true);
+            }
+        });
     }
 
     // If called with no mapInfo for the terrain, this will create a new mapInfo and terrain
@@ -319,6 +323,8 @@ public class BSTerrainManager
 
                 // Make sure the new shape is processed.
                 BulletSimAPI.Activate2(mapInfo.terrainBody.Ptr, true);
+
+                m_terrainModified = true;
             };
 
             // There is the option to do the changes now (we're already in 'taint time'), or
@@ -357,6 +363,8 @@ public class BSTerrainManager
                 m_heightMaps.Add(terrainRegionBase, mapInfo);
                 // Build the terrain
                 UpdateOrCreateTerrain(newTerrainID, heightMap, minCoords, maxCoords, true);
+
+                m_terrainModified = true;
             };
 
             // If already in taint-time, just call Bullet. Otherwise queue the operations for the safe time.
@@ -383,7 +391,7 @@ public class BSTerrainManager
     private float lastHeightTX = 999999f;
     private float lastHeightTY = 999999f;
     private float lastHeight = HEIGHT_INITIAL_LASTHEIGHT;
-    public float GetTerrainHeightAtXY(float tX, float tY)
+    private float GetTerrainHeightAtXY(float tX, float tY)
     {
         // You'd be surprized at the number of times this routine is called
         //    with the same parameters as last time.
@@ -403,11 +411,18 @@ public class BSTerrainManager
         {
             float regionX = tX - offsetX;
             float regionY = tY - offsetY;
-            if (regionX >= mapInfo.sizeX || regionX < 0f) regionX = 0;
-            if (regionY >= mapInfo.sizeY || regionY < 0f) regionY = 0;
             int mapIndex = (int)regionY * (int)mapInfo.sizeY + (int)regionX;
-            ret = mapInfo.heightMap[mapIndex];
-            m_terrainModified = false;
+            try
+            {
+                ret = mapInfo.heightMap[mapIndex];
+            }
+            catch
+            {
+                // Sometimes they give us wonky values of X and Y. Give a warning and return something.
+                m_physicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, x={2}, y={3}",
+                                    LogHeader, terrainBaseXY, regionX, regionY);
+                ret = HEIGHT_GETHEIGHT_RET;
+            }
             // DetailLog("{0},BSTerrainManager.GetTerrainHeightAtXY,bX={1},baseY={2},szX={3},szY={4},regX={5},regY={6},index={7},ht={8}",
             //         BSScene.DetailLogZero, offsetX, offsetY, mapInfo.sizeX, mapInfo.sizeY, regionX, regionY, mapIndex, ret);
         }
@@ -416,6 +431,7 @@ public class BSTerrainManager
             m_physicsScene.Logger.ErrorFormat("{0} GetTerrainHeightAtXY: terrain not found: region={1}, x={2}, y={3}",
                     LogHeader, m_physicsScene.RegionName, tX, tY);
         }
+        m_terrainModified = false;
         lastHeight = ret;
         return ret;
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index e579cf2..043423e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -213,6 +213,7 @@ public struct ConfigurationParameters
     public float linkConstraintTransMotorMaxForce;
     public float linkConstraintERP;
     public float linkConstraintCFM;
+    public float linkConstraintSolverIterations;
 
     public const float numericTrue = 1f;
     public const float numericFalse = 0f;
@@ -395,23 +396,6 @@ public static extern bool DestroyMesh(uint worldID, System.UInt64 meshKey);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool CreateObject(uint worldID, ShapeData shapeData);
 
-/*  Remove old functionality
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void CreateLinkset(uint worldID, int objectCount, ShapeData[] shapeDatas);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void AddConstraint(uint worldID, uint id1, uint id2, 
-                        Vector3 frame1, Quaternion frame1rot,
-                        Vector3 frame2, Quaternion frame2rot,
-                        Vector3 lowLinear, Vector3 hiLinear, Vector3 lowAngular, Vector3 hiAngular);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool RemoveConstraintByID(uint worldID, uint id1);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool RemoveConstraint(uint worldID, uint id1, uint id2);
- */
-
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern Vector3 GetObjectPosition(uint WorldID, uint id);
 
@@ -545,6 +529,15 @@ public static extern bool DeleteCollisionShape2(IntPtr world, IntPtr shape);
 public static extern IntPtr CreateBodyFromShape2(IntPtr sim, IntPtr shape, Vector3 pos, Quaternion rot);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateCompoundShape2(IntPtr sim);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void AddChildToCompoundShape2(IntPtr cShape, IntPtr addShape, Vector3 pos, Quaternion rot);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void RemoveChildFromCompoundShape2(IntPtr cShape, IntPtr removeShape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr CreateBodyFromShapeAndInfo2(IntPtr sim, IntPtr shape, IntPtr constructionInfo);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -1010,7 +1003,7 @@ public static extern void SetLocalScaling2(IntPtr shape, Vector3 scale);
 public static extern Vector3 GetLocalScaling2(IntPtr shape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void CalculateLocalInertia2(IntPtr shape, float mass, Vector3 inertia);
+public static extern Vector3 CalculateLocalInertia2(IntPtr shape, float mass);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern int GetShapeType2(IntPtr shape);