BulletSim: add BSLinkInfo structure to remember link specific information

for each link in a linkset.
Extend BSLinksetConstraint to create and use BSLinkInfo with the default
static constraint.

3 files changed, 174 insertions, 86 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 7f94666..9613fe0 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -70,6 +70,15 @@ public abstract class BSLinkset
         return ret;
     }
 
+    public class BSLinkInfo
+    {
+        public BSPrimLinkable member;
+        public BSLinkInfo(BSPrimLinkable pMember)
+        {
+            member = pMember;
+        }
+    }
+
     public BSPrimLinkable LinksetRoot { get; protected set; }
 
     protected BSScene m_physicsScene { get; private set; }
@@ -78,7 +87,8 @@ public abstract class BSLinkset
     public int LinksetID { get; private set; }
 
     // The children under the root in this linkset.
-    protected HashSet<BSPrimLinkable> m_children;
+    // protected HashSet<BSPrimLinkable> m_children;
+    protected Dictionary<BSPrimLinkable, BSLinkInfo> m_children;
 
     // We lock the diddling of linkset classes to prevent any badness.
     // This locks the modification of the instances of this class. Changes
@@ -109,7 +119,7 @@ public abstract class BSLinkset
             m_nextLinksetID = 1;
         m_physicsScene = scene;
         LinksetRoot = parent;
-        m_children = new HashSet<BSPrimLinkable>();
+        m_children = new Dictionary<BSPrimLinkable, BSLinkInfo>();
         LinksetMass = parent.RawMass;
         Rebuilding = false;
 
@@ -170,17 +180,7 @@ public abstract class BSLinkset
         bool ret = false;
         lock (m_linksetActivityLock)
         {
-            ret = m_children.Contains(child);
-            /* Safer version but the above should work
-            foreach (BSPrimLinkable bp in m_children)
-            {
-                if (child.LocalID == bp.LocalID)
-                {
-                    ret = true;
-                    break;
-                }
-            }
-             */
+            ret = m_children.ContainsKey(child);
         }
         return ret;
     }
