3 files changed, 93 insertions, 80 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index dff71af..4f225ae 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -43,8 +43,17 @@ public class BSLinkset
     static int m_nextLinksetID = 1;
     public int LinksetID { get; private set; }
 
-    // The children under the root in this linkset
+    // The children under the root in this linkset.
+    // There are two lists of children: the current children at runtime
+    //    and the children at taint-time. For instance, if you delink a
+    //    child from the linkset, the child is removed from m_children
+    //    but the constraint won't be removed until taint time.
+    //    Two lists lets this track the 'current' children and
+    //    the physical 'taint' children separately.
+    // After taint processing and before the simulation step, these
+    //    two lists must be the same.
     private List<BSPhysObject> m_children;
+    private List<BSPhysObject> m_taintChildren;
 
     // We lock the diddling of linkset classes to prevent any badness.
     // This locks the modification of the instances of this class. Changes
@@ -82,6 +91,7 @@ public class BSLinkset
         PhysicsScene = scene;
         LinksetRoot = parent;
         m_children = new List<BSPhysObject>();
+        m_taintChildren = new List<BSPhysObject>();
         m_mass = parent.MassRaw;
     }
 
@@ -194,30 +204,40 @@ public class BSLinkset
     // Routine used when rebuilding the body of the root of the linkset
     // Destroy all the constraints have have been made to root.
     // This is called when the root body is changing.
+    // Returns 'true' of something eas actually removed and would need restoring
     // Called at taint-time!!
-    public void RemoveBodyDependencies(BSPrim child)
+    public bool RemoveBodyDependencies(BSPrim child)
     {
+        bool ret = false;
+
         lock (m_linksetActivityLock)
         {
             if (IsRoot(child))
             {
                 // If the one with the dependency is root, must undo all children
                 DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},numChild={2}",
-                                                LinksetRoot.LocalID, m_children.Count);
-                foreach (BSPhysObject bpo in m_children)
+                                                child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
+                foreach (BSPhysObject bpo in m_taintChildren)
                 {
                     PhysicallyUnlinkAChildFromRoot(LinksetRoot, LinksetRoot.BSBody, bpo, bpo.BSBody);
+                    ret = true;
                 }
             }
             else
             {
                 DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
+                                                child.LocalID,
                                                 LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
                                                 child.LocalID, child.BSBody.ptr.ToString("X"));
                 // Remove the dependency on the body of this one 
-                PhysicallyUnlinkAChildFromRoot(LinksetRoot, LinksetRoot.BSBody, child, child.BSBody);
+                if (m_taintChildren.Contains(child))
+                {
+                    PhysicallyUnlinkAChildFromRoot(LinksetRoot, LinksetRoot.BSBody, child, child.BSBody);
+                    ret = true;
+                }
             }
         }
+        return ret;
     }
 
     // Routine used when rebuilding the body of the root of the linkset
@@ -231,8 +251,8 @@ public class BSLinkset
             if (IsRoot(child))
             {
                 DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
-                                                LinksetRoot.LocalID, m_children.Count);
-                foreach (BSPhysObject bpo in m_children)
+                                                child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
+                foreach (BSPhysObject bpo in m_taintChildren)
                 {
                     PhysicallyLinkAChildToRoot(LinksetRoot, LinksetRoot.BSBody, bpo, bpo.BSBody);
                 }
@@ -240,6 +260,7 @@ public class BSLinkset
             else
             {
                 DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
+                                                LinksetRoot.LocalID,
                                                 LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
                                                 child.LocalID, child.BSBody.ptr.ToString("X"));
                 PhysicallyLinkAChildToRoot(LinksetRoot, LinksetRoot.BSBody, child, child.BSBody);
