BulletSim: debugging of compound shape implementation of linksets.

Add compound shape creation and freeing in shape manager.
Add optional taint-time execution method and update code to use it.
Add API2 linkage for more compound shape methods (get num, get/remove by index, ...)
Modify perferred shape return so linkset children can have differet shapes than root.
Add Position and Orientation calls to linksets so children can be moved around by
    the linkset by its own calculation. Allows for very general linkset implementations.

10 files changed, 274 insertions, 132 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 9e1206a..2a5397e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -200,7 +200,9 @@ public sealed class BSCharacter : BSPhysObject
     }
     // I want the physics engine to make an avatar capsule
     public override ShapeData.PhysicsShapeType PreferredPhysicalShape
-        { get { return ShapeData.PhysicsShapeType.SHAPE_AVATAR; } }
+    {
+        get {return ShapeData.PhysicsShapeType.SHAPE_AVATAR; }
+    }
 
     public override bool Grabbed {
         set { _grabbed = value; }
@@ -238,6 +240,7 @@ public sealed class BSCharacter : BSPhysObject
     }
     public override OMV.Vector3 Position {
         get {
+            // Don't refetch the position because this function is called a zillion times
             // _position = BulletSimAPI.GetObjectPosition2(Scene.World.ptr, LocalID);
             return _position;
         }
@@ -304,15 +307,11 @@ public sealed class BSCharacter : BSPhysObject
         {
             // The new position value must be pushed into the physics engine but we can't
             //    just assign to "Position" because of potential call loops.
-            BSScene.TaintCallback sanityOperation = delegate()
+            PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.PositionSanityCheck", delegate()
             {
                 DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
-            };
-            if (inTaintTime)
-                sanityOperation();
-            else
-                PhysicsScene.TaintedObject("BSCharacter.PositionSanityCheck", sanityOperation);
+            });
             ret = true;
         }
         return ret;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 9e0f499..8f973f4 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -48,7 +48,8 @@ public abstract class BSLinkset
          */
 
         // at the moment, there is only one
-        ret = new BSLinksetConstraints(physScene, parent);
+        // ret = new BSLinksetConstraints(physScene, parent);
+        ret = new BSLinksetCompound(physScene, parent);
 
         return ret;
     }
@@ -69,10 +70,19 @@ public abstract class BSLinkset
     protected object m_linksetActivityLock = new Object();
 
     // Some linksets have a preferred physical shape.
-    // Returns SHAPE_UNKNOWN if there is no preference.
-    public virtual ShapeData.PhysicsShapeType PreferredPhysicalShape
-        { get { return ShapeData.PhysicsShapeType.SHAPE_UNKNOWN; } }
+    // Returns SHAPE_UNKNOWN if there is no preference. Causes the correct shape to be selected.
+    public virtual ShapeData.PhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
+    {
+        return ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;
+    }
 
+    // Linksets move around the children so the linkset might need to compute the child position
+    public virtual OMV.Vector3 Position(BSPhysObject member)
+        { return member.RawPosition; }
+    public virtual OMV.Quaternion Orientation(BSPhysObject member)
+        { return member.RawOrientation; }
+    // TODO: does this need to be done for Velocity and RotationalVelocityy?
+    
     // We keep the prim's mass in the linkset structure since it could be dependent on other prims
     protected float m_mass;
     public float LinksetMass
@@ -177,7 +187,6 @@ public abstract class BSLinkset
     }
 
     // Perform an action on each member of the linkset including root prim.
-    // The action is performed only on the objects that are physically in the linkset.
     // Depends on the action on whether this should be done at taint time.
     public delegate bool ForEachMemberAction(BSPhysObject obj);
     public virtual bool ForEachMember(ForEachMemberAction action)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 638fae1..8b97ebb 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -41,18 +41,31 @@ public sealed class BSLinksetCompound : BSLinkset
         base.Initialize(scene, parent);
     }
 
