BulletSim: implementation of setting spring specific physical parameters. Add setting of linkset type to physChangeLinkParams. Lots of detail logging for setting of linkset constraint parameters.

4 files changed, 81 insertions, 36 deletions

diff --git a/OpenSim/Region/OptionalModules/Scripting/ExtendedPhysics/ExtendedPhysics.cs b/OpenSim/Region/OptionalModules/Scripting/ExtendedPhysics/ExtendedPhysics.cs
index 10e13b9..2f88e2b 100755
--- a/OpenSim/Region/OptionalModules/Scripting/ExtendedPhysics/ExtendedPhysics.cs
+++ b/OpenSim/Region/OptionalModules/Scripting/ExtendedPhysics/ExtendedPhysics.cs
@@ -367,6 +367,7 @@ public class ExtendedPhysics : INonSharedRegionModule
     // Code for specifying params.
     // The choice if 14400 is arbitrary and only serves to catch parameter code misuse.
     public const int PHYS_PARAM_MIN                    = 14401;
+
     [ScriptConstant]
     public const int PHYS_PARAM_FRAMEINA_LOC           = 14401;
     [ScriptConstant]
@@ -401,7 +402,10 @@ public class ExtendedPhysics : INonSharedRegionModule
     public const int PHYS_PARAM_SPRING_DAMPING         = 14416;
     [ScriptConstant]
     public const int PHYS_PARAM_SPRING_STIFFNESS       = 14417;
-    public const int PHYS_PARAM_MAX                    = 14417;
+    [ScriptConstant]
+    public const int PHYS_PARAM_LINK_TYPE              = 14418;
+
+    public const int PHYS_PARAM_MAX                    = 14418;
 
     // physChangeLinkParams(integer linkNum, [ PHYS_PARAM_*, value, PHYS_PARAM_*, value, ...])
     [ScriptInvocation]
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
index 5008ff7..7fcb75c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
@@ -59,9 +59,11 @@ public class BSConstraint6Dof : BSConstraint
                                 frame2, frame2rot,
                                 useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies);
         m_enabled = true;
