2 files changed, 36 insertions, 25 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index ad8024c..5a1b5c7 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -319,6 +319,7 @@ 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;     // disable until we get this debugged
     private void RecomputeLinksetCompound()
     {
         try
@@ -331,15 +332,26 @@ public sealed class BSLinksetCompound : BSLinkset
 
             // 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.
-            OMV.Vector3 centerOfMass = ComputeLinksetGeometricCenter();
-            OMV.Vector3 centerDisplacement = centerOfMass - LinksetRoot.RawPosition;
+            // Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass
+            OMV.Vector3 centerOfMass;
+            OMV.Vector3 centerDisplacement = OMV.Vector3.Zero;
+            if (disableCOM)                             // DEBUG DEBUG
+            {                                           // DEBUG DEBUG
+                centerOfMass = LinksetRoot.RawPosition; // DEBUG DEBUG
+                LinksetRoot.PositionDisplacement = OMV.Vector3.Zero;
+            }                                           // DEBUG DEBUG
+            else
+            {
+                centerOfMass = ComputeLinksetGeometricCenter();
+                centerDisplacement = centerOfMass - LinksetRoot.RawPosition;
 
-            // Since we're displacing the center of the shape, we need to move the body in the world
-            LinksetRoot.PositionDisplacement = centerDisplacement * LinksetRoot.RawOrientation;
+                // Since we're displacing the center of the shape, we need to move the body in the world
+                LinksetRoot.PositionDisplacement = centerDisplacement;
 
-            PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, -centerDisplacement, OMV.Quaternion.Identity, false);
-            DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}",
-                                    LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement);
+                PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, -centerDisplacement, OMV.Quaternion.Identity, false);
+                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}",
+                                        LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement);
+            }
 
             DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
                             LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
@@ -357,14 +369,15 @@ public sealed class BSLinksetCompound : BSLinkset
                     BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo;
                     if (lci == null)
                     {
-                        // Each child position and rotation is given relative to the root.
+                        // Each child position and rotation is given relative to the center-of-mass.
                         OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
-                        OMV.Vector3 displacementPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
+                        OMV.Vector3 displacementFromRoot = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
+                        OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
                         OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
 
                         // Save relative position for recomputing child's world position after moving linkset.
-                        lci = new BSLinksetCompoundInfo(memberIndex, displacementPos, displacementRot);
-                        lci.OffsetFromCenterOfMass = displacementPos - centerDisplacement;
+                        lci = new BSLinksetCompoundInfo(memberIndex, displacementFromCOM, displacementRot);
+                        lci.OffsetFromRoot = displacementFromRoot;
                         cPrim.LinksetInfo = lci;
                         DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci);
                     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 003dc54..e5f7ab7 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -297,7 +297,7 @@ public sealed class BSPrim : BSPhysObject
             */
 
             // don't do the GetObjectPosition for root elements because this function is called a zillion times.
-            // _position = PhysicsScene.PE.GetObjectPosition2(PhysicsScene.World, BSBody);
+            // _position = PhysicsScene.PE.GetObjectPosition2(PhysicsScene.World, BSBody) - PositionDisplacement;
             return _position;
         }
         set {
@@ -362,7 +362,7 @@ public sealed class BSPrim : BSPhysObject
     {
         bool ret = false;
 
-        if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(_position))
+        if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(RawPosition))
         {
             // The physical object is out of the known/simulated area.
             // Upper levels of code will handle the transition to other areas so, for
@@ -376,8 +376,11 @@ public sealed class BSPrim : BSPhysObject
         {
             DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
             float targetHeight = terrainHeight + (Size.Z / 2f);
-            // Upforce proportional to the distance away from the terrain. Correct the error in 1 sec.
-            upForce.Z = (terrainHeight - RawPosition.Z) * 1f;
+            // If the object is below ground it just has to be moved up because pushing will
+            //     not get it through the terrain
+            _position.Z = targetHeight;
+            if (!inTaintTime)
+                ForcePosition = _position;
             ret = true;
         }
 
@@ -389,20 +392,15 @@ public sealed class BSPrim : BSPhysObject
             {
                 // Upforce proportional to the distance away from the water. Correct the error in 1 sec.
                 upForce.Z = (waterHeight - RawPosition.Z) * 1f;
+
+                // Apply upforce and overcome gravity.
+                OMV.Vector3 correctionForce = upForce - PhysicsScene.DefaultGravity;
+                DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, _position, upForce, correctionForce);
+                AddForce(correctionForce, false, inTaintTime);
                 ret = true;
             }
         }
 
-        // The above code computes a force to apply to correct any out-of-bounds problems. Apply same.
-        // TODO: This should be intergrated with a geneal physics action mechanism.
-        // TODO: This should be moderated with PID'ness.
-        if (ret)
-        {
-            // Apply upforce and overcome gravity.
-            OMV.Vector3 correctionForce = upForce - PhysicsScene.DefaultGravity;
-            DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, _position, upForce, correctionForce);
-            AddForce(correctionForce, false, inTaintTime);
-        }
         return ret;
     }