+    // For compound implimented linksets, if there are children, use compound shape for the root.
+    public override ShapeData.PhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
+    { 
+        ShapeData.PhysicsShapeType ret = ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;
+        if (IsRoot(requestor) && HasAnyChildren)
+        {
+            ret = ShapeData.PhysicsShapeType.SHAPE_COMPOUND;
+        }
+        // DetailLog("{0},BSLinksetCompound.PreferredPhysicalShape,call,shape={1}", LinksetRoot.LocalID, ret);
+        return ret;
+    }
+
     // When physical properties are changed the linkset needs to recalculate
     //   its internal properties.
     // This is queued in the 'post taint' queue so the
     //   refresh will happen once after all the other taints are applied.
     public override void Refresh(BSPhysObject requestor)
     {
+        DetailLog("{0},BSLinksetCompound.Refresh,schedulingRefresh,requestor={1}", LinksetRoot.LocalID, requestor.LocalID);
         // Queue to happen after all the other taint processing
-        PhysicsScene.PostTaintObject("BSLinksetcompound.Refresh", requestor.LocalID, delegate()
-            {
-                if (HasAnyChildren && IsRoot(requestor))
-                    RecomputeLinksetCompound();
-            });
+        PhysicsScene.PostTaintObject("BSLinksetCompound.Refresh", requestor.LocalID, delegate()
+        {
+            if (IsRoot(requestor) && HasAnyChildren)
+                RecomputeLinksetCompound();
+        });
     }
 
     // The object is going dynamic (physical). Do any setup necessary
@@ -63,8 +76,17 @@ public sealed class BSLinksetCompound : BSLinkset
     // Called at taint-time!
     public override bool MakeDynamic(BSPhysObject child)
     {
-        // What is done for each object in BSPrim is what we want.
-        return false;
+        bool ret = false;
+        DetailLog("{0},BSLinksetCompound.MakeDynamic,call,isChild={1}", child.LocalID, HasChild(child));
+        if (HasChild(child))
+        {
+            // Physical children are removed from the world as the shape ofthe root compound
+            //     shape takes over.
+            BulletSimAPI.AddToCollisionFlags2(child.PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+            BulletSimAPI.ForceActivationState2(child.PhysBody.ptr, ActivationState.DISABLE_SIMULATION);
+            ret = true;
+        }
+        return ret;
     }
 
     // The object is going static (non-physical). Do any setup necessary for a static linkset.
@@ -74,8 +96,17 @@ public sealed class BSLinksetCompound : BSLinkset
     // Called at taint-time!
     public override bool MakeStatic(BSPhysObject child)
     {
-        // What is done for each object in BSPrim is what we want.
-        return false;
+        bool ret = false;
+        DetailLog("{0},BSLinksetCompound.MakeStatic,call,hasChild={1}", child.LocalID, HasChild(child));
+        if (HasChild(child))
+        {
+            // The non-physical children can come back to life.
+            BulletSimAPI.RemoveFromCollisionFlags2(child.PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+            // Don't force activation so setting of DISABLE_SIMULATION can stay.
+            BulletSimAPI.Activate2(child.PhysBody.ptr, false);
+            ret = true;
+        }
+        return ret;
     }
 
     // Called at taint-time!!
@@ -84,20 +115,35 @@ public sealed class BSLinksetCompound : BSLinkset
         // Nothing to do for constraints on property updates
     }
 
+    // The children move around in relationship to the root.
+    // Just grab the current values of wherever it is right now.
+    public override OMV.Vector3 Position(BSPhysObject member)
+    {
+        return BulletSimAPI.GetPosition2(member.PhysBody.ptr);
+    }
+
+    public override OMV.Quaternion Orientation(BSPhysObject member)
+    {
+        return BulletSimAPI.GetOrientation2(member.PhysBody.ptr);
+    }
+
     // Routine called when rebuilding the body of some member of the linkset.
-    // Destroy all the constraints have have been made to root and set
-    //     up to rebuild the constraints before the next simulation step.
+    // Since we don't keep in-physical world relationships, do nothing unless it's a child changing.
     // Returns 'true' of something was actually removed and would need restoring
     // Called at taint-time!!
     public override bool RemoveBodyDependencies(BSPrim child)
     {
         bool ret = false;
 
-        DetailLog("{0},BSLinksetcompound.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
-                                    child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"));
+        DetailLog("{0},BSLinksetCompound.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2},isRoot={3}",
+                                    child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), IsRoot(child));
 
-        // Cause the current shape to be freed and the new one to be built.
-        Refresh(LinksetRoot);
+        if (!IsRoot(child))
+        {
+            // Cause the current shape to be freed and the new one to be built.
+            Refresh(LinksetRoot);
+            ret = true;
+        }
 
         return ret;
     }
@@ -139,13 +185,19 @@ public sealed class BSLinksetCompound : BSLinkset
                             LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"),
                             child.LocalID, child.PhysBody.ptr.ToString("X"));
 
