1 files changed, 95 insertions, 55 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 087b9bb..7e784eb 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -36,14 +36,17 @@ public class BSLinkset
 {
     private static string LogHeader = "[BULLETSIM LINKSET]";
 
-    private BSPrim m_linksetRoot;
-    public BSPrim LinksetRoot { get { return m_linksetRoot; } }
+    private BSPhysObject m_linksetRoot;
+    public BSPhysObject LinksetRoot { get { return m_linksetRoot; } }
 
     private BSScene m_physicsScene;
     public BSScene PhysicsScene { get { return m_physicsScene; } }
 
+    static int m_nextLinksetID = 1;
+    public int LinksetID { get; private set; }
+
     // The children under the root in this linkset
-    private List<BSPrim> m_children;
+    private List<BSPhysObject> m_children;
 
     // We lock the diddling of linkset classes to prevent any badness.
     // This locks the modification of the instances of this class. Changes
@@ -71,19 +74,23 @@ public class BSLinkset
         get { return ComputeLinksetGeometricCenter(); }
     }
 
-    public BSLinkset(BSScene scene, BSPrim parent)
+    public BSLinkset(BSScene scene, BSPhysObject parent)
     {
         // A simple linkset of one (no children)
+        LinksetID = m_nextLinksetID++;
+        // We create LOTS of linksets.
+        if (m_nextLinksetID < 0) 
+            m_nextLinksetID = 1;
         m_physicsScene = scene;
         m_linksetRoot = parent;
-        m_children = new List<BSPrim>();
+        m_children = new List<BSPhysObject>();
         m_mass = parent.MassRaw;
     }
 
     // Link to a linkset where the child knows the parent.
     // Parent changing should not happen so do some sanity checking.
     // We return the parent's linkset so the child can track its membership.
