2 files changed, 197 insertions, 147 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
index 862b744..22ea367 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
@@ -56,21 +56,25 @@ public class BSConstraintCollection : IDisposable
 
     public void Clear()
     {
-        foreach (BSConstraint cons in m_constraints)
+        lock (m_constraints)
         {
-            cons.Dispose();
+            foreach (BSConstraint cons in m_constraints)
+            {
+                cons.Dispose();
+            }
+            m_constraints.Clear();
         }
-        m_constraints.Clear();
     }
 
     public bool AddConstraint(BSConstraint cons)
     {
-        // There is only one constraint between any bodies. Remove any old just to make sure.
-        RemoveAndDestroyConstraint(cons.Body1, cons.Body2);
-
-        m_world.scene.DetailLog("{0},BSConstraintCollection.AddConstraint,call,body1={1},body2={2}", BSScene.DetailLogZero, cons.Body1.ID, cons.Body2.ID);
+        lock (m_constraints)
+        {
+            // There is only one constraint between any bodies. Remove any old just to make sure.
+            RemoveAndDestroyConstraint(cons.Body1, cons.Body2);
 
-        m_constraints.Add(cons);
+            m_constraints.Add(cons);
+        }
 
         return true;
     }
@@ -84,16 +88,19 @@ public class BSConstraintCollection : IDisposable
 
         uint lookingID1 = body1.ID;
         uint lookingID2 = body2.ID;
-        ForEachConstraint(delegate(BSConstraint constrain)
+        lock (m_constraints)
         {
-            if ((constrain.Body1.ID == lookingID1 && constrain.Body2.ID == lookingID2)
-                || (constrain.Body1.ID == lookingID2 && constrain.Body2.ID == lookingID1))
+            foreach (BSConstraint constrain in m_constraints)
             {
-                foundConstraint = constrain;
-                found = true;
+                if ((constrain.Body1.ID == lookingID1 && constrain.Body2.ID == lookingID2)
+                    || (constrain.Body1.ID == lookingID2 && constrain.Body2.ID == lookingID1))
+                {
+                    foundConstraint = constrain;
+                    found = true;
+                    break;
+                }
             }
-            return found;
-        });
+        }
         returnConstraint = foundConstraint;
         return found;
     }
