BulletSim: add LSL function and plumbing for setting a spring

equilibrium point in the physics engine constraint.

3 files changed, 37 insertions, 2 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index 42b5c49..b47e9a8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -64,7 +64,7 @@ public abstract class BSConstraint : IDisposable
             {
                 bool success = PhysicsScene.PE.DestroyConstraint(m_world, m_constraint);
                 m_world.physicsScene.DetailLog("{0},BSConstraint.Dispose,taint,id1={1},body1={2},id2={3},body2={4},success={5}",
-                                    BSScene.DetailLogZero,
+                                    m_body1.ID,
                                     m_body1.ID, m_body1.AddrString,
                                     m_body2.ID, m_body2.AddrString,
                                     success);
@@ -77,7 +77,10 @@ public abstract class BSConstraint : IDisposable
     {
         bool ret = false;
         if (m_enabled)
+        {
+            m_world.physicsScene.DetailLog("{0},BSConstraint.SetLinearLimits,taint,low={1},high={2}", m_body1.ID, low, high);
             ret = PhysicsScene.PE.SetLinearLimits(m_constraint, low, high);
+        }
         return ret;
     }
 
@@ -86,6 +89,7 @@ public abstract class BSConstraint : IDisposable
         bool ret = false;
         if (m_enabled)
         {
+            m_world.physicsScene.DetailLog("{0},BSConstraint.SetAngularLimits,taint,low={1},high={2}", m_body1.ID, low, high);
             ret = PhysicsScene.PE.SetAngularLimits(m_constraint, low, high);
         }
         return ret;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintSpring.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintSpring.cs
index 652c94a..8e7ddff 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintSpring.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintSpring.cs
@@ -85,6 +85,19 @@ public sealed class BSConstraintSpring : BSConstraint6Dof
         return true;
     }
 
+    public bool SetEquilibriumPoint(Vector3 linearEq, Vector3 angularEq)
+    {
+        PhysicsScene.DetailLog("{0},BSConstraintSpring.SetEquilibriumPoint,obj1ID={1},obj2ID={2},linearEq={3},angularEq={4}",
+                                        m_body1.ID, m_body1.ID, m_body2.ID, linearEq, angularEq);
+        PhysicsScene.PE.SpringSetEquilibriumPoint(m_constraint, 0, linearEq.X);
+        PhysicsScene.PE.SpringSetEquilibriumPoint(m_constraint, 1, linearEq.Y);
+        PhysicsScene.PE.SpringSetEquilibriumPoint(m_constraint, 2, linearEq.Z);
+        PhysicsScene.PE.SpringSetEquilibriumPoint(m_constraint, 3, angularEq.X);
+        PhysicsScene.PE.SpringSetEquilibriumPoint(m_constraint, 4, angularEq.Y);
+        PhysicsScene.PE.SpringSetEquilibriumPoint(m_constraint, 5, angularEq.Z);
+        return true;
+    }
+
 }
 
 }
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index b3347bf..aaf92c8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -63,6 +63,8 @@ public sealed class BSLinksetConstraints : BSLinkset
         public bool[] springAxisEnable;
         public float[] springDamping;
         public float[] springStiffness;
+        public OMV.Vector3 springLinearEquilibriumPoint;
+        public OMV.Vector3 springAngularEquilibriumPoint;
 
         public BSLinkInfoConstraint(BSPrimLinkable pMember)
             : base(pMember)
@@ -102,6 +104,8 @@ public sealed class BSLinksetConstraints : BSLinkset
                 springDamping[ii] = BSAPITemplate.SPRING_NOT_SPECIFIED;
                 springStiffness[ii] = BSAPITemplate.SPRING_NOT_SPECIFIED;
             }
+            springLinearEquilibriumPoint = OMV.Vector3.Zero;
+            springAngularEquilibriumPoint = OMV.Vector3.Zero;
             member.PhysScene.DetailLog("{0},BSLinkInfoConstraint.ResetLink", member.LocalID);
         }
 
@@ -155,7 +159,12 @@ public sealed class BSLinksetConstraints : BSLinkset
                             if (springStiffness[ii] != BSAPITemplate.SPRING_NOT_SPECIFIED)
                                 constrainSpring.SetStiffness(ii, springStiffness[ii]);
                         }
-                        constrainSpring.SetEquilibriumPoint(BSAPITemplate.SPRING_NOT_SPECIFIED, BSAPITemplate.SPRING_NOT_SPECIFIED);
+                        constrainSpring.CalculateTransforms();
+
+                        if (springLinearEquilibriumPoint != OMV.Vector3.Zero)
+                            constrainSpring.SetEquilibriumPoint(springLinearEquilibriumPoint, springAngularEquilibriumPoint);
+                        else
+                            constrainSpring.SetEquilibriumPoint(BSAPITemplate.SPRING_NOT_SPECIFIED, BSAPITemplate.SPRING_NOT_SPECIFIED);
                     }
                     break;
                 default:
@@ -618,6 +627,7 @@ public sealed class BSLinksetConstraints : BSLinkset
                             float valueFloat;
                             bool valueBool;
                             OMV.Vector3 valueVector;
+                            OMV.Vector3 valueVector2;
                             OMV.Quaternion valueQuaternion;
                             int axisLow, axisHigh;
 
@@ -764,6 +774,14 @@ public sealed class BSLinksetConstraints : BSLinkset
                                                 linkInfo.springStiffness[ii] = valueFloat;
                                             opIndex += 3;
                                             break;
+                                        case ExtendedPhysics.PHYS_PARAM_SPRING_EQUILIBRIUM_POINT:
+                                            errMsg = "PHYS_PARAM_SPRING_EQUILIBRIUM_POINT takes two parameters of type vector";
+                                            valueVector = (OMV.Vector3)pParams[opIndex + 1];
+                                            valueVector2 = (OMV.Vector3)pParams[opIndex + 2];
+                                            linkInfo.springLinearEquilibriumPoint = valueVector;
+                                            linkInfo.springAngularEquilibriumPoint = valueVector2;
+                                            opIndex += 3;
+                                            break;
                                         case ExtendedPhysics.PHYS_PARAM_USE_LINEAR_FRAMEA:
                                             errMsg = "PHYS_PARAM_USE_LINEAR_FRAMEA takes one parameter of type integer (bool)";
                                             valueBool = ((int)pParams[opIndex + 1]) != 0;