BulletSim: freshen up the code for constraint based linksets.

3 files changed, 64 insertions, 23 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 0204967..4c2c1c1 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -1029,8 +1029,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // Add this correction to the velocity to make it faster/slower.
             VehicleVelocity += linearMotorVelocityW;
 
-            VDetailLog("{0},  MoveLinear,velocity,origVelW={1},velV={2},correctV={3},correctW={4},newVelW={5},fricFact={6}",
-                        ControllingPrim.LocalID, origVelW, currentVelV, linearMotorCorrectionV,
+            VDetailLog("{0},  MoveLinear,velocity,origVelW={1},velV={2},tgt={3},correctV={4},correctW={5},newVelW={6},fricFact={7}",
+                        ControllingPrim.LocalID, origVelW, currentVelV, m_linearMotor.TargetValue, linearMotorCorrectionV,
                         linearMotorVelocityW, VehicleVelocity, frictionFactorV);
         }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 1d94142..3668456 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -59,6 +59,7 @@ public sealed class BSLinksetCompound : BSLinkset
     {
         DetailLog("{0},BSLinksetCompound.ScheduleRebuild,,rebuilding={1},hasChildren={2},actuallyScheduling={3}",
                             requestor.LocalID, Rebuilding, HasAnyChildren, (!Rebuilding && HasAnyChildren));
+
         // When rebuilding, it is possible to set properties that would normally require a rebuild.
         //    If already rebuilding, don't request another rebuild.
         //    If a linkset with just a root prim (simple non-linked prim) don't bother rebuilding.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index a06a44d..f17d698 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -48,12 +48,22 @@ public sealed class BSLinksetConstraints : BSLinkset
     {
         base.Refresh(requestor);
 
-        if (HasAnyChildren && IsRoot(requestor))
+    }
+
+    private void ScheduleRebuild(BSPrimLinkable requestor)
+    {
+        DetailLog("{0},BSLinksetConstraint.ScheduleRebuild,,rebuilding={1},hasChildren={2},actuallyScheduling={3}",
+                            requestor.LocalID, Rebuilding, HasAnyChildren, (!Rebuilding && HasAnyChildren));
+
+        // When rebuilding, it is possible to set properties that would normally require a rebuild.
+        //    If already rebuilding, don't request another rebuild.
+        //    If a linkset with just a root prim (simple non-linked prim) don't bother rebuilding.
+        if (!Rebuilding && HasAnyChildren)
         {
             // Queue to happen after all the other taint processing
             m_physicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
                 {
-                    if (HasAnyChildren && IsRoot(requestor))
+                    if (HasAnyChildren)
                         RecomputeLinksetConstraints();
                 });
         }
@@ -67,8 +77,14 @@ public sealed class BSLinksetConstraints : BSLinkset
     // Called at taint-time!
     public override bool MakeDynamic(BSPrimLinkable child)
     {
-        // What is done for each object in BSPrim is what we want.
-        return false;
+        bool ret = false;
+        DetailLog("{0},BSLinksetConstraints.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child));
+        if (IsRoot(child))
+        {
+            // The root is going dynamic. Rebuild the linkset so parts and mass get computed properly.
+            ScheduleRebuild(LinksetRoot);
+        }
+        return ret;
     }
 
     // The object is going static (non-physical). Do any setup necessary for a static linkset.
@@ -78,8 +94,16 @@ public sealed class BSLinksetConstraints : BSLinkset
     // Called at taint-time!
     public override bool MakeStatic(BSPrimLinkable child)
     {
-        // What is done for each object in BSPrim is what we want.
-        return false;
+        bool ret = false;
+
+        DetailLog("{0},BSLinksetConstraint.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
+        child.ClearDisplacement();
+        if (IsRoot(child))
+        {
+            // Schedule a rebuild to verify that the root shape is set to the real shape.
+            ScheduleRebuild(LinksetRoot);
+        }
+        return ret;
     }
 
     // Called at taint-time!!
@@ -105,7 +129,7 @@ public sealed class BSLinksetConstraints : BSLinkset
             // Just undo all the constraints for this linkset. Rebuild at the end of the step.
             ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
             // Cause the constraints, et al to be rebuilt before the next simulation step.
-            Refresh(LinksetRoot);
+            ScheduleRebuild(LinksetRoot);
         }
         return ret;
     }
@@ -123,7 +147,7 @@ public sealed class BSLinksetConstraints : BSLinkset
             DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
 
             // Cause constraints and assorted properties to be recomputed before the next simulation step.
-            Refresh(LinksetRoot);
+            ScheduleRebuild(LinksetRoot);
         }
         return;
     }
@@ -147,7 +171,7 @@ public sealed class BSLinksetConstraints : BSLinkset
                 PhysicallyUnlinkAChildFromRoot(rootx, childx);
             });
             // See that the linkset parameters are recomputed at the end of the taint time.
-            Refresh(LinksetRoot);
+            ScheduleRebuild(LinksetRoot);
         }
         else
         {
@@ -165,6 +189,7 @@ public sealed class BSLinksetConstraints : BSLinkset
         Refresh(rootPrim);
     }
 
+    // Create a static constraint between the two passed objects
     private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
     {
         // Zero motion for children so they don't interpolate
@@ -281,24 +306,39 @@ public sealed class BSLinksetConstraints : BSLinkset
         DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}",
                             LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString, linksetMass);
 
-        foreach (BSPrimLinkable child in m_children)
+        try
         {
-            // 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);
+            Rebuilding = true;
 
-            BSConstraint constrain;
-            if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
+            // There is no reason to build all this physical stuff for a non-physical linkset.
+            if (!LinksetRoot.IsPhysicallyActive)
             {
-                // If constraint doesn't exist yet, create it.
-                constrain = BuildConstraint(LinksetRoot, child);
+                DetailLog("{0},BSLinksetConstraint.RecomputeLinksetCompound,notPhysical", LinksetRoot.LocalID);
+                return; // Note the 'finally' clause at the botton which will get executed.
             }
-            constrain.RecomputeConstraintVariables(linksetMass);
 
-            // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint);    // DEBUG DEBUG
-        }
+            foreach (BSPrimLinkable child in m_children)
+            {
+                // 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);
+
+                BSConstraint constrain;
+                if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
+                {
+                    // If constraint doesn't exist yet, create it.
+                    constrain = BuildConstraint(LinksetRoot, child);
+                }
+                constrain.RecomputeConstraintVariables(linksetMass);
 
+                // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint);    // DEBUG DEBUG
+            }
+        }
+        finally
+        {
+            Rebuilding = false;
+        }
     }
 }
 }