9 files changed, 182 insertions, 94 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
index ff271fe..39a3421 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
@@ -58,10 +58,22 @@ public class BS6DofConstraint : BSConstraint
         m_world = world;
         m_body1 = obj1;
         m_body2 = obj2;
-        m_constraint = new BulletConstraint(
-                            BulletSimAPI.Create6DofConstraintToPoint2(m_world.ptr, m_body1.ptr, m_body2.ptr,
-                                joinPoint,
-                                useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
+        if (obj1.ptr == IntPtr.Zero || obj2.ptr == IntPtr.Zero)
+        {
+            world.scene.DetailLog("{0},BS6DOFConstraint,badBodyPtr,wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
+                            "[BULLETSIM 6DOF CONSTRAINT]", world.worldID,
+                            obj1.ID, obj1.ptr.ToString("X"), obj2.ID, obj2.ptr.ToString("X"));
+            world.scene.Logger.ErrorFormat("{0} Attempt to build 6DOF constraint with missing bodies: wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
+                            "[BULLETSIM 6DOF CONSTRAINT]", world.worldID,
+                            obj1.ID, obj1.ptr.ToString("X"), obj2.ID, obj2.ptr.ToString("X"));
+        }
+        else
+        {
+            m_constraint = new BulletConstraint(
+                                BulletSimAPI.Create6DofConstraintToPoint2(m_world.ptr, m_body1.ptr, m_body2.ptr,
+                                    joinPoint,
+                                    useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
+        }
         m_enabled = true;
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 2da2331..cf33d0e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -54,18 +54,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
     public class BSDynamics
     {
-        private int frcount = 0;                                        // Used to limit dynamics debug output to
-                                                                        // every 100th frame
-
-        private BSScene m_physicsScene;
-        private BSPrim m_prim;      // the prim this dynamic controller belongs to
+        private BSScene PhysicsScene { get; set; }
+        // the prim this dynamic controller belongs to
+        private BSPrim Prim { get; set; }
 
         // Vehicle properties
-        private Vehicle m_type = Vehicle.TYPE_NONE;                     // If a 'VEHICLE', and what kind
-        public Vehicle Type
-        {
-            get { return m_type; }
-        }
+        public Vehicle Type { get; set; }
+
         // private Quaternion m_referenceFrame = Quaternion.Identity;   // Axis modifier
         private VehicleFlag m_flags = (VehicleFlag) 0;                  // Boolean settings:
                                                                         // HOVER_TERRAIN_ONLY
@@ -126,14 +121,20 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         public BSDynamics(BSScene myScene, BSPrim myPrim)
         {
-            m_physicsScene = myScene;
-            m_prim = myPrim;
-            m_type = Vehicle.TYPE_NONE;
+            PhysicsScene = myScene;
+            Prim = myPrim;
+            Type = Vehicle.TYPE_NONE;
+        }
+
+        // Return 'true' if this vehicle is doing vehicle things
+        public bool IsActive
+        {
+            get { return Type != Vehicle.TYPE_NONE; }
         }
 
         internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
         {
-            VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
+            VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
@@ -232,7 +233,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
         {
-            VDetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
+            VDetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
@@ -267,7 +268,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
         {
-            VDetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
+            VDetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.REFERENCE_FRAME:
@@ -281,7 +282,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessVehicleFlags(int pParam, bool remove)
         {
-            VDetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", m_prim.LocalID, pParam, remove);
+            VDetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", Prim.LocalID, pParam, remove);
             VehicleFlag parm = (VehicleFlag)pParam;
             if (remove)
             {
@@ -301,9 +302,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessTypeChange(Vehicle pType)
         {
-            VDetailLog("{0},ProcessTypeChange,type={1}", m_prim.LocalID, pType);
+            VDetailLog("{0},ProcessTypeChange,type={1}", Prim.LocalID, pType);
             // Set Defaults For Type
-            m_type = pType;
+            Type = pType;
             switch (pType)
             {
                     case Vehicle.TYPE_NONE:
@@ -465,26 +466,37 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
         }//end SetDefaultsForType
 
+        // Some of the properties of this prim may have changed.
+        // Do any updating needed for a vehicle
+        public void Refresh()
+        {
+            if (Type == Vehicle.TYPE_NONE) return;
+
+            // Set the prim's inertia to zero. The vehicle code handles that and this
+            //    removes the torque action introduced by Bullet.
+            Vector3 inertia = Vector3.Zero;
+            BulletSimAPI.SetMassProps2(Prim.BSBody.ptr, Prim.MassRaw, inertia);
+            BulletSimAPI.UpdateInertiaTensor2(Prim.BSBody.ptr);
+        }
+
         // One step of the vehicle properties for the next 'pTimestep' seconds.
         internal void Step(float pTimestep)
         {
-            if (m_type == Vehicle.TYPE_NONE) return;
-
-            frcount++;  // used to limit debug comment output
-            if (frcount > 100)
-                frcount = 0;
+            if (!IsActive) return;
 
             MoveLinear(pTimestep);
             MoveAngular(pTimestep);
             LimitRotation(pTimestep);
 
             // remember the position so next step we can limit absolute movement effects
-            m_lastPositionVector = m_prim.Position;
+            m_lastPositionVector = 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);
+                    Prim.LocalID, Prim.Position, Prim.Force, Prim.Velocity, Prim.RotationalVelocity);
         }// end Step
 
+        // Apply the effect of the linear motor.
+        // Also does hover and float.
         private void MoveLinear(float pTimestep)
         {
             // m_linearMotorDirection is the direction we are moving relative to the vehicle coordinates
@@ -520,18 +532,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 m_linearMotorDirection *= keepfraction;
 
                 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);
+                    Prim.LocalID, origDir, origVel, addAmount, keepfraction, m_linearMotorDirection, m_lastLinearVelocityVector);
             }
             else
             {
                 // 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);
+                VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
             }
 
             // convert requested object velocity to object relative vector
-            Quaternion rotq = m_prim.Orientation;
+            Quaternion rotq = Prim.Orientation;
             m_newVelocity = m_lastLinearVelocityVector * rotq;
 
             // Add the various forces into m_dir which will be our new direction vector (velocity)
@@ -539,7 +551,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // add Gravity and Buoyancy
             // There is some gravity, make a gravity force vector that is applied after object velocity.
             // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            Vector3 grav = m_prim.PhysicsScene.DefaultGravity * (m_prim.Mass * (1f - m_VehicleBuoyancy));
+            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (Prim.Mass * (1f - m_VehicleBuoyancy));
 
             /*
              * RA: Not sure why one would do this
@@ -548,11 +560,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
              */
 
-            Vector3 pos = m_prim.Position;
+            Vector3 pos = Prim.Position;
 //            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 = m_prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
+            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;
@@ -560,8 +572,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             if (pos.Z < terrainHeight)
             {
                 pos.Z = terrainHeight + 2;
-                m_prim.Position = pos;
-                VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", m_prim.LocalID, terrainHeight, pos);
+                Prim.Position = pos;
+                VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
             }
 
             // Check if hovering
@@ -570,7 +582,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // We should hover, get the target height
                 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = m_prim.PhysicsScene.GetWaterLevelAtXYZ(pos) + m_VhoverHeight;
+                    m_VhoverTargetHeight = Prim.PhysicsScene.GetWaterLevelAtXYZ(pos) + m_VhoverHeight;
                 }
                 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                 {
@@ -590,7 +602,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 {
                     if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
                     {
-                        m_prim.Position = pos;
+                        Prim.Position = pos;
                     }
                 }
                 else
@@ -608,7 +620,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     }
                 }
 
-                VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", m_prim.LocalID, pos, m_newVelocity, m_VhoverHeight, m_VhoverTargetHeight);
+                VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", Prim.LocalID, pos, m_newVelocity, m_VhoverHeight, m_VhoverTargetHeight);
             }
 
             Vector3 posChange = pos - m_lastPositionVector;
@@ -642,9 +654,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
                 if (changed)
                 {
-                    m_prim.Position = pos;
+                    Prim.Position = pos;
                     VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
-                                m_prim.LocalID, m_BlockingEndPoint, posChange, pos);
+                                Prim.LocalID, m_BlockingEndPoint, posChange, pos);
                 }
             }
 
@@ -664,7 +676,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 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);
+                VDetailLog("{0},MoveLinear,limitMotorUp,grav={1}", Prim.LocalID, grav);
             }
             if ((m_flags & (VehicleFlag.NO_X)) != 0)
                 m_newVelocity.X = 0;
