1 files changed, 12 insertions, 8 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 96626ca..1d94142 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -271,6 +271,8 @@ public sealed class BSLinksetCompound : BSLinkset
             //     to what they should be as if the root was not in a linkset.
             // Not that bad since we only get into this routine if there are children in the linkset and
             //     something has been updated/changed.
+            // Have to do the rebuild before checking for physical because this might be a linkset
+            //     being destructed and going non-physical.
             LinksetRoot.ForceBodyShapeRebuild(true);
 
             // There is no reason to build all this physical stuff for a non-physical linkset.
@@ -283,28 +285,29 @@ public sealed class BSLinksetCompound : BSLinkset
             // Get a new compound shape to build the linkset shape in.
             BSShape linksetShape = BSShapeCompound.GetReference(m_physicsScene);
 
-            // The center of mass for the linkset is the geometric center of the group.
             // Compute a displacement for each component so it is relative to the center-of-mass.
             // Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass
             OMV.Vector3 centerOfMassW = ComputeLinksetCenterOfMass();
 
             OMV.Quaternion invRootOrientation = OMV.Quaternion.Normalize(OMV.Quaternion.Inverse(LinksetRoot.RawOrientation));
+            OMV.Vector3 origRootPosition = LinksetRoot.RawPosition;
 
-            // 'centerDisplacementV' is the value to subtract from children to give physical offset position
+            // 'centerDisplacementV' is the vehicle relative distance from the simulator root position to the center-of-mass
             OMV.Vector3 centerDisplacementV = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
             if (UseBulletSimRootOffsetHack || !BSParam.LinksetOffsetCenterOfMass)
             {
+                // Zero everything if center-of-mass displacement is not being done.
                 centerDisplacementV = OMV.Vector3.Zero;
                 LinksetRoot.ClearDisplacement();
             }
             else
             {
-                LinksetRoot.SetEffectiveCenterOfMassDisplacement(centerDisplacementV);
                 // The actual center-of-mass could have been set by the user.
-                centerDisplacementV = LinksetRoot.PositionDisplacement;
+                centerDisplacementV = LinksetRoot.SetEffectiveCenterOfMassDisplacement(centerDisplacementV);
             }
+
             DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,rootPos={1},com={2},comDisp={3}",
-                                LinksetRoot.LocalID, LinksetRoot.RawPosition, centerOfMassW, centerDisplacementV);
+                                LinksetRoot.LocalID, origRootPosition, centerOfMassW, centerDisplacementV);
 
             // Add the shapes of all the components of the linkset
             int memberIndex = 1;
@@ -321,11 +324,11 @@ public sealed class BSLinksetCompound : BSLinkset
                     memberIndex++;
                 }
 
-                // Get a reference to the shape of the child and add that shape to the linkset compound shape
+                // Get a reference to the shape of the child for adding of that shape to the linkset compound shape
                 BSShape childShape = cPrim.PhysShape.GetReference(m_physicsScene, cPrim);
 
-                // Offset the child shape from the center-of-mass and rotate it to vehicle relative
-                OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacementV;
+                // Offset the child shape from the center-of-mass and rotate it to vehicle relative.
+                OMV.Vector3 offsetPos = (cPrim.RawPosition - origRootPosition) * invRootOrientation - centerDisplacementV;
                 OMV.Quaternion offsetRot = OMV.Quaternion.Normalize(cPrim.RawOrientation) * invRootOrientation;
 
                 // Add the child shape to the compound shape being built
@@ -366,6 +369,7 @@ public sealed class BSLinksetCompound : BSLinkset
                 // This enables a feature in the C++ code to return the world coordinates of the first shape in the
                 //     compound shape. This aleviates the need to offset the returned physical position by the
                 //     center-of-mass offset.
+                // TODO: either debug this feature or remove it.
                 m_physicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
             }
         }