-        world.physicsScene.DetailLog("{0},BS6DofConstraint,createFrame,wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
-                            BSScene.DetailLogZero, world.worldID,
+        PhysicsScene.DetailLog("{0},BS6DofConstraint,create,wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
+                            m_body1.ID, world.worldID,
                             obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
+        PhysicsScene.DetailLog("{0},BS6DofConstraint,create,  f1Loc={1},f1Rot={2},f2Loc={3},f2Rot={4},usefA={5},disCol={6}",
+                            m_body1.ID, frame1, frame1rot, frame2, frame2rot, useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies);
     }
 
     // 6 Dof constraint based on a midpoint between the two constrained bodies
@@ -86,9 +88,11 @@ public class BSConstraint6Dof : BSConstraint
             m_constraint = PhysicsScene.PE.Create6DofConstraintToPoint(m_world, m_body1, m_body2,
                                     joinPoint,
                                     useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies);
+
             PhysicsScene.DetailLog("{0},BS6DofConstraint,createMidPoint,wID={1}, csrt={2}, rID={3}, rBody={4}, cID={5}, cBody={6}",
-                                BSScene.DetailLogZero, world.worldID, m_constraint.AddrString,
+                                m_body1.ID, world.worldID, m_constraint.AddrString,
                                 obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
+
             if (!m_constraint.HasPhysicalConstraint)
             {
                 world.physicsScene.Logger.ErrorFormat("{0} Failed creation of 6Dof constraint. rootID={1}, childID={2}",
@@ -113,8 +117,10 @@ public class BSConstraint6Dof : BSConstraint
                                 frameInBloc, frameInBrot,
                                 useLinearReferenceFrameB, disableCollisionsBetweenLinkedBodies);
         m_enabled = true;
-        world.physicsScene.DetailLog("{0},BS6DofConstraint,createFixed,wID={1},rID={2},rBody={3}",
-                            BSScene.DetailLogZero, world.worldID, obj1.ID, obj1.AddrString);
+        PhysicsScene.DetailLog("{0},BS6DofConstraint,createFixed,wID={1},rID={2},rBody={3}",
+                                    m_body1.ID, world.worldID, obj1.ID, obj1.AddrString);
+        PhysicsScene.DetailLog("{0},BS6DofConstraint,createFixed,  fBLoc={1},fBRot={2},usefA={3},disCol={4}",
+                            m_body1.ID, frameInBloc, frameInBrot, useLinearReferenceFrameB, disableCollisionsBetweenLinkedBodies);
     }
 
     public bool SetFrames(Vector3 frameA, Quaternion frameArot, Vector3 frameB, Quaternion frameBrot)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintSpring.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintSpring.cs
index 01d67d4..410584d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintSpring.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintSpring.cs
@@ -47,37 +47,42 @@ public sealed class BSConstraintSpring : BSConstraint6Dof
                                 useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies);
         m_enabled = true;
 
-        world.physicsScene.DetailLog("{0},BSConstraintSpring,createFrame,wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
-                            BSScene.DetailLogZero, world.worldID,
-                            obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
+        PhysicsScene.DetailLog("{0},BSConstraintSpring,create,wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
+                            obj1.ID, world.worldID, obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
+        PhysicsScene.DetailLog("{0},BSConstraintSpring,create,  f1Loc={1},f1Rot={2},f2Loc={3},f2Rot={4},usefA={5},disCol={6}",
+                    m_body1.ID, frame1Loc, frame1Rot, frame2Loc, frame2Rot, useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies);
     }
 
-    public bool SetEnable(int index, bool axisEnable)
+    public bool SetEnable(int pIndex, bool pAxisEnable)
     {
-        bool ret = false;
-
-        return ret;
+        PhysicsScene.DetailLog("{0},BSConstraintSpring.SetEnable,obj1ID={1},obj2ID={2},indx={3},enable={4}",
+                                        m_body1.ID, m_body1.ID, m_body2.ID, pIndex, pAxisEnable);
+        PhysicsScene.PE.SpringEnable(m_constraint, pIndex, BSParam.NumericBool(pAxisEnable));
+        return true;
     }
 
-    public bool SetStiffness(int index, float stiffness)
+    public bool SetStiffness(int pIndex, float pStiffness)
     {
-        bool ret = false;
-
-        return ret;
+        PhysicsScene.DetailLog("{0},BSConstraintSpring.SetStiffness,obj1ID={1},obj2ID={2},indx={3},enable={4}",
+                                        m_body1.ID, m_body1.ID, m_body2.ID, pIndex, pStiffness);
+        PhysicsScene.PE.SpringSetStiffness(m_constraint, pIndex, pStiffness);
+        return true;
     }
 
-    public bool SetDamping(int index, float damping)
+    public bool SetDamping(int pIndex, float pDamping)
     {
-        bool ret = false;
-
-        return ret;
+        PhysicsScene.DetailLog("{0},BSConstraintSpring.SetDamping,obj1ID={1},obj2ID={2},indx={3},enable={4}",
+                                        m_body1.ID, m_body1.ID, m_body2.ID, pIndex, pDamping);
+        PhysicsScene.PE.SpringSetDamping(m_constraint, pIndex, pDamping);
+        return true;
     }
 
-    public bool SetEquilibriumPoint(int index, float eqPoint)
+    public bool SetEquilibriumPoint(int pIndex, float pEqPoint)
     {
-        bool ret = false;
-
-        return ret;
+        PhysicsScene.DetailLog("{0},BSConstraintSpring.SetEquilibriumPoint,obj1ID={1},obj2ID={2},indx={3},enable={4}",
+                                        m_body1.ID, m_body1.ID, m_body2.ID, pIndex, pEqPoint);
+        PhysicsScene.PE.SpringSetEquilibriumPoint(m_constraint, pIndex, pEqPoint);
+        return true;
     }
 
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index a9ae89d..be97c29 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -67,6 +67,7 @@ public sealed class BSLinksetConstraints : BSLinkset
         {
             constraint = null;
             ResetLink();
+            member.PhysScene.DetailLog("{0},BSLinkInfoConstraint.creation", member.LocalID);
         }
 
         // Set all the parameters for this constraint to a fixed, non-movable constraint.
@@ -91,11 +92,13 @@ public sealed class BSLinksetConstraints : BSLinkset
             frameInBrot = OMV.Quaternion.Identity;
             springDamping = -1f;
             springStiffness = -1f;
+            member.PhysScene.DetailLog("{0},BSLinkInfoConstraint.ResetLink", member.LocalID);
         }
 
         // Given a constraint, apply the current constraint parameters to same.
         public override void SetLinkParameters(BSConstraint constrain)
         {
+            member.PhysScene.DetailLog("{0},BSLinkInfoConstraint.SetLinkParameters,type={1}", member.LocalID, constraintType);
             switch (constraintType)
             {
                 case ConstraintType.FIXED_CONSTRAINT_TYPE:
@@ -139,7 +142,7 @@ public sealed class BSLinksetConstraints : BSLinkset
                             if (springDamping != -1)
                                 constrainSpring.SetDamping(ii, springDamping);
                             if (springStiffness != -1)
-                            constrainSpring.SetStiffness(ii, springStiffness);
+                                constrainSpring.SetStiffness(ii, springStiffness);
                         }
                     }
                     break;
@@ -155,7 +158,6 @@ public sealed class BSLinksetConstraints : BSLinkset
         public override bool ShouldUpdateChildProperties()
         {
             bool ret = true;
-
             if (constraintType == ConstraintType.FIXED_CONSTRAINT_TYPE)
                 ret = false;
 
@@ -193,10 +195,16 @@ public sealed class BSLinksetConstraints : BSLinkset
         {
             // Queue to happen after all the other taint processing
             m_physicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
+            {
+                if (HasAnyChildren)
                 {
-                    if (HasAnyChildren)
-                        RecomputeLinksetConstraints();
-                });
+                    // Constraints that have not been changed are not rebuild but make sure
+                    //     the constraint of the requestor is rebuilt.
+                    PhysicallyUnlinkAChildFromRoot(LinksetRoot, requestor);
+                    // Rebuild the linkset and all its constraints.
+                    RecomputeLinksetConstraints();
+                }
+            });
         }
     }
 