@@ -256,7 +277,7 @@ public class BSLinkset
         lock (m_linksetActivityLock)
         {
             mass = LinksetRoot.MassRaw;
-            foreach (BSPhysObject bp in m_children)
+            foreach (BSPhysObject bp in m_taintChildren)
             {
                 mass += bp.MassRaw;
             }
@@ -272,7 +293,7 @@ public class BSLinkset
             com = LinksetRoot.Position * LinksetRoot.MassRaw;
             float totalMass = LinksetRoot.MassRaw;
 
-            foreach (BSPhysObject bp in m_children)
+            foreach (BSPhysObject bp in m_taintChildren)
             {
                 com += bp.Position * bp.MassRaw;
                 totalMass += bp.MassRaw;
@@ -291,70 +312,16 @@ public class BSLinkset
         {
             com = LinksetRoot.Position;
 
-            foreach (BSPhysObject bp in m_children)
+            foreach (BSPhysObject bp in m_taintChildren)
             {
                 com += bp.Position * bp.MassRaw;
             }
-            com /= (m_children.Count + 1);
+            com /= (m_taintChildren.Count + 1);
         }
 
         return com;
     }
 
-    // Call each of the constraints that make up this linkset and recompute the
-    //    various transforms and variables. Used when objects are added or removed
-    //    from a linkset to make sure the constraints know about the new mass and
-    //    geometry.
-    // Must only be called at taint time!!
-    private void RecomputeLinksetConstraintVariables()
-    {
-        float linksetMass = LinksetMass;
-        lock (m_linksetActivityLock)
-        {
-            bool somethingMissing = false;
-            foreach (BSPhysObject child in m_children)
-            {
-                BSConstraint constrain;
-                if (PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.BSBody, child.BSBody, out constrain))
-                {
-                    // DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,taint,child={1},mass={2},A={3},B={4}",
-                    //         LinksetRoot.LocalID, child.LocalID, linksetMass, constrain.Body1.ID, constrain.Body2.ID);
-                    constrain.RecomputeConstraintVariables(linksetMass);
-                }
-                else
-                {
-                    // 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.
-                    somethingMissing = true;
-                    break;
-                }
-            }
-
-            // If the whole linkset is not here, doesn't make sense to recompute linkset wide values
-            if (!somethingMissing)
-            {
-                // If this is a multiple object linkset, set everybody's center of mass to the set's center of mass
-                OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
-                BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.ptr, centerOfMass, OMV.Quaternion.Identity);
-                foreach (BSPhysObject child in m_children)
-                {
-                    BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, centerOfMass, OMV.Quaternion.Identity);
-                }
-                /*
-                // 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;
-    }
-
     // I am the root of a linkset and a new child is being added
     // Called while LinkActivity is locked.
     private void AddChildToLinkset(BSPhysObject child)
@@ -377,6 +344,7 @@ public class BSLinkset
             {
                 DetailLog("{0},AddChildToLinkset,taint,child={1}", LinksetRoot.LocalID, child.LocalID);
                 // build the physical binding between me and the child
+                m_taintChildren.Add(childx);
                 PhysicallyLinkAChildToRoot(rootx, rootBodyx, childx, childBodyx);
             });
         }
@@ -405,13 +373,16 @@ public class BSLinkset
             BSPhysObject childx = child;
             BulletBody childBodyx = child.BSBody;
 
-            DetailLog("{0},RemoveChildFromLinkset,call,child={1}", 
-                            rootx.LocalID,
+            DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}", 
+                            childx.LocalID,
                             rootx.LocalID, rootBodyx.ptr.ToString("X"),
                             childx.LocalID, childBodyx.ptr.ToString("X"));
 
             PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
             {
+                if (m_taintChildren.Contains(childx))
+                    m_taintChildren.Remove(childx);
+
                 PhysicallyUnlinkAChildFromRoot(rootx, rootBodyx, childx, childBodyx);
                 RecomputeLinksetConstraintVariables();
             });
@@ -551,6 +522,46 @@ public class BSLinkset
     }
      */
 
+    // Call each of the constraints that make up this linkset and recompute the
+    //    various transforms and variables. Used when objects are added or removed
+    //    from a linkset to make sure the constraints know about the new mass and
+    //    geometry.
+    // Must only be called at taint time!!
+    private void RecomputeLinksetConstraintVariables()
+    {
+        float linksetMass = LinksetMass;
+        foreach (BSPhysObject child in m_taintChildren)
+        {
+            BSConstraint constrain;
+            if (PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.BSBody, child.BSBody, out constrain))
+            {
+                // DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,taint,child={1},mass={2},A={3},B={4}",
+                //         LinksetRoot.LocalID, child.LocalID, linksetMass, constrain.Body1.ID, constrain.Body2.ID);
+                constrain.RecomputeConstraintVariables(linksetMass);
+            }
+            else
+            {
+                // Non-fatal error that happens when children are being added to the linkset but
+                //    their constraints have not been created yet.
+                break;
+            }
+        }
+
+        // If the whole linkset is not here, doesn't make sense to recompute linkset wide values
+        if (m_children.Count == m_taintChildren.Count)
+        {
+            // If this is a multiple object linkset, set everybody's center of mass to the set's center of mass
+            OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
+            BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.ptr, centerOfMass, OMV.Quaternion.Identity);
+            foreach (BSPhysObject child in m_taintChildren)
+            {
+                BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, centerOfMass, OMV.Quaternion.Identity);
+            }
+        }
+        return;
+    }
+
+
     // 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/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index c879143..87ffe3e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -1120,8 +1120,7 @@ public sealed class BSPrim : BSPhysObject
         {
             // Called if the current prim body is about to be destroyed.
             // The problem is the constraints for Linksets which need to be updated for the new body.
-            Linkset.RemoveBodyDependencies(this);
-            needToRestoreLinkset = true;
+            needToRestoreLinkset = Linkset.RemoveBodyDependencies(this);
         });
 
         if (needToRestoreLinkset)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index 2618971..648910a 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -144,34 +144,37 @@ public class BSShapeCollection : IDisposable
 
     // Release the usage of a body.
     // Called when releasing use of a BSBody. BSShape is handled separately.
