1 files changed, 82 insertions, 31 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 8c9a774..2c8dd23 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -51,6 +51,21 @@ sealed class BSLinksetCompoundInfo : BSLinksetInfo
         OffsetFromCenterOfMass = p;
         OffsetRot = r;
     }
+    // 'centerDisplacement' is the distance from the root the the center-of-mass (Bullet 'zero' of the shape)
+    public BSLinksetCompoundInfo(int indx, BSPhysObject root, BSPhysObject child, OMV.Vector3 centerDisplacement)
+    {
+        // Each child position and rotation is given relative to the center-of-mass.
+        OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(root.RawOrientation);
+        OMV.Vector3 displacementFromRoot = (child.RawPosition - root.RawPosition) * invRootOrientation;
+        OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
+        OMV.Quaternion displacementRot = child.RawOrientation * invRootOrientation;
+
+        // Save relative position for recomputing child's world position after moving linkset.
+        Index = indx;
+        OffsetFromRoot = displacementFromRoot;
+        OffsetFromCenterOfMass = displacementFromCOM;
+        OffsetRot = displacementRot;
+    }
     public override void Clear()
     {
         Index = 0;
@@ -182,24 +197,71 @@ public sealed class BSLinksetCompound : BSLinkset
 
     // 'physicalUpdate' is true if these changes came directly from the physics engine. Don't need to rebuild then.
     // Called at taint-time.
-    public override void UpdateProperties(BSPhysObject updated, bool physicalUpdate)
+    public override void UpdateProperties(UpdatedProperties whichUpdated, BSPhysObject updated)
     {
         // The user moving a child around requires the rebuilding of the linkset compound shape
         // One problem is this happens when a border is crossed -- the simulator implementation
-        //    is to store the position into the group which causes the move of the object
+        //    stores the position into the group which causes the move of the object
         //    but it also means all the child positions get updated.
         //    What would cause an unnecessary rebuild so we make sure the linkset is in a
         //    region before bothering to do a rebuild.
-        if (!IsRoot(updated) 
-                && !physicalUpdate 
-                && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
+        if (!IsRoot(updated) && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
         {
-            // TODO: replace this with are calculation of the child prim's orientation and pos.
-            // TODO: for the moment, don't rebuild the compound shape.
-            //    This is often just the car turning its wheels. When we can just reorient the one
-            //    member shape of the compound shape, the overhead of rebuilding won't be a problem.
-            // updated.LinksetInfo = null;
-            // ScheduleRebuild(updated);
+            // If a child of the linkset is updating only the position or rotation, that can be done
+            //    without rebuilding the linkset.
+            // If a handle for the child can be fetch, we update the child here. If a rebuild was
+            //    scheduled by someone else, the rebuild will just replace this setting.
+
+            bool updatedChild = false;
+            // Anything other than updating position or orientation usually means a physical update
+            //     and that is caused by us updating the object.
+            if ((whichUpdated & ~(UpdatedProperties.Position | UpdatedProperties.Orientation)) == 0)
+            {
+                // Gather the child info. It might not be there if the linkset is in transition.
+                BSLinksetCompoundInfo lsi = updated.LinksetInfo as BSLinksetCompoundInfo;
+                if (LinksetRoot.PhysShape.HasPhysicalShape && lsi != null)
+                {
+                    if (PhysicsScene.PE.IsCompound(LinksetRoot.PhysShape))
+                    {
+                        BulletShape linksetChildShape = PhysicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape, lsi.Index);
+                        if (linksetChildShape.HasPhysicalShape)
+                        {
+                            // Compute the offset from the center-of-gravity
+                            BSLinksetCompoundInfo newLsi = new BSLinksetCompoundInfo(lsi.Index, LinksetRoot, updated, LinksetRoot.PositionDisplacement);
+                            PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, lsi.Index,
+                                                                        newLsi.OffsetFromCenterOfMass,
+                                                                        newLsi.OffsetRot,
+                                                                        true /* shouldRecalculateLocalAabb */);
+                            DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1}newLsi={2}",
+                                                                        updated.LocalID, whichUpdated, newLsi);
+                            updated.LinksetInfo = newLsi;
+                            updatedChild = true;
+                        }
+                        else    // DEBUG DEBUG
+                        {       // DEBUG DEBUG
+                            DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}",
+                                                                        updated.LocalID, linksetChildShape);
+                        }       // DEBUG DEBUG
+                    }
+                    else    // DEBUG DEBUG
+                    {       // DEBUG DEBUG
+                        DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,notCompound", updated.LocalID);
+                    }       // DEBUG DEBUG
+                }
+                else    // DEBUG DEBUG
+                {       // DEBUG DEBUG
+                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,rootPhysShape={1},lsi={2}",
+                                updated.LocalID, LinksetRoot.PhysShape, lsi == null ? "NULL" : lsi.ToString());
+                }       // DEBUG DEBUG
+                if (!updatedChild)
+                {
+                    // If couldn't do the individual child, the linkset needs a rebuild to incorporate the new child info.
+                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild.schedulingRebuild,whichUpdated={1}",
+                                                                    updated.LocalID, whichUpdated);
+                    updated.LinksetInfo = null; // setting to 'null' causes relative position to be recomputed.
+                    ScheduleRebuild(updated);
+                }
+            }
         }
     }
 