@@ -674,7 +686,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 m_newVelocity.Z = 0;
 
             // Apply velocity
-            m_prim.Velocity = m_newVelocity;
+            Prim.Velocity = m_newVelocity;
             // apply gravity force
             // Why is this set here? The physics engine already does gravity.
             // m_prim.AddForce(grav, false);
@@ -684,10 +696,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_lastLinearVelocityVector *= keepFraction;
 
             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);
+                    Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, grav, keepFraction);
 
         } // end MoveLinear()
 
+        // Apply the effect of the angular motor.
         private void MoveAngular(float pTimestep)
         {
             // m_angularMotorDirection         // angular velocity requested by LSL motor
@@ -699,7 +712,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // m_lastAngularVelocity           // what was last applied to body
 
             // Get what the body is doing, this includes 'external' influences
-            Vector3 angularVelocity = m_prim.RotationalVelocity;
+            Vector3 angularVelocity = Prim.RotationalVelocity;
 
             if (m_angularMotorApply > 0)
             {
@@ -716,7 +729,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 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}", 
-                        m_prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
+                        Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
 
                 // This is done so that if script request rate is less than phys frame rate the expected
                 //    velocity may still be acheived.
@@ -737,7 +750,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 float VAservo = 0.2f / (m_verticalAttractionTimescale / pTimestep);
                 // get present body rotation
-                Quaternion rotq = m_prim.Orientation;
+                Quaternion rotq = Prim.Orientation;
                 // make a vector pointing up
                 Vector3 verterr = Vector3.Zero;
                 verterr.Z = 1.0f;
@@ -767,7 +780,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 vertattr.Y += bounce * angularVelocity.Y;
 
                 VDetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}", 
