4 files changed, 39 insertions, 20 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 308cf13..30f6c8c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -315,7 +315,6 @@ public sealed class BSLinksetCompound : BSLinkset
     // Note that this works for rebuilding just the root after a linkset is taken apart.
     // Called at taint time!!
     private bool UseBulletSimRootOffsetHack = false;    // Attempt to have Bullet track the coords of root compound shape
-    private bool disableCOM = true; // For basic linkset debugging, turn off the center-of-mass setting
     private void RecomputeLinksetCompound()
     {
         try
@@ -331,9 +330,7 @@ public sealed class BSLinksetCompound : BSLinkset
             // There is no reason to build all this physical stuff for a non-physical linkset.
             if (!LinksetRoot.IsPhysicallyActive)
             {
-                // Clean up any old linkset shape and make sure the root shape is set to the root object.
                 DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,notPhysical", LinksetRoot.LocalID);
-
                 return; // Note the 'finally' clause at the botton which will get executed.
             }
 
@@ -347,9 +344,9 @@ public sealed class BSLinksetCompound : BSLinkset
 
             OMV.Quaternion invRootOrientation = OMV.Quaternion.Normalize(OMV.Quaternion.Inverse(LinksetRoot.RawOrientation));
 
-            // 'centerDisplacement' is the value to subtract from children to give physical offset position
+            // 'centerDisplacementV' is the value to subtract from children to give physical offset position
             OMV.Vector3 centerDisplacementV = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
-            if (UseBulletSimRootOffsetHack || disableCOM)
+            if (UseBulletSimRootOffsetHack || !BSParam.LinksetOffsetCenterOfMass)
             {
                 centerDisplacementV = OMV.Vector3.Zero;
                 LinksetRoot.ClearDisplacement();
@@ -357,6 +354,8 @@ public sealed class BSLinksetCompound : BSLinkset
             else
             {
                 LinksetRoot.SetEffectiveCenterOfMassDisplacement(centerDisplacementV);
+                // The actual center-of-mass could have been set by the user.
+                centerDisplacementV = LinksetRoot.PositionDisplacement;
             }
             DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,rootPos={1},com={2},comDisp={3}",
                                 LinksetRoot.LocalID, LinksetRoot.RawPosition, centerOfMassW, centerDisplacementV);
@@ -409,7 +408,7 @@ public sealed class BSLinksetCompound : BSLinkset
             {
                 // Enable the physical position updator to return the position and rotation of the root shape.
                 // This enables a feature in the C++ code to return the world coordinates of the first shape in the
-                //     compound shape. This eleviates the need to offset the returned physical position by the
+                //     compound shape. This aleviates the need to offset the returned physical position by the
                 //     center-of-mass offset.
                 m_physicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
             }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index d17c8e7..0f84bf7 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -176,6 +176,7 @@ public static class BSParam
 
     // Linkset implementation parameters
     public static float LinksetImplementation { get; private set; }
+    public static bool LinksetOffsetCenterOfMass { get; private set; }
     public static bool LinkConstraintUseFrameOffset { get; private set; }
     public static bool LinkConstraintEnableTransMotor { get; private set; }
     public static float LinkConstraintTransMotorMaxVel { get; private set; }
@@ -684,6 +685,8 @@ public static class BSParam
 
             new ParameterDefn<float>("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)",
             (float)BSLinkset.LinksetImplementation.Compound ),
+            new ParameterDefn<bool>("LinksetOffsetCenterOfMass", "If 'true', compute linkset center-of-mass and offset linkset position to account for same",
+            false ),
             new ParameterDefn<bool>("LinkConstraintUseFrameOffset", "For linksets built with constraints, enable frame offsetFor linksets built with constraints, enable frame offset.",
             false ),
             new ParameterDefn<bool>("LinkConstraintEnableTransMotor", "Whether to enable translational motor on linkset constraints",
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
index f5ee671..9a05349 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -44,12 +44,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public class BSPrimDisplaced : BSPrim
 {
-    // The purpose of this module is to do any mapping between what the simulator thinks
+    // The purpose of this subclass is to do any mapping between what the simulator thinks
     //    the prim position and orientation is and what the physical position/orientation.
     //    This difference happens because Bullet assumes the center-of-mass is the <0,0,0>
-    //    of the prim/linkset. The simulator tracks the location of the prim/linkset by
-    //    the location of the root prim. So, if center-of-mass is anywhere but the origin
-    //    of the root prim, the physical origin is displaced from the simulator origin.
+    //    of the prim/linkset. The simulator, on the other hand, tracks the location of
+    //    the prim/linkset by the location of the root prim. So, if center-of-mass is anywhere
+    //    but the origin of the root prim, the physical origin is displaced from the simulator origin.
     //
     // This routine works by capturing the Force* setting of position/orientation/... and
     //    adjusting the simulator values (being set) into the physical values.
@@ -61,6 +61,7 @@ public class BSPrimDisplaced : BSPrim
 
     public virtual OMV.Vector3 PositionDisplacement { get; set; }
     public virtual OMV.Quaternion OrientationDisplacement { get; set; }
+    public virtual OMV.Quaternion OrientationDisplacementOrigInv { get; set; }
 
     public BSPrimDisplaced(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
                        OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
@@ -99,7 +100,8 @@ public class BSPrimDisplaced : BSPrim
             // Remember the displacement from root as well as the origional rotation of the
             //    new center-of-mass.
             PositionDisplacement = comDisp;
-            OrientationDisplacement = OMV.Quaternion.Identity;
+            OrientationDisplacement = Quaternion.Normalize(RawOrientation);
+            OrientationDisplacementOrigInv = Quaternion.Inverse(OrientationDisplacement);
         }
     }
 
@@ -110,7 +112,7 @@ public class BSPrimDisplaced : BSPrim
         {
             if (PositionDisplacement != OMV.Vector3.Zero)
             {
-                OMV.Vector3 displacedPos = value - (PositionDisplacement * RawOrientation);
+                OMV.Vector3 displacedPos = value - (PositionDisplacement * (OrientationDisplacementOrigInv * RawOrientation));
                 DetailLog("{0},BSPrimDisplaced.ForcePosition,val={1},disp={2},newPos={3}", LocalID, value, PositionDisplacement, displacedPos);
                 base.ForcePosition = displacedPos;
             }
@@ -126,7 +128,8 @@ public class BSPrimDisplaced : BSPrim
         get { return base.ForceOrientation; }
         set
         {
-            // TODO:
+            // Changing orientation also changes the position of the center-of-mass since
+            //    the orientation passed is for a rotation around the root prim's center.
             base.ForceOrientation = value;
         }
     }
@@ -150,10 +153,13 @@ public class BSPrimDisplaced : BSPrim
         // Undo any center-of-mass displacement that might have been done.
         if (PositionDisplacement != OMV.Vector3.Zero || OrientationDisplacement != OMV.Quaternion.Identity)
         {
-            // Correct for any rotation around the center-of-mass
-            // TODO!!!
+            // The origional shape was offset from 'zero' by PositionDisplacement and rotated by OrientationDisplacement.
+            // These physical locations and rotation must be back converted to be centered around the displaced
+            //     root shape.
+
+            // The root position is the reported position displaced by the rotated displacement.
+            OMV.Vector3 displacedPos = entprop.Position + (PositionDisplacement * (entprop.Rotation * OrientationDisplacementOrigInv));
             
-            OMV.Vector3 displacedPos = entprop.Position + (PositionDisplacement * entprop.Rotation);
             DetailLog("{0},BSPrimDisplaced.ForcePosition,physPos={1},disp={2},newPos={3}", LocalID, entprop.Position, PositionDisplacement, displacedPos);
             entprop.Position = displacedPos;
             // entprop.Rotation = something;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
index 87eed98..6d7de35 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
@@ -37,6 +37,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public class BSPrimLinkable : BSPrimDisplaced
 {
+    // The purpose of this subclass is to add linkset functionality to the prim. This overrides
+    //    operations necessary for keeping the linkset created and, additionally, this
+    //    calls the linkset implementation for its creation and management.
+
+    // This adds the overrides for link() and delink() so the prim is linkable.
+
     public BSLinkset Linkset { get; set; }
     // The index of this child prim.
     public int LinksetChildIndex { get; set; }
@@ -69,8 +75,8 @@ public class BSPrimLinkable : BSPrimDisplaced
         BSPrimLinkable parent = obj as BSPrimLinkable;
         if (parent != null)
         {
-            BSPhysObject parentBefore = Linkset.LinksetRoot;
-            int childrenBefore = Linkset.NumberOfChildren;
+            BSPhysObject parentBefore = Linkset.LinksetRoot;    // DEBUG
+            int childrenBefore = Linkset.NumberOfChildren;      // DEBUG
 
             Linkset = parent.Linkset.AddMeToLinkset(this);
 
@@ -85,8 +91,8 @@ public class BSPrimLinkable : BSPrimDisplaced
         // TODO: decide if this parent checking needs to happen at taint time
         // Race condition here: if link() and delink() in same simulation tick, the delink will not happen
 
-        BSPhysObject parentBefore = Linkset.LinksetRoot;
-        int childrenBefore = Linkset.NumberOfChildren;
+        BSPhysObject parentBefore = Linkset.LinksetRoot;        // DEBUG
+        int childrenBefore = Linkset.NumberOfChildren;          // DEBUG
 
         Linkset = Linkset.RemoveMeFromLinkset(this);
 
@@ -128,6 +134,7 @@ public class BSPrimLinkable : BSPrimDisplaced
         get { return Linkset.LinksetMass; }
     }
 
+    // Refresh the linkset structure and parameters when the prim's physical parameters are changed.
     public override void UpdatePhysicalParameters()
     {
         base.UpdatePhysicalParameters();
@@ -139,6 +146,7 @@ public class BSPrimLinkable : BSPrimDisplaced
             Linkset.Refresh(this);
     }
 
+    // When the prim is made dynamic or static, the linkset needs to change.
     protected override void MakeDynamic(bool makeStatic)
     {
         base.MakeDynamic(makeStatic);
@@ -155,6 +163,8 @@ public class BSPrimLinkable : BSPrimDisplaced
         base.RemoveDependencies();
     }
 
+    // Called after a simulation step for the changes in physical object properties.
+    // Do any filtering/modification needed for linksets.
     public override void UpdateProperties(EntityProperties entprop)
     {
         if (Linkset.IsRoot(this))
@@ -176,6 +186,7 @@ public class BSPrimLinkable : BSPrimDisplaced
         Linkset.UpdateProperties(UpdatedProperties.EntPropUpdates, this);
     }
 
+    // Called after a simulation step to post a collision with this object.
     public override bool Collide(uint collidingWith, BSPhysObject collidee,
                     OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
     {