-    public BSLinkset AddMeToLinkset(BSPrim child)
+    public BSLinkset AddMeToLinkset(BSPhysObject child)
     {
         lock (m_linksetActivityLock)
         {
@@ -95,7 +102,7 @@ public class BSLinkset
     // 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)
+    public BSLinkset RemoveMeFromLinkset(BSPhysObject child)
     {
         lock (m_linksetActivityLock)
         {
@@ -122,7 +129,7 @@ public class BSLinkset
     }
 
     // Return 'true' if the passed object is the root object of this linkset
-    public bool IsRoot(BSPrim requestor)
+    public bool IsRoot(BSPhysObject requestor)
     {
         return (requestor.LocalID == m_linksetRoot.LocalID);
     }
@@ -133,12 +140,12 @@ public class BSLinkset
     public bool HasAnyChildren { get { return (m_children.Count > 0); } }
 
     // Return 'true' if this child is in this linkset
-    public bool HasChild(BSPrim child)
+    public bool HasChild(BSPhysObject child)
     {
         bool ret = false;
         lock (m_linksetActivityLock)
         {
-            foreach (BSPrim bp in m_children)
+            foreach (BSPhysObject bp in m_children)
             {
                 if (child.LocalID == bp.LocalID)
                 {
@@ -153,7 +160,7 @@ public class BSLinkset
     private float ComputeLinksetMass()
     {
         float mass = m_linksetRoot.MassRaw;
-        foreach (BSPrim bp in m_children)
+        foreach (BSPhysObject bp in m_children)
         {
             mass += bp.MassRaw;
         }
@@ -167,7 +174,7 @@ public class BSLinkset
 
         lock (m_linksetActivityLock)
         {
-            foreach (BSPrim bp in m_children)
+            foreach (BSPhysObject bp in m_children)
             {
                 com += bp.Position * bp.MassRaw;
                 totalMass += bp.MassRaw;
@@ -185,7 +192,7 @@ public class BSLinkset
 
         lock (m_linksetActivityLock)
         {
-            foreach (BSPrim bp in m_children)
+            foreach (BSPhysObject bp in m_children)
             {
                 com += bp.Position * bp.MassRaw;
             }
@@ -195,11 +202,33 @@ public class BSLinkset
         return com;
     }
 
+    // The object is going dynamic (physical). Do any setup necessary
+    //     for a dynamic linkset.
+    // Only the state of the passed object can be modified. The rest of the linkset
+    //     has not yet been fully constructed.
+    // Return 'true' if any properties updated on the passed object.
+    // Called at taint-time!
+    public bool MakeDynamic(BSPhysObject child)
+    {
+        bool ret = false;
+        return ret;
+    }
+
+    // The object is going static (non-physical). Do any setup necessary
+    //     for a static linkset.
+    // Return 'true' if any properties updated on the passed object.
+    // Called at taint-time!
+    public bool MakeStatic(BSPhysObject child)
+    {
+        // What is done for each object in BSPrim is what we want.
+        return false;
+    }
+
     // When physical properties are changed the linkset needs to recalculate
     //   its internal properties.
-    public void Refresh(BSPrim requestor)
+    public void Refresh(BSPhysObject requestor)
     {
-        // If there are no children, there aren't any constraints to recompute
+        // If there are no children, there can't be any constraints to recompute
         if (!HasAnyChildren)
             return;
 
@@ -218,15 +247,16 @@ public class BSLinkset
     //    from a linkset to make sure the constraints know about the new mass and
     //    geometry.
     // Must only be called at taint time!!
-    private bool RecomputeLinksetConstraintVariables()
+    private void RecomputeLinksetConstraintVariables()
     {
         float linksetMass = LinksetMass;
         lock (m_linksetActivityLock)
         {
-            foreach (BSPrim child in m_children)
+            bool somethingMissing = false;
+            foreach (BSPhysObject child in m_children)
             {
                 BSConstraint constrain;
-                if (m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.Body, child.Body, out constrain))
+                if (m_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);
@@ -234,32 +264,51 @@ public class BSLinkset
                 }
                 else
                 {
-                    // Non-fatal error that can happen when children are being added to the linkset but
+                    // Non-fatal error that happens when children are being added to the linkset but
                     //    their constraints have not been created yet.
                     // Caused by the fact that m_children is built at run time but building constraints
                     //    happens at taint time.
-                    // m_physicsScene.Logger.ErrorFormat("[BULLETSIM LINKSET] RecomputeLinksetConstraintVariables: constraint not found for root={0}, child={1}",
-                    //                                 m_linksetRoot.Body.ID, child.Body.ID);
+                    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 false;
+        return;
     }
 
     // I am the root of a linkset and a new child is being added
     // Called while LinkActivity is locked.
-    private void AddChildToLinkset(BSPrim child)
+    private void AddChildToLinkset(BSPhysObject child)
     {
         if (!HasChild(child))
         {
             m_children.Add(child);
 
-            BSPrim rootx = LinksetRoot; // capture the root as of now
-            BSPrim childx = child;
+            BSPhysObject rootx = LinksetRoot; // capture the root as of now
+            BSPhysObject 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);
+                DetailLog("{0},AddChildToLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
                 PhysicallyLinkAChildToRoot(rootx, childx);     // build the physical binding between me and the child
             });
         }
@@ -271,7 +320,7 @@ 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).
-    private void RemoveChildFromOtherLinkset(BSPrim pchild)
+    private void RemoveChildFromOtherLinkset(BSPhysObject pchild)
     {
         pchild.Linkset = new BSLinkset(m_physicsScene, pchild);
         RemoveChildFromLinkset(pchild);
@@ -279,21 +328,20 @@ public class BSLinkset
 
     // 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.
-    private void RemoveChildFromLinkset(BSPrim child)
+    private void RemoveChildFromLinkset(BSPhysObject child)
     {
         if (m_children.Remove(child))
         {
-            BSPrim rootx = LinksetRoot; // capture the root as of now
-            BSPrim childx = child;
+            BSPhysObject rootx = LinksetRoot; // capture the root as of now
+            BSPhysObject 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);
+                DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
 
                 PhysicallyUnlinkAChildFromRoot(rootx, childx);
+                RecomputeLinksetConstraintVariables();
             });
 
-            RecomputeLinksetConstraintVariables();
         }
         else
         {
@@ -305,7 +353,7 @@ public class BSLinkset
 
     // Create a constraint between me (root of linkset) and the passed prim (the child).
     // Called at taint time!
-    private void PhysicallyLinkAChildToRoot(BSPrim rootPrim, BSPrim childPrim)
+    private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
     {
         // Zero motion for children so they don't interpolate
         childPrim.ZeroMotion();
@@ -319,19 +367,18 @@ 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
-        // 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,
+                        m_physicsScene.World, rootPrim.BSBody, childPrim.BSBody,
                         midPoint,
                         true,
                         true
                         );
-        /* NOTE: attempt to build constraint with full frame computation, etc.
+        /* 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
          *     of the objects.
-         * Code left here as an example.
+         * Code left as a warning to future programmers.
         // ==================================================================================
         // relative position normalized to the root prim
         OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
@@ -343,7 +390,6 @@ 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
-        // 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(
                         PhysicsScene.World, rootPrim.Body, childPrim.Body,
@@ -374,40 +420,34 @@ public class BSLinkset
                         PhysicsScene.Params.linkConstraintTransMotorMaxVel,
                         PhysicsScene.Params.linkConstraintTransMotorMaxForce);
         constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
+        if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
+        {
+            constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
+        }
 
         RecomputeLinksetConstraintVariables();
     }
 
     // Remove linkage between myself and a particular child
     // Called at taint time!
-    private void PhysicallyUnlinkAChildFromRoot(BSPrim rootPrim, BSPrim childPrim)
+    private void PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
     {
-        // DebugLog("{0}: PhysicallyUnlinkAChildFromRoot: RemoveConstraint between root prim {1} and child prim {2}", 
-        //             LogHeader, rootPrim.LocalID, childPrim.LocalID);
         DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
 
         // 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);
+        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody);
 
         // Make the child refresh its location
-        BulletSimAPI.PushUpdate2(childPrim.Body.Ptr);
+        BulletSimAPI.PushUpdate2(childPrim.BSBody.Ptr);
     }
 
     // Remove linkage between myself and any possible children I might have
     // Called at taint time!
-    private void PhysicallyUnlinkAllChildrenFromRoot(BSPrim rootPrim)
+    private void PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
     {
-        // DebugLog("{0}: PhysicallyUnlinkAllChildren:", LogHeader);
         DetailLog("{0},PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
 
-        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_physicsScene.ShouldDebugLog)
-            m_physicsScene.Logger.DebugFormat(msg, args);
+        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody);
     }
 
     // Invoke the detailed logger and output something if it's enabled.