-                            m_prim.LocalID, verterr, bounce, vertattr);
+                            Prim.LocalID, verterr, bounce, vertattr);
 
             } // else vertical attractor is off
 
@@ -784,13 +797,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 m_lastAngularVelocity.X = 0;
                 m_lastAngularVelocity.Y = 0;
-                VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
+                VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
             }
 
             if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
             {
                 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
-                VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
+                VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
             }
 
              // apply friction
@@ -798,14 +811,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
 
             // Apply to the body
-            m_prim.RotationalVelocity = m_lastAngularVelocity;
+            Prim.RotationalVelocity = m_lastAngularVelocity;
 
-            VDetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", m_prim.LocalID, decayamount, m_lastAngularVelocity);
+            VDetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", Prim.LocalID, decayamount, m_lastAngularVelocity);
         } //end MoveAngular
 
         internal void LimitRotation(float timestep)
         {
-            Quaternion rotq = m_prim.Orientation;
+            Quaternion rotq = Prim.Orientation;
             Quaternion m_rot = rotq;
             bool changed = false;
             if (m_RollreferenceFrame != Quaternion.Identity)
@@ -840,8 +853,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
             if (changed)
             {
-                m_prim.Orientation = m_rot;
-                VDetailLog("{0},LimitRotation,done,orig={1},new={2}", m_prim.LocalID, rotq, m_rot);
+                Prim.Orientation = m_rot;
+                VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
             }
 
         }