@@ -228,13 +290,6 @@ public sealed class BSLinksetCompound : BSLinkset
         return ret;
     }
 
-    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
-    //     this routine will restore the removed constraints.
-    // Called at taint-time!!
-    public override void RestoreBodyDependencies(BSPrim child)
-    {
-    }
-
     // When the linkset is built, the child shape is added to the compound shape relative to the
     //    root shape. The linkset then moves around but this does not move the actual child
     //    prim. The child prim's location must be recomputed based on the location of the root shape.
@@ -322,7 +377,7 @@ public sealed class BSLinksetCompound : BSLinkset
     // Constraint linksets are rebuilt every time.
     // Note that this works for rebuilding just the root after a linkset is taken apart.
     // Called at taint time!!
-    private bool disableCOM = true;     // disable until we get this debugged
+    private bool disableCOM = false;     // disable until we get this debugged
     private void RecomputeLinksetCompound()
     {
         try
@@ -345,12 +400,16 @@ public sealed class BSLinksetCompound : BSLinkset
             }                                           // DEBUG DEBUG
             else
             {
-                centerOfMass = ComputeLinksetGeometricCenter();
-                centerDisplacement = centerOfMass - LinksetRoot.RawPosition;
+                centerOfMass = ComputeLinksetCenterOfMass();
+                // 'centerDisplacement' is the value to *add* to all the shape offsets
+                centerDisplacement = LinksetRoot.RawPosition - centerOfMass;
 
                 // Since we're displacing the center of the shape, we need to move the body in the world
                 LinksetRoot.PositionDisplacement = centerDisplacement;
 
+                // This causes the root prim position to be set properly based on the new PositionDisplacement
+                LinksetRoot.ForcePosition = LinksetRoot.RawPosition;
+                // Update the local transform for the root child shape so it is offset from the <0,0,0> which is COM
                 PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, -centerDisplacement, OMV.Quaternion.Identity, false);
                 DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}",
                                         LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement);
@@ -372,15 +431,7 @@ public sealed class BSLinksetCompound : BSLinkset
                     BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo;
                     if (lci == null)
                     {
-                        // Each child position and rotation is given relative to the center-of-mass.
-                        OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
-                        OMV.Vector3 displacementFromRoot = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
-                        OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
-                        OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
-
-                        // Save relative position for recomputing child's world position after moving linkset.
-                        lci = new BSLinksetCompoundInfo(memberIndex, displacementFromCOM, displacementRot);
-                        lci.OffsetFromRoot = displacementFromRoot;
+                        lci = new BSLinksetCompoundInfo(memberIndex, LinksetRoot, cPrim, centerDisplacement);
                         cPrim.LinksetInfo = lci;
                         DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci);
                     }
@@ -390,7 +441,7 @@ public sealed class BSLinksetCompound : BSLinkset
 
                     if (cPrim.PhysShape.isNativeShape)
                     {
-                        // A native shape is turning into a hull collision shape because native
+                        // A native shape is turned into a hull collision shape because native
                         //    shapes are not shared so we have to hullify it so it will be tracked
                         //    and freed at the correct time. This also solves the scaling problem
                         //    (native shapes scaled but hull/meshes are assumed to not be).