1 files changed, 45 insertions, 15 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
index a11bca0..35d5a08 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -59,6 +59,7 @@ public class BSPrimDisplaced : BSPrim
     //    are converted into simulator origin values before being passed to the base
     //    class.
 
+    // PositionDisplacement is the vehicle relative distance from the root prim position to the center-of-mass.
     public virtual OMV.Vector3 PositionDisplacement { get; set; }
 
     public BSPrimDisplaced(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
@@ -72,49 +73,77 @@ public class BSPrimDisplaced : BSPrim
     // Does not clear the displacement set by the user.
     public void ClearDisplacement()
     {
-        SetEffectiveCenterOfMassDisplacement(OMV.Vector3.Zero);
+        if (UserSetCenterOfMassDisplacement.HasValue)
+            PositionDisplacement = (OMV.Vector3)UserSetCenterOfMassDisplacement;
+        else
+            PositionDisplacement = OMV.Vector3.Zero;
     }
 
     // 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).
+    // Returns the relative offset from the root position to the center-of-mass.
     // Called at taint time.
-    public virtual void SetEffectiveCenterOfMassDisplacement(Vector3 centerOfMassDisplacement)
+    public virtual Vector3 SetEffectiveCenterOfMassDisplacement(Vector3 centerOfMassDisplacement)
     {
+        PhysScene.AssertInTaintTime("BSPrimDisplaced.SetEffectiveCenterOfMassDisplacement");
         Vector3 comDisp;
         if (UserSetCenterOfMassDisplacement.HasValue)
             comDisp = (OMV.Vector3)UserSetCenterOfMassDisplacement;
         else
             comDisp = centerOfMassDisplacement;
 
+        // Eliminate any jitter caused be very slight differences in masses and positions
         if (comDisp.ApproxEquals(Vector3.Zero, 0.01f) )
             comDisp = Vector3.Zero;
 
         DetailLog("{0},BSPrimDisplaced.SetEffectiveCenterOfMassDisplacement,userSet={1},comDisp={2}",
                                     LocalID, UserSetCenterOfMassDisplacement.HasValue, comDisp);
-        if ( comDisp != PositionDisplacement )
+        if ( !comDisp.ApproxEquals(PositionDisplacement, 0.01f) )
         {
             // Displacement setting is changing.
             // The relationship between the physical object and simulated object must be aligned.
             PositionDisplacement = comDisp;
             this.ForcePosition = RawPosition;
         }
+
+        return PositionDisplacement;
     }
 
+    // 'ForcePosition' is the one way to set the physical position of the body in the physics engine.
+    // Displace the simulator idea of position (center of root prim) to the physical position.
     public override Vector3 ForcePosition
     {
-        get { return base.ForcePosition; }
+        get {
+            OMV.Vector3 physPosition = base.ForcePosition;
+            if (PositionDisplacement != OMV.Vector3.Zero)
+            {
+                // If there is some displacement, return the physical position (center-of-mass)
+                //     location minus the displacement to give the center of the root prim.
+                OMV.Vector3 displacement = PositionDisplacement * ForceOrientation;
+                DetailLog("{0},BSPrimDisplaced.ForcePosition,get,physPos={1},disp={2},simPos={3}",
+                                LocalID, physPosition, displacement, physPosition - displacement);
+                physPosition -= displacement;
+            }
+            return physPosition;
+        }
         set
         {
             if (PositionDisplacement != OMV.Vector3.Zero)
             {
-                // The displacement is always relative to the vehicle so, when setting position,
-                //    the caller means to set the position of the root prim.
-                // This offsets the setting value to the center-of-mass and sends that to the
-                //    physics engine.
-                OMV.Vector3 displacedPos = value - (PositionDisplacement * RawOrientation);
-                DetailLog("{0},BSPrimDisplaced.ForcePosition,val={1},disp={2},newPos={3}", LocalID, value, PositionDisplacement, displacedPos);
-                base.ForcePosition = displacedPos;
+                // This value is the simulator's idea of where the prim is: the center of the root prim
+                RawPosition = value;
+
+                // Move the passed root prim postion to the center-of-mass position and set in the physics engine.
+                OMV.Vector3 displacement = PositionDisplacement * RawOrientation;
+                OMV.Vector3 displacedPos = RawPosition + displacement;
+                DetailLog("{0},BSPrimDisplaced.ForcePosition,set,simPos={1},disp={2},physPos={3}",
+                                        LocalID, RawPosition, displacement, displacedPos);
+                if (PhysBody.HasPhysicalBody)
+                {
+                    PhysScene.PE.SetTranslation(PhysBody, displacedPos, RawOrientation);
+                    ActivateIfPhysical(false);
+                }
             }
             else
             {
@@ -143,10 +172,11 @@ public class BSPrimDisplaced : BSPrim
             // These physical location 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);
-            DetailLog("{0},BSPrimDisplaced.UpdateProperties,physPos={1},disp={2},newPos={3}",
-                                    LocalID, entprop.Position, PositionDisplacement, displacedPos);
+            // Move the returned center-of-mass location to the root prim location.
+            OMV.Vector3 displacement = PositionDisplacement * entprop.Rotation;
+            OMV.Vector3 displacedPos = entprop.Position - displacement;
+            DetailLog("{0},BSPrimDisplaced.UpdateProperties,physPos={1},disp={2},simPos={3}",
+                                    LocalID, entprop.Position, displacement, displacedPos);
             entprop.Position = displacedPos;
         }