@@ -849,8 +862,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // Invoke the detailed logger and output something if it's enabled.
         private void VDetailLog(string msg, params Object[] args)
         {
-            if (m_prim.PhysicsScene.VehicleLoggingEnabled)
-                m_prim.PhysicsScene.PhysicsLogging.Write(msg, args);
+            if (Prim.PhysicsScene.VehicleLoggingEnabled)
+                Prim.PhysicsScene.PhysicsLogging.Write(msg, args);
         }
     }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 84a7fac..6967108 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -92,7 +92,9 @@ public class BSLinkset
     {
         lock (m_linksetActivityLock)
         {
-            AddChildToLinkset(child);
+            // Don't add the root to its own linkset
+            if (!IsRoot(child))
+                AddChildToLinkset(child);
         }
         return this;
     }
@@ -106,6 +108,17 @@ public class BSLinkset
         {
             if (IsRoot(child))
             {
+                // Cannot remove the root from a linkset.
+                return this;
+            }
+
+            RemoveChildFromLinkset(child);
+
+            /* Alternate implementation that destroys the linkset of the root is removed.
+             * This fails because items are added and removed from linksets to build shapes.
+             * Code left for reference.
+            if (IsRoot(child))
+            {
                 // if root of linkset, take the linkset apart
                 while (m_children.Count > 0)
                 {
@@ -120,6 +133,7 @@ public class BSLinkset
                 // Just removing a child from an existing linkset
                 RemoveChildFromLinkset(child);
             }
+             */
         }
 
         // The child is down to a linkset of just itself
@@ -308,12 +322,16 @@ public class BSLinkset
         {
             m_children.Add(child);
 
-            BSPhysObject rootx = LinksetRoot; // capture the root as of now
+            BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
+            BulletBody rootBodyx = LinksetRoot.BSBody;
             BSPhysObject childx = child;
+            BulletBody childBodyx = child.BSBody;
+
             PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
             {
                 DetailLog("{0},AddChildToLinkset,taint,child={1}", LinksetRoot.LocalID, child.LocalID);
-                PhysicallyLinkAChildToRoot(rootx, childx);     // build the physical binding between me and the child
+                // build the physical binding between me and the child
+                PhysicallyLinkAChildToRoot(rootx, rootBodyx, childx, childBodyx);
             });
         }
         return;
@@ -323,7 +341,7 @@ public class BSLinkset
     // This is not being called by the child so we have to make sure the child doesn't think
     //    it's still connected to the linkset.
     // Normal OpenSimulator operation will never do this because other SceneObjectPart information
