BulletSim: complete linkage of spring constraint into linkset constraint.

3 files changed, 77 insertions, 15 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
index d0949f5..5008ff7 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
@@ -32,12 +32,19 @@ using OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 
-public sealed class BSConstraint6Dof : BSConstraint
+public class BSConstraint6Dof : BSConstraint
 {
     private static string LogHeader = "[BULLETSIM 6DOF CONSTRAINT]";
 
     public override ConstraintType Type { get { return ConstraintType.D6_CONSTRAINT_TYPE; } }
 
+    public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2) :base(world)
+    {
+        m_body1 = obj1;
+        m_body2 = obj2;
+        m_enabled = false;
+    }
+
     // Create a btGeneric6DofConstraint
     public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2,
                     Vector3 frame1, Quaternion frame1rot,
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index a051002..d4b1c1e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -77,6 +77,8 @@ public abstract class BSLinkset
         {
             member = pMember;
         }
+        public virtual void ResetLink() { }
+        public virtual void SetLinkParameters(BSConstraint constrain) { }
     }
 
     public LinksetImplementation LinksetImpl { get; protected set; }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index d4ee27d..f3b70c3 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -51,16 +51,26 @@ public sealed class BSLinksetConstraints : BSLinkset
         public float cfm;
         public float erp;
         public float solverIterations;
+        //
+        public OMV.Vector3 frameInAloc;
+        public OMV.Quaternion frameInArot;
+        public OMV.Vector3 frameInBloc;
+        public OMV.Quaternion frameInBrot;
+        // Spring
+        public float springDamping;
+        public float springStiffness;
+
+
 
         public BSLinkInfoConstraint(BSPrimLinkable pMember)
             : base(pMember)
         {
             constraint = null;
-            ResetToFixedConstraint();
+            ResetLink();
         }
 
         // Set all the parameters for this constraint to a fixed, non-movable constraint.
-        public void ResetToFixedConstraint()
+        public override void ResetLink()
         {
             constraintType = ConstraintType.D6_CONSTRAINT_TYPE;
             linearLimitLow = OMV.Vector3.Zero;
@@ -74,10 +84,16 @@ public sealed class BSLinksetConstraints : BSLinkset
             cfm = BSParam.LinkConstraintCFM;
             erp = BSParam.LinkConstraintERP;
             solverIterations = BSParam.LinkConstraintSolverIterations;
+            frameInAloc = OMV.Vector3.Zero;
+            frameInArot = OMV.Quaternion.Identity;
+            frameInBloc = OMV.Vector3.Zero;
+            frameInBrot = OMV.Quaternion.Identity;
+            springDamping = -1f;
+            springStiffness = -1f;
         }
 
         // Given a constraint, apply the current constraint parameters to same.
-        public void SetConstraintParameters(BSConstraint constrain)
+        public override void SetLinkParameters(BSConstraint constrain)
         {
             switch (constraintType)
             {
@@ -85,6 +101,7 @@ public sealed class BSLinksetConstraints : BSLinkset
                     BSConstraint6Dof constrain6dof = constrain as BSConstraint6Dof;
                     if (constrain6dof != null)
                     {
+                        // NOTE: D6_SPRING_CONSTRAINT_TYPE should be updated if you change any of this code.
                         // zero linear and angular limits makes the objects unable to move in relation to each other
                         constrain6dof.SetLinearLimits(linearLimitLow, linearLimitHigh);
                         constrain6dof.SetAngularLimits(angularLimitLow, angularLimitHigh);
@@ -99,6 +116,31 @@ public sealed class BSLinksetConstraints : BSLinkset
                         }
                     }
                     break;
+                case ConstraintType.D6_SPRING_CONSTRAINT_TYPE:
+                    BSConstraintSpring constrainSpring = constrain as BSConstraintSpring;
+                    if (constrainSpring != null)
+                    {
+                        // zero linear and angular limits makes the objects unable to move in relation to each other
+                        constrainSpring.SetLinearLimits(linearLimitLow, linearLimitHigh);
+                        constrainSpring.SetAngularLimits(angularLimitLow, angularLimitHigh);
+
+                        // tweek the constraint to increase stability
+                        constrainSpring.UseFrameOffset(useFrameOffset);
+                        constrainSpring.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce);
+                        constrainSpring.SetCFMAndERP(cfm, erp);
+                        if (solverIterations != 0f)
+                        {
+                            constrainSpring.SetSolverIterations(solverIterations);
+                        }
+                        for (int ii = 0; ii < 6; ii++)
+                        {
+                            if (springDamping != -1)
+                                constrainSpring.SetDamping(ii, springDamping);
+                            if (springStiffness != -1)
+                            constrainSpring.SetStiffness(ii, springStiffness);
+                        }
+                    }
+                    break;
                 default:
                     break;
             }
@@ -262,8 +304,8 @@ public sealed class BSLinksetConstraints : BSLinkset
     // Create a static constraint between the two passed objects
     private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li)
     {
-        BSLinkInfoConstraint liConstraint = li as BSLinkInfoConstraint;
-        if (liConstraint == null)
+        BSLinkInfoConstraint linkInfo = li as BSLinkInfoConstraint;
+        if (linkInfo == null)
             return null;
 
         // Zero motion for children so they don't interpolate
@@ -271,27 +313,25 @@ public sealed class BSLinksetConstraints : BSLinkset
 
         BSConstraint constrain = null;
 
-        switch (liConstraint.constraintType)
+        switch (linkInfo.constraintType)
         {
             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;
+                OMV.Vector3 childRelativePosition = linkInfo.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);
+                DetailLog("{0},BSLinksetConstraint.BuildConstraint,6Dof,rBody={1},cBody={2},rLoc={3},cLoc={4},midLoc={7}",
+                                                rootPrim.LocalID, rootPrim.PhysBody, linkInfo.member.PhysBody,
+                                                rootPrim.Position, linkInfo.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 );
+                                    m_physicsScene.World, rootPrim.PhysBody, linkInfo.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
@@ -320,11 +360,23 @@ public sealed class BSLinksetConstraints : BSLinkset
                 */
 
                 break;
+            case ConstraintType.D6_SPRING_CONSTRAINT_TYPE:
+                constrain = new BSConstraintSpring(m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody,
+                                linkInfo.frameInAloc, linkInfo.frameInArot, linkInfo.frameInBloc, linkInfo.frameInBrot,
+                                true /*useLinearReferenceFrameA*/,
+                                true /*disableCollisionsBetweenLinkedBodies*/);
+                DetailLog("{0},BSLinksetConstraint.BuildConstraint,spring,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
+                                                rootPrim.LocalID,
+                                                rootPrim.LocalID, rootPrim.PhysBody.AddrString,
+                                                linkInfo.member.LocalID, linkInfo.member.PhysBody.AddrString,
+                                                rootPrim.Position, linkInfo.member.Position);
+
+                break;
             default:
                 break;
         }
 
-        liConstraint.SetConstraintParameters(constrain);
+        linkInfo.SetLinkParameters(constrain);
 
         m_physicsScene.Constraints.AddConstraint(constrain);
 
@@ -401,6 +453,7 @@ public sealed class BSLinksetConstraints : BSLinkset
                     // If constraint doesn't exist yet, create it.
                     constrain = BuildConstraint(LinksetRoot, li);
                 }
+                li.SetLinkParameters(constrain);
                 constrain.RecomputeConstraintVariables(linksetMass);
 
                 // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint);    // DEBUG DEBUG