1 files changed, 60 insertions, 25 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
index 6401308..f1c3b5c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -44,72 +44,107 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public class BSPrimDisplaced : BSPrim
 {
-    // 'Position' and 'Orientation' is what the simulator thinks the positions of the prim is.
-    // Because Bullet needs the zero coordinate to be the center of mass of the linkset,
-    //     sometimes it is necessary to displace the position the physics engine thinks
-    //     the position is. PositionDisplacement must be added and removed from the
-    //     position as the simulator position is stored and fetched from the physics
-    //     engine. Similar to OrientationDisplacement.
+    // The purpose of this module 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.
+    //
+    // This routine works by capturing the Force* setting of position/orientation/... and
+    //    adjusting the simulator values (being set) into the physical values.
+    //    The conversion is also done in the opposite direction (physical origin -> simulator origin).
+    //
+    // The updateParameter call is also captured and the values from the physics engine
+    //    are converted into simulator origin values before being passed to the base
+    //    class.
+
     public virtual OMV.Vector3 PositionDisplacement { get; set; }
     public virtual OMV.Quaternion OrientationDisplacement { get; set; }
-    public virtual OMV.Vector3 CenterOfMassLocation { get; set; }
-    public virtual OMV.Vector3 GeometricCenterLocation { get; set; }
 
     public BSPrimDisplaced(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
                        OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
         : base(localID, primName, parent_scene, pos, size, rotation, pbs, pisPhysical)
     {
-        CenterOfMassLocation = RawPosition;
-        GeometricCenterLocation = RawPosition;
+        ClearDisplacement();
     }
 
-    public override Vector3 ForcePosition
+    public void ClearDisplacement()
+    {
+        PositionDisplacement = OMV.Vector3.Zero;
+        OrientationDisplacement = OMV.Quaternion.Identity;
+    }
+
+    // Set this sets and computes the displacement from the passed prim to the center-of-mass.
+    // A user set value for center-of-mass overrides whatever might be passed in here.
+    // The displacement is in local coordinates (relative to root prim in linkset oriented coordinates).
+    public virtual void SetEffectiveCenterOfMassW(Vector3 centerOfMassDisplacement)
     {
-        get
+        Vector3 comDisp;
+        if (UserSetCenterOfMass.HasValue)
+            comDisp = (OMV.Vector3)UserSetCenterOfMass;
+        else
+            comDisp = centerOfMassDisplacement;
+
+        if (comDisp == Vector3.Zero)
         {
-            return base.ForcePosition;
+            // If there is no diplacement. Things get reset.
+            PositionDisplacement = OMV.Vector3.Zero;
+            OrientationDisplacement = OMV.Quaternion.Identity;
         }
-        set
+        else
         {
-            base.ForcePosition = value;
-            CenterOfMassLocation = RawPosition;
-            GeometricCenterLocation = RawPosition;
+            // Remember the displacement from root as well as the origional rotation of the
+            //    new center-of-mass.
+            PositionDisplacement = comDisp;
+            OrientationDisplacement = OMV.Quaternion.Identity;
         }
     }
 
-    public override Quaternion ForceOrientation
+    public override Vector3 ForcePosition
     {
-        get
+        get { return base.ForcePosition; }
+        set
         {
-            return base.ForceOrientation;
+            if (PositionDisplacement != OMV.Vector3.Zero)
+                base.ForcePosition = value - (PositionDisplacement * RawOrientation);
+            else
+                base.ForcePosition = value;
         }
+    }
+
+    public override Quaternion ForceOrientation
+    {
+        get { return base.ForceOrientation; }
         set
         {
             base.ForceOrientation = value;
         }
     }
 
+    // TODO: decide if this is the right place for these variables.
+    //     Somehow incorporate the optional settability by the user.
     // Is this used?
     public override OMV.Vector3 CenterOfMass
     {
-        get { return CenterOfMassLocation; }
+        get { return RawPosition; }
     }
 
     // Is this used?
     public override OMV.Vector3 GeometricCenter
     {
-        get { return GeometricCenterLocation; }
+        get { return RawPosition; }
     }
 
-
     public override void UpdateProperties(EntityProperties entprop)
     {
         // Undo any center-of-mass displacement that might have been done.
-        if (PositionDisplacement != OMV.Vector3.Zero)
+        if (PositionDisplacement != OMV.Vector3.Zero || OrientationDisplacement != OMV.Quaternion.Identity)
         {
             // Correct for any rotation around the center-of-mass
             // TODO!!!
-            entprop.Position -= PositionDisplacement;
+            entprop.Position = entprop.Position + (PositionDisplacement * entprop.Rotation);
+            // entprop.Rotation = something;
         }
 
         base.UpdateProperties(entprop);