-            // See that the linkset parameters are recomputed at the end of the taint time.
-            Refresh(LinksetRoot);
-        }
-        else
-        {
-            // Non-fatal occurance.
-            // PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
+            // Cause the child's body to be rebuilt and thus restored to normal operation
+            child.ForceBodyShapeRebuild(false);
+
+            if (!HasAnyChildren)
+            {
+                // The linkset is now empty. The root needs rebuilding.
+                LinksetRoot.ForceBodyShapeRebuild(false);
+            }
+            else
+            {
+                // Schedule a rebuild of the linkset  before the next simulation tick.
+                Refresh(LinksetRoot);
+            }
         }
         return;
     }
@@ -158,16 +210,18 @@ public sealed class BSLinksetCompound : BSLinkset
     // Called at taint time!!
     private void RecomputeLinksetCompound()
     {
-        // Release the existing shape
-        PhysicsScene.Shapes.DereferenceShape(LinksetRoot.PhysShape, true, null);
-        
+        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},numChildren={2}",
+                        LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), NumberOfChildren);
+
+        LinksetRoot.ForceBodyShapeRebuild(true);
+
         float linksetMass = LinksetMass;
         LinksetRoot.UpdatePhysicalMassProperties(linksetMass);
 
             // DEBUG: see of inter-linkset collisions are causing problems
         // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, 
         //                     (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
-        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,set,rBody={1},linksetMass={2}",
+        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,end,rBody={1},linksetMass={2}",
                             LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), linksetMass);
 
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index 65aed77..67a59ef 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -84,6 +84,18 @@ public sealed class BSLinksetConstraints : BSLinkset
         // Nothing to do for constraints on property updates
     }
 
+    // The children of the linkset are moved around by the constraints.
+    // Just grab the current values of wherever it is right now.
+    public override OMV.Vector3 Position(BSPhysObject member)
+    {
+        return BulletSimAPI.GetPosition2(member.PhysBody.ptr);
+    }
+
+    public override OMV.Quaternion Orientation(BSPhysObject member)
+    {
+        return BulletSimAPI.GetOrientation2(member.PhysBody.ptr);
+    }
+
     // Routine called when rebuilding the body of some member of the linkset.
     // Destroy all the constraints have have been made to root and set
     //     up to rebuild the constraints before the next simulation step.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 65d7f34..7127aaf 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -81,7 +81,9 @@ public abstract class BSPhysObject : PhysicsActor
     // Some types of objects have preferred physical representations.
     // Returns SHAPE_UNKNOWN if there is no preference.
     public virtual ShapeData.PhysicsShapeType PreferredPhysicalShape
-        { get { return ShapeData.PhysicsShapeType.SHAPE_UNKNOWN; } }
+    {
+        get { return ShapeData.PhysicsShapeType.SHAPE_UNKNOWN; }
+    }
 
     // When the physical properties are updated, an EntityProperty holds the update values.
     // Keep the current and last EntityProperties to enable computation of differences
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 1754be6..af403aa 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -173,20 +173,16 @@ public sealed class BSPrim : BSPhysObject
     }
     // Whatever the linkset wants is what I want.
     public override ShapeData.PhysicsShapeType PreferredPhysicalShape
