5 files changed, 70 insertions, 29 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);
             }
         }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index a0d5c42..738e2d0 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -90,6 +90,8 @@ public abstract class BSPhysObject : PhysicsActor
         PhysBody = new BulletBody(localID);
         PhysShape = new BSShapeNull();
 
+        UserSetCenterOfMassDisplacement = null;
+
         PrimAssetState = PrimAssetCondition.Unknown;
 
         // Default material type. Also sets Friction, Restitution and Density.
@@ -180,6 +182,7 @@ public abstract class BSPhysObject : PhysicsActor
         Material = (MaterialAttributes.Material)material;
 
         // Setting the material sets the material attributes also.
+        // TODO: decide if this is necessary -- the simulator does this.
         MaterialAttributes matAttrib = BSMaterials.GetAttributes(Material, false);
         Friction = matAttrib.friction;
         Restitution = matAttrib.restitution;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index f0858ca..ce4c3da 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -802,6 +802,7 @@ public class BSPrim : BSPhysObject
     //     isSolid: other objects bounce off of this object
     //     isVolumeDetect: other objects pass through but can generate collisions
     //     collisionEvents: whether this object returns collision events
+    // NOTE: overloaded by BSPrimLinkable to also update linkset physical parameters.
     public virtual void UpdatePhysicalParameters()
     {
         if (!PhysBody.HasPhysicalBody)
@@ -1532,6 +1533,8 @@ public class BSPrim : BSPhysObject
 
     // The physics engine says that properties have updated. Update same and inform
     // the world that things have changed.
+    // NOTE: BSPrim.UpdateProperties is overloaded by BSPrimLinkable which modifies updates from root and children prims.
+    // NOTE: BSPrim.UpdateProperties is overloaded by BSPrimDisplaced which handles mapping physical position to simulator position.
     public override void UpdateProperties(EntityProperties entprop)
     {
         // Let anyone (like the actors) modify the updated properties before they are pushed into the object and the simulator.
@@ -1567,8 +1570,6 @@ public class BSPrim : BSPhysObject
         LastEntityProperties = CurrentEntityProperties;
         CurrentEntityProperties = entprop;
 
-        // Note that BSPrim can be overloaded by BSPrimLinkable which controls updates from root and children prims.
-        
         PhysScene.PostUpdate(this);
     }
 }
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;
         }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
index 7058646..1fbcfcc 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
@@ -160,10 +160,13 @@ public class BSPrimLinkable : BSPrimDisplaced
     protected override void MakeDynamic(bool makeStatic)
     {
         base.MakeDynamic(makeStatic);
-        if (makeStatic)
-            Linkset.MakeStatic(this);
-        else
-            Linkset.MakeDynamic(this);
+        if (Linkset != null)    // null can happen during initialization
+        {
+            if (makeStatic)
+                Linkset.MakeStatic(this);
+            else
+                Linkset.MakeDynamic(this);
+        }
     }
 
     // Body is being taken apart. Remove physical dependencies and schedule a rebuild.