@@ -103,17 +110,16 @@ public class BSConstraintCollection : IDisposable
     // Return 'true' if a constraint was found and destroyed.
     public bool RemoveAndDestroyConstraint(BulletBody body1, BulletBody body2)
     {
-        // return BulletSimAPI.RemoveConstraint(m_world.ID, obj1.ID, obj2.ID);
-
         bool ret = false;
-        BSConstraint constrain;
-
-        if (this.TryGetConstraint(body1, body2, out constrain))
+        lock (m_constraints)
         {
-            m_world.scene.DetailLog("{0},BSConstraintCollection.RemoveAndDestroyConstraint,taint,body1={1},body2={2}", BSScene.DetailLogZero, body1.ID, body2.ID);
-            // remove the constraint from our collection
-            RemoveAndDestroyConstraint(constrain);
-            ret = true;
+            BSConstraint constrain;
+            if (this.TryGetConstraint(body1, body2, out constrain))
+            {
+                // remove the constraint from our collection
+                RemoveAndDestroyConstraint(constrain);
+                ret = true;
+            }
         }
 
         return ret;
@@ -122,8 +128,11 @@ public class BSConstraintCollection : IDisposable
     // The constraint MUST exist in the collection
     public bool RemoveAndDestroyConstraint(BSConstraint constrain)
     {
-        // remove the constraint from our collection
-        m_constraints.Remove(constrain);
+        lock (m_constraints)
+        {
+            // remove the constraint from our collection
+            m_constraints.Remove(constrain);
+        }
         // tell the engine that all its structures need to be freed
         constrain.Dispose();
         // we destroyed something
@@ -138,16 +147,15 @@ public class BSConstraintCollection : IDisposable
 
         List<BSConstraint> toRemove = new List<BSConstraint>();
         uint lookingID = body1.ID;
-        ForEachConstraint(delegate(BSConstraint constrain)
+        lock (m_constraints)
         {
-            if (constrain.Body1.ID == lookingID || constrain.Body2.ID == lookingID)
+            foreach (BSConstraint constrain in m_constraints)
             {
-                toRemove.Add(constrain);
+                if (constrain.Body1.ID == lookingID || constrain.Body2.ID == lookingID)
+                {
+                    toRemove.Add(constrain);
+                }
             }
-            return false;
-        });
-        lock (m_constraints)
-        {
             foreach (BSConstraint constrain in toRemove)
             {
                 m_constraints.Remove(constrain);
@@ -159,28 +167,16 @@ public class BSConstraintCollection : IDisposable
 
     public bool RecalculateAllConstraints()
     {
-        ForEachConstraint(delegate(BSConstraint constrain)
-        {
-            constrain.CalculateTransforms();
-            return false;
-        });
-        return true;
-    }
-
-    // Lock the constraint list and loop through it.
-    // The constraint action returns 'true' if it wants the loop aborted.
-    private void ForEachConstraint(ConstraintAction action)
-    {
+        bool ret = false;
         lock (m_constraints)
         {
             foreach (BSConstraint constrain in m_constraints)
             {
-                if (action(constrain))
-                    break;
+                constrain.CalculateTransforms();
+                ret = true;
             }
         }
+        return ret;
     }
-
-
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 4a71612..087b9bb 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -37,11 +37,12 @@ public class BSLinkset
     private static string LogHeader = "[BULLETSIM LINKSET]";
 
     private BSPrim m_linksetRoot;
-    public BSPrim Root { get { return m_linksetRoot; } }
+    public BSPrim LinksetRoot { get { return m_linksetRoot; } }
 
-    private BSScene m_scene;
-    public BSScene Scene { get { return m_scene; } }
+    private BSScene m_physicsScene;
+    public BSScene PhysicsScene { get { return m_physicsScene; } }
 
+    // The children under the root in this linkset
     private List<BSPrim> m_children;
 
     // We lock the diddling of linkset classes to prevent any badness.
@@ -73,7 +74,7 @@ public class BSLinkset
     public BSLinkset(BSScene scene, BSPrim parent)
     {
         // A simple linkset of one (no children)
-        m_scene = scene;
+        m_physicsScene = scene;
         m_linksetRoot = parent;
         m_children = new List<BSPrim>();
         m_mass = parent.MassRaw;
@@ -91,6 +92,9 @@ public class BSLinkset
         return this;
     }
 
+    // Remove a child from a linkset.
+    // Returns a new linkset for the child which is a linkset of one (just the
+    //    orphened child).
     public BSLinkset RemoveMeFromLinkset(BSPrim child)
     {
         lock (m_linksetActivityLock)
@@ -114,60 +118,9 @@ public class BSLinkset
         }
 
         // The child is down to a linkset of just itself
-        return new BSLinkset(Scene, child);
+        return new BSLinkset(PhysicsScene, child);
     }
 
-    /* DEPRECATED: this is really bad in that it trys to unlink other prims.
-    // An existing linkset had one of its members rebuilt or something.
-    // Go through the linkset and rebuild the pointers to the bodies of the linkset members.
-    public BSLinkset RefreshLinkset(BSPrim requestor)
-    {
-        BSLinkset ret = requestor.Linkset;
-
-        lock (m_linksetActivityLock)
-        {
-            // The body pointer is refetched in case anything has moved.
-            System.IntPtr aPtr = BulletSimAPI.GetBodyHandle2(m_scene.World.Ptr, m_linksetRoot.LocalID);
-            if (aPtr == System.IntPtr.Zero)
-            {
-                // That's odd. We can't find the root of the linkset.
-                // The linkset is somehow dead.  The requestor is now a member of a linkset of one.
-                DetailLog("{0},RefreshLinkset.RemoveRoot,child={1}", m_linksetRoot.LocalID, m_linksetRoot.LocalID);
-                ret = RemoveMeFromLinkset(m_linksetRoot);
-            }
-            else
-            {
-                // Reconstruct the pointer to the body of the linkset root.
-                DetailLog("{0},RefreshLinkset.RebuildRoot,rootID={1},ptr={2}", m_linksetRoot.LocalID, m_linksetRoot.LocalID, aPtr);
-                m_linksetRoot.Body = new BulletBody(m_linksetRoot.LocalID, aPtr);
-
-                List<BSPrim> toRemove = new List<BSPrim>();
-                foreach (BSPrim bsp in m_children)
-                {
-                    aPtr = BulletSimAPI.GetBodyHandle2(m_scene.World.Ptr, bsp.LocalID);
-                    if (aPtr == System.IntPtr.Zero)
-                    {
-                        toRemove.Add(bsp);
-                    }
-                    else
-                    {
-                        // Reconstruct the pointer to the body of the linkset root.
-                        DetailLog("{0},RefreshLinkset.RebuildChild,rootID={1},ptr={2}", bsp.LocalID, m_linksetRoot.LocalID, aPtr);
-                        bsp.Body = new BulletBody(bsp.LocalID, aPtr);
-                    }
-                }
-                foreach (BSPrim bsp in toRemove)
-                {
-                    RemoveChildFromOtherLinkset(bsp);
-                }
-            }
-        }
-
-        return ret;
-    }
-     */
-
-
     // Return 'true' if the passed object is the root object of this linkset
     public bool IsRoot(BSPrim requestor)
     {
@@ -183,12 +136,15 @@ public class BSLinkset
     public bool HasChild(BSPrim child)
     {
         bool ret = false;
-        foreach (BSPrim bp in m_children)
+        lock (m_linksetActivityLock)
         {
-            if (child.LocalID == bp.LocalID)
+            foreach (BSPrim bp in m_children)
             {
-                ret = true;
-                break;
+                if (child.LocalID == bp.LocalID)
+                {
+                    ret = true;
+                    break;
+                }
             }
         }
         return ret;
@@ -209,13 +165,16 @@ public class BSLinkset
         OMV.Vector3 com = m_linksetRoot.Position * m_linksetRoot.MassRaw;
         float totalMass = m_linksetRoot.MassRaw;
 
-        foreach (BSPrim bp in m_children)
+        lock (m_linksetActivityLock)
         {
-            com += bp.Position * bp.MassRaw;
-            totalMass += bp.MassRaw;
+            foreach (BSPrim bp in m_children)
+            {
+                com += bp.Position * bp.MassRaw;
+                totalMass += bp.MassRaw;
+            }
+            if (totalMass != 0f)
+                com /= totalMass;
         }
-        if (totalMass != 0f)
-            com /= totalMass;
 
         return com;
     }
@@ -224,29 +183,84 @@ public class BSLinkset
     {
         OMV.Vector3 com = m_linksetRoot.Position;
 
-        foreach (BSPrim bp in m_children)
+        lock (m_linksetActivityLock)
         {
-            com += bp.Position * bp.MassRaw;
+            foreach (BSPrim bp in m_children)
+            {
+                com += bp.Position * bp.MassRaw;
+            }
+            com /= (m_children.Count + 1);
         }
-        com /= (m_children.Count + 1);
 
         return com;
     }
 
+    // When physical properties are changed the linkset needs to recalculate
+    //   its internal properties.
+    public void Refresh(BSPrim requestor)
+    {
+        // If there are no children, there aren't any constraints to recompute
+        if (!HasAnyChildren)
+            return;
+
+        // Only the root does the recomputation
+        if (IsRoot(requestor))
+        {
+            PhysicsScene.TaintedObject("BSLinkSet.Refresh", delegate()
+            {
+                RecomputeLinksetConstraintVariables();
+            });
+        }
+    }
+
+    // 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 bool RecomputeLinksetConstraintVariables()
+    {
+        float linksetMass = LinksetMass;
+        lock (m_linksetActivityLock)
+        {
+            foreach (BSPrim child in m_children)
+            {
+                BSConstraint constrain;
+                if (m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.Body, child.Body, 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 can happen when children are being added to the linkset but
+                    //    their constraints have not been created yet.
+                    // Caused by the fact that m_children is built at run time but building constraints
+                    //    happens at taint time.
+                    // m_physicsScene.Logger.ErrorFormat("[BULLETSIM LINKSET] RecomputeLinksetConstraintVariables: constraint not found for root={0}, child={1}",
+                    //                                 m_linksetRoot.Body.ID, child.Body.ID);
+                }
+            }
+        }
+        return false;
+    }
+
     // I am the root of a linkset and a new child is being added
     // Called while LinkActivity is locked.
-    public void AddChildToLinkset(BSPrim child)
+    private void AddChildToLinkset(BSPrim child)
     {
         if (!HasChild(child))
         {
             m_children.Add(child);
 
-            BSPrim root = Root; // capture the root as of now
-            m_scene.TaintedObject("AddChildToLinkset", delegate()
+            BSPrim rootx = LinksetRoot; // capture the root as of now
+            BSPrim childx = child;
+            m_physicsScene.TaintedObject("AddChildToLinkset", delegate()
             {
-                DebugLog("{0}: AddChildToLinkset: adding child {1} to {2}", LogHeader, child.LocalID, m_linksetRoot.LocalID);
-                DetailLog("{0},AddChildToLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
-                PhysicallyLinkAChildToRoot(root, child);     // build the physical binding between me and the child
+                // DebugLog("{0}: AddChildToLinkset: adding child {1} to {2}", LogHeader, child.LocalID, m_linksetRoot.LocalID);
+                // DetailLog("{0},AddChildToLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
+                PhysicallyLinkAChildToRoot(rootx, childx);     // build the physical binding between me and the child
             });
         }
         return;
@@ -257,31 +271,34 @@ public class BSLinkset
     //    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).
-    public void RemoveChildFromOtherLinkset(BSPrim pchild)
+    private void RemoveChildFromOtherLinkset(BSPrim pchild)
     {
-        pchild.Linkset = new BSLinkset(m_scene, pchild);
+        pchild.Linkset = new BSLinkset(m_physicsScene, pchild);
         RemoveChildFromLinkset(pchild);
     }
 
     // I am the root of a linkset and one of my children is being removed.
     // Safe to call even if the child is not really in my linkset.
-    public void RemoveChildFromLinkset(BSPrim child)
+    private void RemoveChildFromLinkset(BSPrim child)
     {
         if (m_children.Remove(child))
         {
-            BSPrim root = Root; // capture the root as of now
-            m_scene.TaintedObject("RemoveChildFromLinkset", delegate()
+            BSPrim rootx = LinksetRoot; // capture the root as of now
+            BSPrim childx = child;
+            m_physicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
             {
-                DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
-                DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
+                // DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
+                // DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
 
-                PhysicallyUnlinkAChildFromRoot(root, child);
+                PhysicallyUnlinkAChildFromRoot(rootx, childx);
             });
+
+            RecomputeLinksetConstraintVariables();
         }
         else
         {
             // This will happen if we remove the root of the linkset first. Non-fatal occurance.
-            // m_scene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
+            // PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
         }
         return;
     }
@@ -293,37 +310,72 @@ public class BSLinkset
         // Zero motion for children so they don't interpolate
         childPrim.ZeroMotion();
 
+        // Relative position normalized to the root prim
+        // Essentually a vector pointing from center of rootPrim to center of childPrim
+        OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position;
+
+        // real world coordinate of midpoint between the two objects
+        OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
+
+        // create a constraint that allows no freedom of movement between the two objects
+        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+        // DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
+        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);
+        BS6DofConstraint constrain = new BS6DofConstraint(
+                        m_physicsScene.World, rootPrim.Body, childPrim.Body,
+                        midPoint,
+                        true,
+                        true
+                        );
+        /* NOTE: attempt to build constraint with full frame computation, etc.
+         *     Using the midpoint is easier since it lets the Bullet code use the transforms
+         *     of the objects.
+         * Code left here as an example.
+        // ==================================================================================
         // relative position normalized to the root prim
         OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
         OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
 
         // relative rotation of the child to the parent
         OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
+        OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
 
         // create a constraint that allows no freedom of movement between the two objects
         // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
         // DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
         DetailLog("{0},PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
         BS6DofConstraint constrain = new BS6DofConstraint(
-                        m_scene.World, rootPrim.Body, childPrim.Body,
-                        childRelativePosition,
-                        childRelativeRotation,
+                        PhysicsScene.World, rootPrim.Body, childPrim.Body,
                         OMV.Vector3.Zero,
-                        -childRelativeRotation
+                        OMV.Quaternion.Inverse(rootPrim.Orientation),
+                        OMV.Vector3.Zero,
+                        OMV.Quaternion.Inverse(childPrim.Orientation),
+                        // A point half way between the parent and child
+                        // childRelativePosition/2,
+                        // childRelativeRotation,
+                        // childRelativePosition/2,
+                        // inverseChildRelativeRotation,
+                        true,
+                        true
                         );
-        m_scene.Constraints.AddConstraint(constrain);
+        // ==================================================================================
+        */
+
+        m_physicsScene.Constraints.AddConstraint(constrain);
 
         // zero linear and angular limits makes the objects unable to move in relation to each other
         constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
         constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
 
         // tweek the constraint to increase stability
-        constrain.UseFrameOffset(m_scene.BoolNumeric(m_scene.Params.linkConstraintUseFrameOffset));
-        constrain.TranslationalLimitMotor(m_scene.BoolNumeric(m_scene.Params.linkConstraintEnableTransMotor),
-                        m_scene.Params.linkConstraintTransMotorMaxVel,
-                        m_scene.Params.linkConstraintTransMotorMaxForce);
-        constrain.SetCFMAndERP(m_scene.Params.linkConstraintCFM, m_scene.Params.linkConstraintERP);
+        constrain.UseFrameOffset(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintUseFrameOffset));
+        constrain.TranslationalLimitMotor(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintEnableTransMotor),
+                        PhysicsScene.Params.linkConstraintTransMotorMaxVel,
+                        PhysicsScene.Params.linkConstraintTransMotorMaxForce);
+        constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
 
+        RecomputeLinksetConstraintVariables();
     }
 
     // Remove linkage between myself and a particular child
@@ -334,7 +386,9 @@ public class BSLinkset
         //             LogHeader, rootPrim.LocalID, childPrim.LocalID);
         DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
 
-        m_scene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body, childPrim.Body);
+        // Find the constraint for this link and get rid of it from the overall collection and from my list
+        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body, childPrim.Body);
+
         // Make the child refresh its location
         BulletSimAPI.PushUpdate2(childPrim.Body.Ptr);
     }
@@ -346,20 +400,20 @@ public class BSLinkset
         // DebugLog("{0}: PhysicallyUnlinkAllChildren:", LogHeader);
         DetailLog("{0},PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
 
-        m_scene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body);
+        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body);
     }
 
     // Invoke the detailed logger and output something if it's enabled.
     private void DebugLog(string msg, params Object[] args)
     {
-        if (m_scene.ShouldDebugLog)
-            m_scene.Logger.DebugFormat(msg, args);
+        if (m_physicsScene.ShouldDebugLog)
+            m_physicsScene.Logger.DebugFormat(msg, args);
     }
 
     // Invoke the detailed logger and output something if it's enabled.
     private void DetailLog(string msg, params Object[] args)
     {
-        m_scene.PhysicsLogging.Write(msg, args);
+        m_physicsScene.PhysicsLogging.Write(msg, args);
     }
 
 }