5 files changed, 50 insertions, 17 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index e967dfc..e3ce7fb 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -395,7 +395,8 @@ public sealed class BSLinksetCompound : BSLinkset
     // Constraint linksets are rebuilt every time.
     // Note that this works for rebuilding just the root after a linkset is taken apart.
     // Called at taint time!!
-    private bool disableCOM = true;     // DEBUG DEBUG: disable until we get this debugged
+    private bool UseBulletSimRootOffsetHack = false;
+    private bool disableCOM = true; // For basic linkset debugging, turn off the center-of-mass setting
     private void RecomputeLinksetCompound()
     {
         if (!LinksetRoot.IsPhysicallyActive)
@@ -428,11 +429,19 @@ public sealed class BSLinksetCompound : BSLinkset
 
             // 'centerDisplacement' is the value to subtract from children to give physical offset position
             OMV.Vector3 centerDisplacement = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
-            LinksetRoot.SetEffectiveCenterOfMassW(centerDisplacement);
-
-            // TODO: add phantom root shape to be the center-of-mass
+            if (UseBulletSimRootOffsetHack || disableCOM)
+            {
+                centerDisplacement = OMV.Vector3.Zero;
+                LinksetRoot.ClearDisplacement();
+            }
+            else
+            {
+                LinksetRoot.SetEffectiveCenterOfMassDisplacement(centerDisplacement);
+            }
+            DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,rootPos={1},com={2},comDisp={3}",
+                                LinksetRoot.LocalID, LinksetRoot.RawPosition, centerOfMassW, centerDisplacement);
 
-            // Add a shape for each of the other children in the linkset
+            // Add the shapes of all the components of the linkset
             int memberIndex = 1;
             ForEachMember(delegate(BSPrimLinkable cPrim)
             {
@@ -449,8 +458,8 @@ public sealed class BSLinksetCompound : BSLinkset
                 OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement;
                 OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
                 m_physicsScene.PE.AddChildShapeToCompoundShape(LinksetShape.physShapeInfo, childShape.physShapeInfo, offsetPos, offsetRot);
-                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChild,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}",
-                                    LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot);
+                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChild,indx={1}cShape={2},offPos={3},offRot={4}",
+                                    LinksetRoot.LocalID, memberIndex, cPrim.PhysShape, offsetPos, offsetRot);
 
                 memberIndex++;
 
@@ -463,13 +472,21 @@ public sealed class BSLinksetCompound : BSLinkset
             m_physicsScene.PE.RemoveObjectFromWorld(m_physicsScene.World, LinksetRoot.PhysBody);
             m_physicsScene.PE.SetCollisionShape(m_physicsScene.World, LinksetRoot.PhysBody, LinksetShape.physShapeInfo);
             m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, LinksetRoot.PhysBody);
+            DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addBody,body={1},shape={2}",
+                                        LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetShape);
 
             // With all of the linkset packed into the root prim, it has the mass of everyone.
             LinksetMass = ComputeLinksetMass();
             LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);
 
-            // Enable the physical position updator to return the position and rotation of the root shape
-            m_physicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
+            if (UseBulletSimRootOffsetHack)
+            {
+                // 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
+                //     center-of-mass offset.
+                m_physicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
+            }
         }
         finally
         {
@@ -477,7 +494,7 @@ public sealed class BSLinksetCompound : BSLinkset
         }
 
         // See that the Aabb surrounds the new shape
-        m_physicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape.physShapeInfo);
+        m_physicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetShape.physShapeInfo);
     }
 }
 }
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 28d4bd7..7eba851 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -259,7 +259,8 @@ public abstract class BSPhysObject : PhysicsActor
 
     // The user can optionally set the center of mass. The user's setting will override any
     //    computed center-of-mass (like in linksets).
-    public OMV.Vector3? UserSetCenterOfMass { get; set; }
+    // Note this is a displacement from the root's coordinates. Zero means use the root prim as center-of-mass.
+    public OMV.Vector3? UserSetCenterOfMassDisplacement { get; set; }
 
     public OMV.Vector3 LockedAxis { get; set; } // zero means locked. one means free.
     public readonly OMV.Vector3 LockedAxisFree = new OMV.Vector3(1f, 1f, 1f);  // All axis are free
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
index f1c3b5c..f5ee671 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -78,14 +78,16 @@ public class BSPrimDisplaced : BSPrim
     // 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)
+    public virtual void SetEffectiveCenterOfMassDisplacement(Vector3 centerOfMassDisplacement)
     {
         Vector3 comDisp;
-        if (UserSetCenterOfMass.HasValue)
-            comDisp = (OMV.Vector3)UserSetCenterOfMass;
+        if (UserSetCenterOfMassDisplacement.HasValue)
+            comDisp = (OMV.Vector3)UserSetCenterOfMassDisplacement;
         else
             comDisp = centerOfMassDisplacement;
 
+        DetailLog("{0},BSPrimDisplaced.SetEffectiveCenterOfMassDisplacement,userSet={1},comDisp={2}",
+                                    LocalID, UserSetCenterOfMassDisplacement.HasValue, comDisp);
         if (comDisp == Vector3.Zero)
         {
             // If there is no diplacement. Things get reset.
@@ -107,9 +109,15 @@ public class BSPrimDisplaced : BSPrim
         set
         {
             if (PositionDisplacement != OMV.Vector3.Zero)
-                base.ForcePosition = value - (PositionDisplacement * RawOrientation);
+            {
+                OMV.Vector3 displacedPos = value - (PositionDisplacement * RawOrientation);
+                DetailLog("{0},BSPrimDisplaced.ForcePosition,val={1},disp={2},newPos={3}", LocalID, value, PositionDisplacement, displacedPos);
+                base.ForcePosition = displacedPos;
+            }
             else
+            {
                 base.ForcePosition = value;
+            }
         }
     }
 
@@ -118,6 +126,7 @@ public class BSPrimDisplaced : BSPrim
         get { return base.ForceOrientation; }
         set
         {
+            // TODO:
             base.ForceOrientation = value;
         }
     }
@@ -143,7 +152,10 @@ public class BSPrimDisplaced : BSPrim
         {
             // Correct for any rotation around the center-of-mass
             // TODO!!!
-            entprop.Position = entprop.Position + (PositionDisplacement * entprop.Rotation);
+            
+            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/BSShapes.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
index f0c9fd5..3faa484 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
@@ -117,7 +117,7 @@ public abstract class BSShape
         StringBuilder buff = new StringBuilder();
         buff.Append("<t=");
         buff.Append(ShapeType.ToString());
-        buff.Append("<p=");
+        buff.Append(",p=");
         buff.Append(AddrString);
         buff.Append(",c=");
         buff.Append(referenceCount.ToString());
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
index 559a73f..5792ae6 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -52,6 +52,9 @@ One sided meshes? Should terrain be built into a closed shape?
 
 VEHICLES TODO LIST:
 =================================================
+LINEAR_MOTOR_DIRECTION values should be clamped to reasonable numbers.
+        What are the limits in SL?
+        Same for other velocity settings.
 UBit improvements to remove rubber-banding of avatars sitting on vehicle child prims:
         https://github.com/UbitUmarov/Ubit-opensim
 Border crossing with linked vehicle causes crash