-    //    has to be updated also (like pointer to prim's parent).
+    //    also has to be updated (like pointer to prim's parent).
     private void RemoveChildFromOtherLinkset(BSPhysObject pchild)
     {
         pchild.Linkset = new BSLinkset(PhysicsScene, pchild);
@@ -336,13 +354,15 @@ public class BSLinkset
     {
         if (m_children.Remove(child))
         {
-            BSPhysObject rootx = LinksetRoot; // capture the root as of now
+            BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
+            BulletBody rootBodyx = LinksetRoot.BSBody;
             BSPhysObject childx = child;
+            BulletBody childBodyx = child.BSBody;
             PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
             {
                 DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", LinksetRoot.LocalID, child.LocalID);
 
-                PhysicallyUnlinkAChildFromRoot(rootx, childx);
+                PhysicallyUnlinkAChildFromRoot(rootx, rootBodyx, childx, childBodyx);
                 RecomputeLinksetConstraintVariables();
             });
 
@@ -357,7 +377,8 @@ public class BSLinkset
 
     // Create a constraint between me (root of linkset) and the passed prim (the child).
     // Called at taint time!
-    private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
+    private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BulletBody rootBody,
+                                    BSPhysObject childPrim, BulletBody childBody)
     {
         // Zero motion for children so they don't interpolate
         childPrim.ZeroMotion();
@@ -371,16 +392,32 @@ public class BSLinkset
 
         // create a constraint that allows no freedom of movement between the two objects
         // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
-        DetailLog("{0},PhysicallyLinkAChildToRoot,taint,root={1},child={2},rLoc={3},cLoc={4},midLoc={5}", 
-            rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID, rootPrim.Position, childPrim.Position, midPoint);
+        DetailLog("{0},PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}", 
+                                        rootPrim.LocalID, 
+                                        rootPrim.LocalID, rootBody.ptr.ToString("X"), 
+                                        childPrim.LocalID, childBody.ptr.ToString("X"), 
+                                        rootPrim.Position, childPrim.Position, midPoint);
+
+        // There is great subtlty in these paramters. Notice the check for a ptr of zero.
+        // We pass the BulletBody structure into the taint in order to capture the pointer 
+        //     of the body at the time of constraint creation. This doesn't work for the very first
+        //     construction because there is no body yet. The body
+        //     is constructed later at taint time. Thus we use the body address at time of the
+        //     taint creation but, if it is zero, use what's in the prim at the moment.
+        //     There is a possible race condition since shape can change without a taint call
+        //     (like changing to a mesh that is already constructed). The fix for that would be
+        //     to only change BSShape at taint time thus syncronizing these operations at
+        //     the cost of efficiency and lag.
         BS6DofConstraint constrain = new BS6DofConstraint(
-                        PhysicsScene.World, rootPrim.BSBody, childPrim.BSBody,
+                        PhysicsScene.World, 
+                        rootBody.ptr == IntPtr.Zero ? rootPrim.BSBody : rootBody, 
+                        childBody.ptr == IntPtr.Zero ? childPrim.BSBody : childBody,
                         midPoint,
                         true,
                         true
                         );
         /* NOTE: below is an attempt to build constraint with full frame computation, etc.
-         *     Using the midpoint is easier since it lets the Bullet code use the transforms
+         *     Using the midpoint is easier since it lets the Bullet code manipulate the transforms
          *     of the objects.
          * Code left as a warning to future programmers.
         // ==================================================================================
@@ -433,19 +470,26 @@ public class BSLinkset
     }
 
     // Remove linkage between myself and a particular child
+    // The root and child bodies are passed in because we need to remove the constraint between
+    //      the bodies that were at unlink time.
     // Called at taint time!
-    private void PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
+    private void PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BulletBody rootBody, 
+                                                    BSPhysObject childPrim, BulletBody childBody)
     {
-        DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
+        DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}", 
+                            rootPrim.LocalID, 
+                            rootPrim.LocalID, rootBody.ptr.ToString("X"),
+                            childPrim.LocalID, childBody.ptr.ToString("X"));
 
         // Find the constraint for this link and get rid of it from the overall collection and from my list
-        PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody);
+        PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootBody, childBody);
 
         // Make the child refresh its location
         BulletSimAPI.PushUpdate2(childPrim.BSBody.ptr);
     }
 
-    // Remove linkage between myself and any possible children I might have
+    /*
+    // Remove linkage between myself and any possible children I might have.
     // Called at taint time!
     private void PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
     {
@@ -453,6 +497,7 @@ public class BSLinkset
 
         PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody);
     }
+     */
 
     // Invoke the detailed logger and output something if it's enabled.
     private void DetailLog(string msg, params Object[] args)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 4f83adc..df95625 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -135,8 +135,11 @@ public abstract class BSPhysObject : PhysicsActor
     //      collision event to make collision_end work.
     // Called at taint time from within the Step() function thus no locking problems
     //      with CollisionCollection and ObjectsWithNoMoreCollisions.
-    public virtual void SendCollisions()
+    // Return 'true' if there were some actual collisions passed up
+    public virtual bool SendCollisions()
     {
+        bool ret = true;
+
         // throttle the collisions to the number of milliseconds specified in the subscription
         int nowTime = PhysicsScene.SimulationNowTime;
         if (nowTime >= NextCollisionOkTime)
@@ -148,7 +151,7 @@ public abstract class BSPhysObject : PhysicsActor
             if (CollisionCollection.Count == 0)
             {
                 // If I have no collisions this time, remove me from the list of objects with collisions.
-                PhysicsScene.ObjectsWithNoMoreCollisions.Add(this);
+                ret = false;
             }
 
             // DetailLog("{0},{1}.SendCollisionUpdate,call,numCollisions={2}", LocalID, TypeName, CollisionCollection.Count);
@@ -158,6 +161,7 @@ public abstract class BSPhysObject : PhysicsActor
             // Make sure we don't have a handle to that one and that a new one is used for next time.
             CollisionCollection = new CollisionEventUpdate();
         }
+        return ret;
     }
 
     // Subscribe for collision events.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 68a153e..17ba85a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -507,6 +507,9 @@ public sealed class BSPrim : BSPhysObject
         // Set up the object physicalness (does gravity and collisions move this object)
         MakeDynamic(IsStatic);
 
+        // Do any vehicle stuff
+        _vehicle.Refresh();
+
         // Arrange for collision events if the simulator wants them
         EnableCollisions(SubscribedEvents());
 
@@ -556,9 +559,8 @@ public sealed class BSPrim : BSPhysObject
             BulletSimAPI.UpdateInertiaTensor2(BSBody.ptr);
             // There can be special things needed for implementing linksets
             Linkset.MakeStatic(this);
-            // The activation state is 'sleeping' so Bullet will not try to act on it
-            BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ISLAND_SLEEPING);
-            // BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.DISABLE_SIMULATION);
+            // The activation state is 'disabled' so Bullet will not try to act on it
+            BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.DISABLE_SIMULATION);
 
             BSBody.collisionFilter = CollisionFilterGroups.StaticObjectFilter;
             BSBody.collisionMask = CollisionFilterGroups.StaticObjectMask;