@@ -194,7 +194,24 @@ public abstract class BSLinkset
         lock (m_linksetActivityLock)
         {
             action(LinksetRoot);
-            foreach (BSPrimLinkable po in m_children)
+            foreach (BSPrimLinkable po in m_children.Keys)
+            {
+                if (action(po))
+                    break;
+            }
+        }
+        return ret;
+    }
+
+    // Perform an action on each member of the linkset including root prim.
+    // Depends on the action on whether this should be done at taint time.
+    public delegate bool ForEachLinkInfoAction(BSLinkInfo obj);
+    public virtual bool ForEachLinkInfo(ForEachLinkInfoAction action)
+    {
+        bool ret = false;
+        lock (m_linksetActivityLock)
+        {
+            foreach (BSLinkInfo po in m_children.Values)
             {
                 if (action(po))
                     break;
@@ -364,7 +381,7 @@ public abstract class BSLinkset
         {
             lock (m_linksetActivityLock)
             {
-                foreach (BSPrimLinkable bp in m_children)
+                foreach (BSPrimLinkable bp in m_children.Keys)
                 {
                     mass += bp.RawMass;
                 }
@@ -382,7 +399,7 @@ public abstract class BSLinkset
             com = LinksetRoot.Position * LinksetRoot.RawMass;
             float totalMass = LinksetRoot.RawMass;
 
-            foreach (BSPrimLinkable bp in m_children)
+            foreach (BSPrimLinkable bp in m_children.Keys)
             {
                 com += bp.Position * bp.RawMass;
                 totalMass += bp.RawMass;
@@ -401,7 +418,7 @@ public abstract class BSLinkset
         {
             com = LinksetRoot.Position;
 
-            foreach (BSPrimLinkable bp in m_children)
+            foreach (BSPrimLinkable bp in m_children.Keys)
             {
                 com += bp.Position;
             }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 6359046..53c3584 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -257,7 +257,7 @@ public sealed class BSLinksetCompound : BSLinkset
     {
         if (!HasChild(child))
         {
-            m_children.Add(child);
+            m_children.Add(child, new BSLinkInfo(child));
 
             DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
 
@@ -353,7 +353,7 @@ public sealed class BSLinksetCompound : BSLinkset
 
             // Add the shapes of all the components of the linkset
             int memberIndex = 1;
-            ForEachMember(delegate(BSPrimLinkable cPrim)
+            ForEachMember((cPrim) =>
             {
                 if (IsRoot(cPrim))
                 {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index f17d698..d98bf77 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -36,6 +36,75 @@ public sealed class BSLinksetConstraints : BSLinkset
 {
     // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
 
+    public class BSLinkInfoConstraint : BSLinkInfo
+    {
+        public ConstraintType constraintType;
+        public BSConstraint constraint;
+        public OMV.Vector3 linearLimitLow;
+        public OMV.Vector3 linearLimitHigh;
+        public OMV.Vector3 angularLimitLow;
+        public OMV.Vector3 angularLimitHigh;
+        public bool useFrameOffset;
+        public bool enableTransMotor;
+        public float transMotorMaxVel;
+        public float transMotorMaxForce;
+        public float cfm;
+        public float erp;
+        public float solverIterations;
+
+        public BSLinkInfoConstraint(BSPrimLinkable pMember)
+            : base(pMember)
+        {
+            constraint = null;
+            ResetToFixedConstraint();
+        }
+
+        // Set all the parameters for this constraint to a fixed, non-movable constraint.
+        public void ResetToFixedConstraint()
+        {
+            constraintType = ConstraintType.D6_CONSTRAINT_TYPE;
+            linearLimitLow = OMV.Vector3.Zero;
+            linearLimitHigh = OMV.Vector3.Zero;
+            angularLimitLow = OMV.Vector3.Zero;
+            angularLimitHigh = OMV.Vector3.Zero;
+            useFrameOffset = BSParam.LinkConstraintUseFrameOffset;
+            enableTransMotor = BSParam.LinkConstraintEnableTransMotor;
+            transMotorMaxVel = BSParam.LinkConstraintTransMotorMaxVel;
+            transMotorMaxForce = BSParam.LinkConstraintTransMotorMaxForce;
+            cfm = BSParam.LinkConstraintCFM;
+            erp = BSParam.LinkConstraintERP;
+            solverIterations = BSParam.LinkConstraintSolverIterations;
+        }
+
+        // Given a constraint, apply the current constraint parameters to same.
+        public void SetConstraintParameters(BSConstraint constrain)
+        {
+            switch (constraintType)
+            {
+                case ConstraintType.D6_CONSTRAINT_TYPE:
+                    BSConstraint6Dof constrain6dof = constrain as BSConstraint6Dof;
+                    if (constrain6dof != null)
+                    {
+                        // zero linear and angular limits makes the objects unable to move in relation to each other
+                        constrain6dof.SetLinearLimits(linearLimitLow, linearLimitHigh);
+                        constrain6dof.SetAngularLimits(angularLimitLow, angularLimitHigh);
+
+                        // tweek the constraint to increase stability
+                        constrain6dof.UseFrameOffset(useFrameOffset);
+                        constrain6dof.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce);
+                        constrain6dof.SetCFMAndERP(cfm, erp);
+                        if (solverIterations != 0f)
+                        {
+                            constrain6dof.SetSolverIterations(solverIterations);
+                        }
+                    }
+                    break;
+                default:
+                    break;
+            }
+        }
+    }
+
     public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent)
     {
     }
@@ -142,7 +211,7 @@ public sealed class BSLinksetConstraints : BSLinkset
     {
         if (!HasChild(child))
         {
-            m_children.Add(child);
+            m_children.Add(child, new BSLinkInfoConstraint(child));
 
             DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
 
@@ -190,73 +259,74 @@ public sealed class BSLinksetConstraints : BSLinkset
     }
 
     // Create a static constraint between the two passed objects
-    private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
+    private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li)
     {
+        BSLinkInfoConstraint liConstraint = li as BSLinkInfoConstraint;
+        if (liConstraint == null)
+            return null;
+
         // Zero motion for children so they don't interpolate
-        childPrim.ZeroMotion(true);
-
-        // 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);
-
-        DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
-                                        rootPrim.LocalID,
-                                        rootPrim.LocalID, rootPrim.PhysBody.AddrString,
-                                        childPrim.LocalID, childPrim.PhysBody.AddrString,
-                                        rootPrim.Position, childPrim.Position, midPoint);
-
-        // create a constraint that allows no freedom of movement between the two objects
-        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
-
-        BSConstraint6Dof constrain = new BSConstraint6Dof(
-                            m_physicsScene.World, rootPrim.PhysBody, childPrim.PhysBody, midPoint, true, true );
-                            // PhysicsScene.World, childPrim.BSBody, rootPrim.BSBody, midPoint, true, true );
-
-        /* NOTE: below is an attempt to build constraint with full frame computation, etc.
-         *     Using the midpoint is easier since it lets the Bullet code manipulate the transforms
-         *     of the objects.
-         * Code left for future programmers.
-        // ==================================================================================
-        // 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);
-
-        DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
-        BS6DofConstraint constrain = new BS6DofConstraint(
-                        PhysicsScene.World, rootPrim.Body, childPrim.Body,
-                        OMV.Vector3.Zero,
-                        OMV.Quaternion.Inverse(rootPrim.Orientation),
-                        OMV.Vector3.Zero,
-                        OMV.Quaternion.Inverse(childPrim.Orientation),
-                        true,
-                        true
-                        );
-        // ==================================================================================
-        */
+        li.member.ZeroMotion(true);
 
-        m_physicsScene.Constraints.AddConstraint(constrain);
+        BSConstraint constrain = null;
 
-        // 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(BSParam.LinkConstraintUseFrameOffset);
-        constrain.TranslationalLimitMotor(BSParam.LinkConstraintEnableTransMotor,
-                        BSParam.LinkConstraintTransMotorMaxVel,
-                        BSParam.LinkConstraintTransMotorMaxForce);
-        constrain.SetCFMAndERP(BSParam.LinkConstraintCFM, BSParam.LinkConstraintERP);
-        if (BSParam.LinkConstraintSolverIterations != 0f)
+        switch (liConstraint.constraintType)
         {
-            constrain.SetSolverIterations(BSParam.LinkConstraintSolverIterations);
+            case ConstraintType.D6_CONSTRAINT_TYPE:
+                // Relative position normalized to the root prim
+                // Essentually a vector pointing from center of rootPrim to center of li.member
+                OMV.Vector3 childRelativePosition = liConstraint.member.Position - rootPrim.Position;
+
+                // real world coordinate of midpoint between the two objects
+                OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
+
+                DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
+                                                rootPrim.LocalID,
+                                                rootPrim.LocalID, rootPrim.PhysBody.AddrString,
+                                                liConstraint.member.LocalID, liConstraint.member.PhysBody.AddrString,
+                                                rootPrim.Position, liConstraint.member.Position, midPoint);
+
+                // create a constraint that allows no freedom of movement between the two objects
+                // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+
+                constrain = new BSConstraint6Dof(
+                                    m_physicsScene.World, rootPrim.PhysBody, liConstraint.member.PhysBody, midPoint, true, true );
+
+                /* NOTE: below is an attempt to build constraint with full frame computation, etc.
+                 *     Using the midpoint is easier since it lets the Bullet code manipulate the transforms
+                 *     of the objects.
+                 * Code left for future programmers.
+                // ==================================================================================
+                // relative position normalized to the root prim
+                OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
+                OMV.Vector3 childRelativePosition = (liConstraint.member.Position - rootPrim.Position) * invThisOrientation;
+
+                // relative rotation of the child to the parent
+                OMV.Quaternion childRelativeRotation = invThisOrientation * liConstraint.member.Orientation;
+                OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
+
+                DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, liConstraint.member.LocalID);
+                constrain = new BS6DofConstraint(
+                                PhysicsScene.World, rootPrim.Body, liConstraint.member.Body,
+                                OMV.Vector3.Zero,
+                                OMV.Quaternion.Inverse(rootPrim.Orientation),
+                                OMV.Vector3.Zero,
+                                OMV.Quaternion.Inverse(liConstraint.member.Orientation),
+                                true,
+                                true
+                                );
+                // ==================================================================================
+                */
+
+                break;
+            default:
+                break;
         }
+
+        liConstraint.SetConstraintParameters(constrain);
+
+        m_physicsScene.Constraints.AddConstraint(constrain);
+
         return constrain;
     }
 
@@ -317,23 +387,24 @@ public sealed class BSLinksetConstraints : BSLinkset
                 return; // Note the 'finally' clause at the botton which will get executed.
             }
 
-            foreach (BSPrimLinkable child in m_children)
+            ForEachLinkInfo((li) =>
             {
                 // A child in the linkset physically shows the mass of the whole linkset.
                 // This allows Bullet to apply enough force on the child to move the whole linkset.
                 // (Also do the mass stuff before recomputing the constraint so mass is not zero.)
-                child.UpdatePhysicalMassProperties(linksetMass, true);
+                li.member.UpdatePhysicalMassProperties(linksetMass, true);
 
                 BSConstraint constrain;
-                if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
+                if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, li.member.PhysBody, out constrain))
                 {
                     // If constraint doesn't exist yet, create it.
-                    constrain = BuildConstraint(LinksetRoot, child);
+                    constrain = BuildConstraint(LinksetRoot, li);
                 }
                 constrain.RecomputeConstraintVariables(linksetMass);
 
                 // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint);    // DEBUG DEBUG
-            }
+                return false;   // 'false' says to keep processing other members
+            });
         }
         finally
         {