-        { get { return Linkset.PreferredPhysicalShape; } }
+        { get { return Linkset.PreferredPhysicalShape(this); } }
 
     public override bool ForceBodyShapeRebuild(bool inTaintTime)
     {
         LastAssetBuildFailed = false;
-        BSScene.TaintCallback rebuildOperation = delegate()
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ForceBodyShapeRebuild", delegate()
         {
             _mass = CalculateMass();   // changing the shape changes the mass
             CreateGeomAndObject(true);
-        };
-        if (inTaintTime)
-            rebuildOperation();
-        else
-            PhysicsScene.TaintedObject("BSPrim.ForceBodyShapeRebuild", rebuildOperation);
+        });
         return true;
     }
     public override bool Grabbed {
@@ -263,9 +259,9 @@ public sealed class BSPrim : BSPhysObject
     }
     public override OMV.Vector3 Position {
         get {
+            // child prims move around based on their parent. Need to get the latest location
             if (!Linkset.IsRoot(this))
-                // child prims move around based on their parent. Need to get the latest location
-                _position = BulletSimAPI.GetPosition2(PhysBody.ptr);
+                _position = Linkset.Position(this);
 
             // don't do the GetObjectPosition for root elements because this function is called a zillion times
             // _position = BulletSimAPI.GetObjectPosition2(PhysicsScene.World.ptr, BSBody.ptr);
@@ -344,16 +340,11 @@ public sealed class BSPrim : BSPhysObject
         {
             // The new position value must be pushed into the physics engine but we can't
             //    just assign to "Position" because of potential call loops.
-            BSScene.TaintCallback sanityOperation = delegate()
+            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.PositionSanityCheck", delegate()
             {
                 DetailLog("{0},BSPrim.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 ForcePosition = _position;
-            };
-            if (inTaintTime)
-                sanityOperation();
-            else
-                PhysicsScene.TaintedObject("BSPrim.PositionSanityCheck", sanityOperation);
-
+            });
             ret = true;
         }
         return ret;
@@ -542,10 +533,10 @@ public sealed class BSPrim : BSPhysObject
     }
     public override OMV.Quaternion Orientation {
         get {
+            // Children move around because tied to parent. Get a fresh value.
             if (!Linkset.IsRoot(this))
             {
-                // Children move around because tied to parent. Get a fresh value.
-                _orientation = BulletSimAPI.GetOrientation2(PhysBody.ptr);
+                _orientation = Linkset.Orientation(this);
             }
             return _orientation;
         }
@@ -946,7 +937,7 @@ public sealed class BSPrim : BSPhysObject
             m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
             return;
         }
-        BSScene.TaintCallback addForceOperation = delegate()
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
         {
             OMV.Vector3 fSum = OMV.Vector3.Zero;
             lock (m_accumulatedForces)
@@ -961,11 +952,7 @@ public sealed class BSPrim : BSPhysObject
             DetailLog("{0},BSPrim.AddForce,taint,force={1}", LocalID, fSum);
             if (fSum != OMV.Vector3.Zero)
                 BulletSimAPI.ApplyCentralForce2(PhysBody.ptr, fSum);
-        };
-        if (inTaintTime)
-            addForceOperation();
-        else
-            PhysicsScene.TaintedObject("BSPrim.AddForce", addForceOperation);
+        });
     }
 
     private List<OMV.Vector3> m_accumulatedAngularForces = new List<OMV.Vector3>();
@@ -985,7 +972,7 @@ public sealed class BSPrim : BSPhysObject
             m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
             return;
         }
-        BSScene.TaintCallback addAngularForceOperation = delegate()
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddAngularForce", delegate()
         {
             OMV.Vector3 fSum = OMV.Vector3.Zero;
             lock (m_accumulatedAngularForces)
@@ -1003,26 +990,19 @@ public sealed class BSPrim : BSPhysObject
                 BulletSimAPI.ApplyTorque2(PhysBody.ptr, fSum);
                 _torque = fSum;
             }
-        };
-        if (inTaintTime)
-            addAngularForceOperation();
-        else
-            PhysicsScene.TaintedObject("BSPrim.AddAngularForce", addAngularForceOperation);
+        });
     }
     // A torque impulse.
     public void ApplyTorqueImpulse(OMV.Vector3 impulse, bool inTaintTime)
     {
         OMV.Vector3 applyImpulse = impulse;
-        BSScene.TaintCallback applyTorqueImpulseOperation = delegate()
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ApplyTorqueImpulse", delegate()
         {
             DetailLog("{0},BSPrim.ApplyTorqueImpulse,taint,tImpulse={1}", LocalID, applyImpulse);
             BulletSimAPI.ApplyTorqueImpulse2(PhysBody.ptr, applyImpulse);
-        };
-        if (inTaintTime)
-            applyTorqueImpulseOperation();
-        else
-            PhysicsScene.TaintedObject("BSPrim.ApplyTorqueImpulse", applyTorqueImpulseOperation);
+        });
     }
+
     public override void SetMomentum(OMV.Vector3 momentum) {
         // DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum);
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index cc5dbb2..dcfcb83 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -692,6 +692,16 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         return;
     }
 