@@ -577,7 +579,8 @@ public sealed class BSPrim : BSPhysObject
 
             // A dynamic object has mass
             IntPtr collisionShapePtr = BulletSimAPI.GetCollisionShape2(BSBody.ptr);
-            OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(collisionShapePtr, Linkset.LinksetMass);
+            OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(collisionShapePtr, Mass);
+            // OMV.Vector3 inertia = OMV.Vector3.Zero;
             BulletSimAPI.SetMassProps2(BSBody.ptr, _mass, inertia);
             BulletSimAPI.UpdateInertiaTensor2(BSBody.ptr);
 
@@ -587,10 +590,12 @@ 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 can be special things needed for implementing linksets.
             Linkset.MakeDynamic(this);
 
             // Force activation of the object so Bullet will act on it.
+            // Must do the ForceActivationState2() to overcome the DISABLE_SIMULATION from static objects.
+            BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ISLAND_SLEEPING);
             BulletSimAPI.Activate2(BSBody.ptr, true);
 
             BSBody.collisionFilter = CollisionFilterGroups.ObjectFilter;
@@ -1457,9 +1462,16 @@ public sealed class BSPrim : BSPhysObject
         ShapeData shapeData;
         FillShapeInfo(out shapeData);
 
+        // Undo me from any possible linkset so, if body is rebuilt, the link will get restored.
+        // NOTE that the new linkset is not set. This saves the handle to the linkset
+        //     so we can add ourselves back when shape mangling is complete.
+        Linkset.RemoveMeFromLinkset(this);
+
         // 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);
+
+        Linkset = Linkset.AddMeToLinkset(this);
             
         // Make sure the properties are set on the new object
         UpdatePhysicalParameters();
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index e8c628c..44a249c 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -544,12 +544,15 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         // The above SendCollision's batch up the collisions on the objects.
         //      Now push the collisions into the simulator.