@@ -415,13 +423,22 @@ public sealed class BSLinksetConstraints : BSLinkset
                             rootPrim.LocalID, rootPrim.PhysBody.AddrString,
                             childPrim.LocalID, childPrim.PhysBody.AddrString);
 
-        // Find the constraint for this link and get rid of it from the overall collection and from my list
-        if (m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody, childPrim.PhysBody))
+        // If asked to unlink root from root, just remove all the constraints
+        if (rootPrim == childPrim || childPrim == LinksetRoot)
         {
-            // Make the child refresh its location
-            m_physicsScene.PE.PushUpdate(childPrim.PhysBody);
+            PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
             ret = true;
         }
+        else
+        {
+            // Find the constraint for this link and get rid of it from the overall collection and from my list
+            if (m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody, childPrim.PhysBody))
+            {
+                // Make the child refresh its location
+                m_physicsScene.PE.PushUpdate(childPrim.PhysBody);
+                ret = true;
+            }
+        }
 
         return ret;
     }
@@ -521,8 +538,6 @@ public sealed class BSLinksetConstraints : BSLinkset
                                     BSLinkInfoConstraint linkInfoC = linkInfo as BSLinkInfoConstraint;
                                     if (linkInfoC != null)
                                     {
-                                        // Setting to fixed is easy. The reset state is the fixed link configuration.
-                                        linkInfoC.ResetLink();
                                         linkInfoC.constraintType = (ConstraintType)requestedType;
                                         ret = (object)true;
                                         DetailLog("{0},BSLinksetConstraint.ChangeLinkType,link={1},type={2}",
@@ -589,6 +604,7 @@ public sealed class BSLinksetConstraints : BSLinkset
                             BSLinkInfoConstraint linkInfo = baseLinkInfo as BSLinkInfoConstraint;
                             if (linkInfo != null)
                             {
+                                int valueInt;
                                 float valueFloat;
                                 bool valueBool;
                                 OMV.Vector3 valueVector;
@@ -602,6 +618,20 @@ public sealed class BSLinksetConstraints : BSLinkset
                                                         linkInfo.member.LocalID, thisOp, pParams[opIndex+1]);
                                     switch (thisOp)
                                     {
+                                        case ExtendedPhysics.PHYS_PARAM_LINK_TYPE:
+                                            valueInt = (int)pParams[opIndex + 1];
+                                            ConstraintType valueType = (ConstraintType)valueInt;
+                                            if (valueType == ConstraintType.FIXED_CONSTRAINT_TYPE
+                                                || valueType == ConstraintType.D6_CONSTRAINT_TYPE
+                                                || valueType == ConstraintType.D6_SPRING_CONSTRAINT_TYPE
+                                                || valueType == ConstraintType.HINGE_CONSTRAINT_TYPE
+                                                || valueType == ConstraintType.CONETWIST_CONSTRAINT_TYPE
+                                                || valueType == ConstraintType.SLIDER_CONSTRAINT_TYPE)
+                                            {
+                                                linkInfo.constraintType = valueType;
+                                            }
+                                            opIndex += 2;
+                                            break;
                                         case ExtendedPhysics.PHYS_PARAM_FRAMEINA_LOC:
                                             valueVector = (OMV.Vector3)pParams[opIndex + 1];
                                             linkInfo.frameInAloc = valueVector;