-    public void DereferenceBody(BulletBody shape, bool inTaintTime, BodyDestructionCallback bodyCallback )
+    public void DereferenceBody(BulletBody body, bool inTaintTime, BodyDestructionCallback bodyCallback )
     {
-        if (shape.ptr == IntPtr.Zero)
+        if (body.ptr == IntPtr.Zero)
             return;
 
         lock (m_collectionActivityLock)
         {
             BodyDesc bodyDesc;
-            if (Bodies.TryGetValue(shape.ID, out bodyDesc))
+            if (Bodies.TryGetValue(body.ID, out bodyDesc))
             {
                 bodyDesc.referenceCount--;
                 bodyDesc.lastReferenced = System.DateTime.Now;
-                Bodies[shape.ID] = bodyDesc;
-                DetailLog("{0},BSShapeCollection.DereferenceBody,ref={1}", shape.ID, bodyDesc.referenceCount);
+                Bodies[body.ID] = bodyDesc;
+                DetailLog("{0},BSShapeCollection.DereferenceBody,ref={1}", body.ID, bodyDesc.referenceCount);
 
                 // If body is no longer being used, free it -- bodies are never shared.
                 if (bodyDesc.referenceCount == 0)
                 {
-                    Bodies.Remove(shape.ID);
+                    Bodies.Remove(body.ID);
                     BSScene.TaintCallback removeOperation = delegate()
                     {
                         DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody. ptr={1}",
-                                                    shape.ID, shape.ptr.ToString("X"));
+                                                    body.ID, body.ptr.ToString("X"));
+                        // If the caller needs to know, pass the event up.
+                        if (bodyCallback != null) bodyCallback(body);
+
                         // Zero any reference to the shape so it is not freed when the body is deleted.
-                        BulletSimAPI.SetCollisionShape2(PhysicsScene.World.ptr, shape.ptr, IntPtr.Zero);
+                        BulletSimAPI.SetCollisionShape2(PhysicsScene.World.ptr, body.ptr, IntPtr.Zero);
                         // It may have already been removed from the world in which case the next is a NOOP.
-                        BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, shape.ptr);
-                        BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, shape.ptr);
+                        BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, body.ptr);
+                        BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, body.ptr);
                     };
                     // If already in taint-time, do the operations now. Otherwise queue for later.
                     if (inTaintTime)
@@ -182,7 +185,7 @@ public class BSShapeCollection : IDisposable
             }
             else
             {
-                DetailLog("{0},BSShapeCollection.DereferenceBody,DID NOT FIND BODY", shape.ID, bodyDesc.referenceCount);
+                DetailLog("{0},BSShapeCollection.DereferenceBody,DID NOT FIND BODY", body.ID, bodyDesc.referenceCount);
             }
         }
     }