-        // If the object is done colliding, it will add itself to the ObjectsWithNoMoreCollisions list.
         if (ObjectsWithCollisions.Count > 0)
         {
             foreach (BSPhysObject bsp in ObjectsWithCollisions)
                 if (!m_avatars.Contains(bsp))   // don't call avatars twice
-                    bsp.SendCollisions();
+                    if (!bsp.SendCollisions())
+                    {
+                        // If the object is done colliding, see that it's removed from the colliding list
+                        ObjectsWithNoMoreCollisions.Add(bsp);
+                    }
         }
 
         // Objects that are done colliding are removed from the ObjectsWithCollisions list.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index aadb8d6..72c6df5 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -498,7 +498,8 @@ public class BSShapeCollection : IDisposable
         ulong newHullKey = ComputeShapeKey(shapeData, pbs, out lod);
 
         // if the hull hasn't changed, don't rebuild it
-        if (newHullKey == prim.BSShape.shapeKey) return false;
+        if (newHullKey == prim.BSShape.shapeKey && prim.BSShape.type == ShapeData.PhysicsShapeType.SHAPE_HULL)
+            return false;
 
         DetailLog("{0},BSShapeCollection.CreateGeomHull,create,oldKey={1},newKey={2}", 
                         prim.LocalID, prim.BSShape.shapeKey.ToString("X"), newHullKey.ToString("X"));
@@ -508,11 +509,8 @@ public class BSShapeCollection : IDisposable
 
         newShape = CreatePhysicalHull(prim.PhysObjectName, newHullKey, pbs, shapeData.Size, lod);
    
-        if (!ReferenceShape(newShape))
-        {
-            PhysicsScene.Logger.ErrorFormat("{0} Created new hull shape but one already exists: id={1}, key={2}, refCnt={3}", 
-                            LogHeader, shapeData.ID, newHullKey.ToString("X"), Hulls[newHullKey].referenceCount);
-        }
+        ReferenceShape(newShape);
+
         // hulls are already scaled by the meshmerizer
         prim.Scale = new OMV.Vector3(1f, 1f, 1f);
         prim.BSShape = newShape;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
index 50638d6..269c3d5 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -334,7 +334,8 @@ public class BSTerrainManager
                                     (uint)CollisionFilterGroups.TerrainMask);
 
                 // Make sure the new shape is processed.
-                BulletSimAPI.Activate2(mapInfo.terrainBody.ptr, true);
+                // BulletSimAPI.Activate2(mapInfo.terrainBody.ptr, true);
+                BulletSimAPI.ForceActivationState2(mapInfo.terrainBody.ptr, ActivationState.DISABLE_SIMULATION);
 
                 m_terrainModified = true;
             };
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 6910050..2d65a35 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -369,16 +369,16 @@ public enum CollisionFilterGroups : uint
     BSolidFilter             = 1 << 13,
 
     // The collsion filters and masked are defined in one place -- don't want them scattered
-    AvatarFilter            = BDefaultFilter | BCharacterFilter | BSolidFilter,
+    AvatarFilter            = BCharacterFilter,
     AvatarMask              = BAllFilter,
-    ObjectFilter            = BDefaultFilter | BSolidFilter,
+    ObjectFilter            = BSolidFilter,
     ObjectMask              = BAllFilter,
-    StaticObjectFilter      = BDefaultFilter | BStaticFilter | BSolidFilter,
+    StaticObjectFilter      = BStaticFilter,
     StaticObjectMask        = BAllFilter,
     VolumeDetectFilter      = BSensorTrigger,
     VolumeDetectMask        = ~BSensorTrigger,
     TerrainFilter           = BTerrainFilter,
-    TerrainMask             = BAllFilter,
+    TerrainMask             = BAllFilter & ~BStaticFilter,
     GroundPlaneFilter       = BAllFilter,
     GroundPlaneMask         = BAllFilter