+    // Sometimes a potentially tainted operation can be used in and out of taint time.
+    // This routine executes the command immediately if in taint-time otherwise it is queued.
+    public void TaintedObject(bool inTaintTime, string ident, TaintCallback callback)
+    {
+        if (inTaintTime)
+            callback();
+        else
+            TaintedObject(ident, callback);
+    }
+
     // When someone tries to change a property on a BSPrim or BSCharacter, the object queues
     // a callback into itself to do the actual property change. That callback is called
     // here just before the physics engine is called to step the simulation.
@@ -1438,7 +1448,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     {
         PhysicsLogging.Write(msg, args);
         // Add the Flush() if debugging crashes to get all the messages written out.
-        // PhysicsLogging.Flush();
+        PhysicsLogging.Flush();
     }
     // Used to fill in the LocalID when there isn't one. It's the correct number of characters.
     public const string DetailLogZero = "0000000000";
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index e131919..107befe 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -122,18 +122,14 @@ public sealed class BSShapeCollection : IDisposable
         lock (m_collectionActivityLock)
         {
             DetailLog("{0},BSShapeCollection.ReferenceBody,newBody", body.ID, body);
-            BSScene.TaintCallback createOperation = delegate()
+            PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.ReferenceBody", delegate()
             {
                 if (!BulletSimAPI.IsInWorld2(body.ptr))
                 {
                     BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, body.ptr);
                     DetailLog("{0},BSShapeCollection.ReferenceBody,addedToWorld,ref={1}", body.ID, body);
                 }
-            };
-            if (inTaintTime)
-                createOperation();
-            else
-                PhysicsScene.TaintedObject("BSShapeCollection.ReferenceBody", createOperation);
+            });
         }
     }
 
@@ -146,7 +142,7 @@ public sealed class BSShapeCollection : IDisposable
 
         lock (m_collectionActivityLock)
         {
-            BSScene.TaintCallback removeOperation = delegate()
+            PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.DereferenceBody", delegate()
             {
                 DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody. ptr={1}, inTaintTime={2}",
                                             body.ID, body.ptr.ToString("X"), inTaintTime);
@@ -159,12 +155,7 @@ public sealed class BSShapeCollection : IDisposable
                 // Zero any reference to the shape so it is not freed when the body is deleted.
                 BulletSimAPI.SetCollisionShape2(PhysicsScene.World.ptr, body.ptr, IntPtr.Zero);
                 BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, body.ptr);
-            };
-            // If already in taint-time, do the operations now. Otherwise queue for later.
-            if (inTaintTime)
-                removeOperation();
-            else
-                PhysicsScene.TaintedObject("BSShapeCollection.DereferenceBody", removeOperation);
+            });
         }
     }
 
@@ -238,7 +229,7 @@ public sealed class BSShapeCollection : IDisposable
         if (shape.ptr == IntPtr.Zero)
             return;
 
-        BSScene.TaintCallback dereferenceOperation = delegate()
+        PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.DereferenceShape", delegate()
         {
             if (shape.ptr != IntPtr.Zero)
             {
@@ -270,18 +261,7 @@ public sealed class BSShapeCollection : IDisposable
                     }
                 }
             }
-        };
-        if (inTaintTime)
-        {
-            lock (m_collectionActivityLock)
-            {
-                dereferenceOperation();
-            }
-        }
-        else
-        {
-            PhysicsScene.TaintedObject("BSShapeCollection.DereferenceShape", dereferenceOperation);
-        }
+        });
     }
 
     // Count down the reference count for a mesh shape
@@ -311,7 +291,10 @@ public sealed class BSShapeCollection : IDisposable
         {
             hullDesc.referenceCount--;
             // TODO: release the Bullet storage (aging old entries?)
+
+            // Tell upper layers that, if they have dependencies on this shape, this link is going away
             if (shapeCallback != null) shapeCallback(shape);
+
             hullDesc.lastReferenced = System.DateTime.Now;
             Hulls[shape.shapeKey] = hullDesc;
             DetailLog("{0},BSShapeCollection.DereferenceHull,key={1},refCnt={2}",
@@ -320,10 +303,48 @@ public sealed class BSShapeCollection : IDisposable
     }
 
     // Remove a reference to a compound shape.
+    // Taking a compound shape apart is a little tricky because if you just delete the
+    //      physical object, it will free all the underlying children. We can't do that because
+    //      they could be shared. So, this removes each of the children from the compound and
+    //      dereferences them separately before destroying the compound collision object itself.
     // Called at taint-time.
     private void DereferenceCompound(BulletShape shape, ShapeDestructionCallback shapeCallback)
     {
-        // Compound shape is made of a bunch of meshes and natives.
+        if (!BulletSimAPI.IsCompound2(shape.ptr))
+        {
+            // Failed the sanity check!!
+            PhysicsScene.Logger.ErrorFormat("{0} Attempt to free a compound shape that is not compound!! type={1}, ptr={2}",
+                                        LogHeader, shape.type, shape.ptr.ToString("X"));
+            DetailLog("{0},BSShapeCollection.DereferenceCompound,notACompoundShape,type={1},ptr={2}",
+                                        BSScene.DetailLogZero, shape.type, shape.ptr.ToString("X"));
+            return;
+        }
+        int numChildren = BulletSimAPI.GetNumberOfCompoundChildren2(shape.ptr);
+        for (int ii = 0; ii < numChildren; ii++)
+        {
+            IntPtr childShape = BulletSimAPI.RemoveChildShapeFromCompoundShapeIndex2(shape.ptr, ii);
+            DereferenceAnonCollisionShape(childShape);
+        }
+        BulletSimAPI.DeleteCollisionShape2(PhysicsScene.World.ptr, shape.ptr);
+    }
+
+    // Sometimes we have a pointer to a collision shape but don't know what type it is.
+    // Figure out type and call the correct dereference routine.
+    // This is coming from a compound shape that we created so we know it is either native or mesh.
+    // Called at taint-time.
+    private void DereferenceAnonCollisionShape(IntPtr cShape)
+    {
+        BulletShape shapeInfo = new BulletShape(cShape, ShapeData.PhysicsShapeType.SHAPE_MESH);
+        if (BulletSimAPI.IsCompound2(cShape))
+            shapeInfo.type = ShapeData.PhysicsShapeType.SHAPE_COMPOUND;
+
+        if (BulletSimAPI.IsNativeShape2(cShape))
+        {
+            shapeInfo.isNativeShape = true;
+            shapeInfo.type = ShapeData.PhysicsShapeType.SHAPE_BOX; // (technically, type doesn't matter)
+        }
+
+        DereferenceShape(shapeInfo, true, null);
     }
 
     // Create the geometry information in Bullet for later use.
@@ -338,10 +359,8 @@ public sealed class BSShapeCollection : IDisposable
     {
         bool ret = false;
         bool haveShape = false;
-        bool nativeShapePossible = true;
-        PrimitiveBaseShape pbs = prim.BaseShape;
 
-        if (prim.PreferredPhysicalShape == ShapeData.PhysicsShapeType.SHAPE_AVATAR)
+        if (!haveShape && prim.PreferredPhysicalShape == ShapeData.PhysicsShapeType.SHAPE_AVATAR)
         {
             // an avatar capsule is close to a native shape (it is not shared)
             ret = GetReferenceToNativeShape(prim, ShapeData.PhysicsShapeType.SHAPE_AVATAR,
@@ -350,6 +369,31 @@ public sealed class BSShapeCollection : IDisposable
             ret = true;
             haveShape = true;
         }
+
+        // Compound shapes are handled special as they are rebuilt from scratch.
+        // This isn't too great a hardship since most of the child shapes will already been created.
+        if (!haveShape && prim.PreferredPhysicalShape == ShapeData.PhysicsShapeType.SHAPE_COMPOUND)
+        {
+            ret = GetReferenceToCompoundShape(prim, shapeCallback);
+            DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, prim.PhysShape);
+            haveShape = true;
+        }
+
+        if (!haveShape)
+        {
+            ret = CreateGeomNonSpecial(forceRebuild, prim, shapeCallback);
+        }
+
+        return ret;
+    }
+
+    private bool CreateGeomNonSpecial(bool forceRebuild, BSPhysObject prim, ShapeDestructionCallback shapeCallback)
+    {
+        bool ret = false;
+        bool haveShape = false;
+        bool nativeShapePossible = true;
+        PrimitiveBaseShape pbs = prim.BaseShape;
+
         // If the prim attributes are simple, this could be a simple Bullet native shape
         if (!haveShape
                 && pbs != null
@@ -363,6 +407,7 @@ public sealed class BSShapeCollection : IDisposable
                         && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
                         && pbs.PathShearX == 0 && pbs.PathShearY == 0) ) )
         {
+            // It doesn't look like Bullet scales spheres so make sure the scales are all equal
             if ((pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1)
                                 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)
             {
@@ -378,7 +423,7 @@ public sealed class BSShapeCollection : IDisposable
                                         prim.LocalID, forceRebuild, prim.PhysShape);
                 }
             }
-            if (pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
+            if (!haveShape && pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
             {
                 haveShape = true;
                 if (forceRebuild
@@ -393,9 +438,10 @@ public sealed class BSShapeCollection : IDisposable
                 }
             }
         }
+
         // If a simple shape is not happening, create a mesh and possibly a hull.
         // Note that if it's a native shape, the check for physical/non-physical is not
-        //     made. Native shapes are best used in either case.
+        //     made. Native shapes work in either case.
         if (!haveShape && pbs != null)
         {
             if (prim.IsPhysical && PhysicsScene.ShouldUseHullsForPhysicalObjects)
@@ -487,7 +533,7 @@ public sealed class BSShapeCollection : IDisposable
         if (newMeshKey == prim.PhysShape.shapeKey && prim.PhysShape.type == ShapeData.PhysicsShapeType.SHAPE_MESH)
             return false;
 
-        DetailLog("{0},BSShapeCollection.CreateGeomMesh,create,oldKey={1},newKey={2}",
+        DetailLog("{0},BSShapeCollection.GetReferenceToMesh,create,oldKey={1},newKey={2}",
                                 prim.LocalID, prim.PhysShape.shapeKey.ToString("X"), newMeshKey.ToString("X"));
 
         // Since we're recreating new, get rid of the reference to the previous shape
@@ -535,7 +581,7 @@ public sealed class BSShapeCollection : IDisposable
                     verticesAsFloats[vi++] = vv.Z;
                 }
 
-                // m_log.DebugFormat("{0}: CreateGeomMesh: calling CreateMesh. lid={1}, key={2}, indices={3}, vertices={4}",
+                // m_log.DebugFormat("{0}: BSShapeCollection.CreatePhysicalMesh: calling CreateMesh. lid={1}, key={2}, indices={3}, vertices={4}",
                 //                  LogHeader, prim.LocalID, newMeshKey, indices.Length, vertices.Count);
 
                 meshPtr = BulletSimAPI.CreateMeshShape2(PhysicsScene.World.ptr,
@@ -561,7 +607,7 @@ public sealed class BSShapeCollection : IDisposable
         if (newHullKey == prim.PhysShape.shapeKey && prim.PhysShape.type == ShapeData.PhysicsShapeType.SHAPE_HULL)
             return false;
 
-        DetailLog("{0},BSShapeCollection.CreateGeomHull,create,oldKey={1},newKey={2}",
+        DetailLog("{0},BSShapeCollection.GetReferenceToHull,create,oldKey={1},newKey={2}",
                         prim.LocalID, prim.PhysShape.shapeKey.ToString("X"), newHullKey.ToString("X"));
 
         // Remove usage of the previous shape.
@@ -693,6 +739,42 @@ public sealed class BSShapeCollection : IDisposable
         return;
     }
 
+    // Compound shapes are always built from scratch.
+    // This shouldn't be to bad since most of the parts will be meshes that had been built previously.
+    private bool GetReferenceToCompoundShape(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
+    {
+        BulletShape cShape = new BulletShape(
+            BulletSimAPI.CreateCompoundShape2(PhysicsScene.World.ptr), ShapeData.PhysicsShapeType.SHAPE_COMPOUND);
+
+        // The prim's linkset is the source of the children.
+        // TODO: there is too much knowledge here about the internals of linksets and too much
+        //     dependency on the relationship of compound shapes and linksets (what if we want to use
+        //     compound shapes for something else?). Think through this and clean up so the
+        //     appropriate knowledge is used at the correct software levels.
+
+        // Recreate the geometry of the root prim (might have been a linkset root in the past)
+        CreateGeomNonSpecial(true, prim, null);
+
+        BSPhysObject rootPrim = prim.Linkset.LinksetRoot;
+
+        prim.Linkset.ForEachMember(delegate(BSPhysObject cPrim)
+        {
+            OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(rootPrim.RawOrientation);
+            OMV.Vector3 displacementPos = (cPrim.RawPosition - rootPrim.RawPosition) * invRootOrientation;
+            OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
+
+            DetailLog("{0},BSShapeCollection.GetReferenceToCompoundShape,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}",
+                prim.LocalID, cPrim.LocalID, cPrim.PhysShape.ptr.ToString("X"), displacementPos, displacementRot);
+
+            BulletSimAPI.AddChildShapeToCompoundShape2(cShape.ptr, cPrim.PhysShape.ptr, displacementPos, displacementRot);
+            return false;
+        });
+
+        prim.PhysShape = cShape;
+
+        return true;
+    }
+
     // Create a hash of all the shape parameters to be used as a key
     //    for this particular shape.
     private System.UInt64 ComputeShapeKey(OMV.Vector3 size, PrimitiveBaseShape pbs, out float retLod)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
index 11298fe..7c34af2 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -238,7 +238,7 @@ public sealed class BSTerrainManager
             DetailLog("{0},UpdateOrCreateTerrain:UpdateExisting,call,terrainBase={1},minC={2}, maxC={3}, szX={4}, szY={5}",
                         BSScene.DetailLogZero, terrainRegionBase, mapInfo.minCoords, mapInfo.maxCoords, mapInfo.sizeX, mapInfo.sizeY);
 
-            BSScene.TaintCallback rebuildOperation = delegate()
+            PhysicsScene.TaintedObject(inTaintTime, "BSScene.UpdateOrCreateTerrain:UpdateExisting", delegate()
             {
                 if (MegaRegionParentPhysicsScene != null)
                 {
@@ -337,14 +337,7 @@ public sealed class BSTerrainManager
                 BulletSimAPI.ForceActivationState2(mapInfo.terrainBody.ptr, ActivationState.DISABLE_SIMULATION);
 
                 m_terrainModified = true;
-            };
-
-            // There is the option to do the changes now (we're already in 'taint time'), or
-            //     to do the Bullet operations later.
-            if (inTaintTime)
-                rebuildOperation();
-            else
-                PhysicsScene.TaintedObject("BSScene.UpdateOrCreateTerrain:UpdateExisting", rebuildOperation);
+            });
         }
         else
         {
@@ -364,7 +357,7 @@ public sealed class BSTerrainManager
                             BSScene.DetailLogZero, newTerrainID, minCoords, minCoords);
 
             // Code that must happen at taint-time
-            BSScene.TaintCallback createOperation = delegate()
+            PhysicsScene.TaintedObject(inTaintTime, "BSScene.UpdateOrCreateTerrain:NewTerrain", delegate()
             {
                 DetailLog("{0},UpdateOrCreateTerrain:NewTerrain,taint,baseX={1},baseY={2}", BSScene.DetailLogZero, minCoords.X, minCoords.Y);
                 // Create a new mapInfo that will be filled with the new info
@@ -377,13 +370,7 @@ public sealed class BSTerrainManager
                 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.
-            if (inTaintTime)
-                createOperation();
-            else
-                PhysicsScene.TaintedObject("BSScene.UpdateOrCreateTerrain:NewTerrain", createOperation);
+            });
         }
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 3b6355c..143b8be 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -399,8 +399,6 @@ public enum CollisionFilterGroups : uint
 
 };
 
-
-
 // CFM controls the 'hardness' of the constraint. 0=fixed, 0..1=violatable. Default=0
 // ERP controls amount of correction per tick. Usable range=0.1..0.8. Default=0.2.
 public enum ConstraintParams : int
@@ -618,10 +616,19 @@ public static extern IntPtr BuildCapsuleShape2(IntPtr world, float radius, float
 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);
+public static extern int GetNumberOfCompoundChildren2(IntPtr cShape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void AddChildShapeToCompoundShape2(IntPtr cShape, IntPtr addShape, Vector3 pos, Quaternion rot);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetChildShapeFromCompoundShapeIndex2(IntPtr cShape, int indx);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr RemoveChildShapeFromCompoundShapeIndex2(IntPtr cShape, int indx);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void RemoveChildFromCompoundShape2(IntPtr cShape, IntPtr removeShape);
+public static extern void RemoveChildShapeFromCompoundShape2(IntPtr cShape, IntPtr removeShape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr DuplicateCollisionShape2(IntPtr sim, IntPtr srcShape, uint id);