BulletSim: More tweaking on center-of-mass. Almost there. Changes have no effect if LinksetOffsetCenterOfMass=false (the default).

4 files changed, 51 insertions, 101 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 30f6c8c..96626ca 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -35,62 +35,6 @@ using OMV = OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 
-    /*
-// When a child is linked, the relationship position of the child to the parent
-//    is remembered so the child's world position can be recomputed when it is
-//    removed from the linkset.
-sealed class BSLinksetCompoundInfo : BSLinksetInfo
-{
-    public int Index;
-    public OMV.Vector3 OffsetFromRoot;
-    public OMV.Vector3 OffsetFromCenterOfMass;
-    public OMV.Quaternion OffsetRot;
-    public BSLinksetCompoundInfo(int indx, OMV.Vector3 p, OMV.Quaternion r)
-    {
-        Index = indx;
-        OffsetFromRoot = p;
-        OffsetFromCenterOfMass = p;
-        OffsetRot = r;
-    }
-    // 'centerDisplacement' is the distance from the root the the center-of-mass (Bullet 'zero' of the shape)
-    public BSLinksetCompoundInfo(int indx, BSPrimLinkable root, BSPrimLinkable child, OMV.Vector3 centerDisplacement)
-    {
-        // Each child position and rotation is given relative to the center-of-mass.
-        OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(root.RawOrientation);
-        OMV.Vector3 displacementFromRoot = (child.RawPosition - root.RawPosition) * invRootOrientation;
-        OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
-        OMV.Quaternion displacementRot = child.RawOrientation * invRootOrientation;
-
-        // Save relative position for recomputing child's world position after moving linkset.
-        Index = indx;
-        OffsetFromRoot = displacementFromRoot;
-        OffsetFromCenterOfMass = displacementFromCOM;
-        OffsetRot = displacementRot;
-    }
-    public override void Clear()
-    {
-        Index = 0;
-        OffsetFromRoot = OMV.Vector3.Zero;
-        OffsetFromCenterOfMass = OMV.Vector3.Zero;
-        OffsetRot = OMV.Quaternion.Identity;
-    }
-    public override string ToString()
-    {
-        StringBuilder buff = new StringBuilder();
-        buff.Append("<i=");
-        buff.Append(Index.ToString());
-        buff.Append(",p=");
-        buff.Append(OffsetFromRoot.ToString());
-        buff.Append(",m=");
-        buff.Append(OffsetFromCenterOfMass.ToString());
-        buff.Append(",r=");
-        buff.Append(OffsetRot.ToString());
-        buff.Append(">");
-        return buff.ToString();
-    }
-};
-     */
-
 public sealed class BSLinksetCompound : BSLinkset
 {
     private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]";
@@ -151,7 +95,9 @@ public sealed class BSLinksetCompound : BSLinkset
     public override bool MakeStatic(BSPrimLinkable child)
     {
         bool ret = false;
+
         DetailLog("{0},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
+        child.ClearDisplacement();
         if (IsRoot(child))
         {
             // Schedule a rebuild to verify that the root shape is set to the real shape.
@@ -364,16 +310,28 @@ public sealed class BSLinksetCompound : BSLinkset
             int memberIndex = 1;
             ForEachMember(delegate(BSPrimLinkable cPrim)
             {
-                // Root shape is always index zero.
-                cPrim.LinksetChildIndex = IsRoot(cPrim) ? 0 : memberIndex;
+                if (IsRoot(cPrim))
+                {
+                    // Root shape is always index zero.
+                    cPrim.LinksetChildIndex = 0;
+                }
+                else
+                {
+                    cPrim.LinksetChildIndex = memberIndex;
+                    memberIndex++;
+                }
 
                 // Get a reference to the shape of the child and add 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;
                 OMV.Quaternion offsetRot = OMV.Quaternion.Normalize(cPrim.RawOrientation) * invRootOrientation;
+
+                // Add the child shape to the compound shape being built
                 m_physicsScene.PE.AddChildShapeToCompoundShape(linksetShape.physShapeInfo, childShape.physShapeInfo, offsetPos, offsetRot);
                 DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChild,indx={1},cShape={2},offPos={3},offRot={4}",
-                                    LinksetRoot.LocalID, memberIndex, childShape, offsetPos, offsetRot);
+                                    LinksetRoot.LocalID, cPrim.LinksetChildIndex, childShape, offsetPos, offsetRot);
 
                 // Since we are borrowing the shape of the child, disable the origional child body
                 if (!IsRoot(cPrim))
@@ -385,8 +343,6 @@ public sealed class BSLinksetCompound : BSLinkset
                     cPrim.PhysBody.collisionType = CollisionType.LinksetChild;
                 }
 
-                memberIndex++;
-
                 return false;   // 'false' says to move onto the next child in the list
             });
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
index 9a05349..a11bca0 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -51,7 +51,7 @@ public class BSPrimDisplaced : BSPrim
     //    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
+    // This routine works by capturing ForcePosition and
     //    adjusting the simulator values (being set) into the physical values.
     //    The conversion is also done in the opposite direction (physical origin -> simulator origin).
     //
@@ -60,8 +60,6 @@ public class BSPrimDisplaced : BSPrim
     //    class.
 
     public virtual OMV.Vector3 PositionDisplacement { get; set; }
-    public virtual OMV.Quaternion OrientationDisplacement { get; set; }
-    public virtual OMV.Quaternion OrientationDisplacementOrigInv { get; set; }
 
     public BSPrimDisplaced(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
                        OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
@@ -70,15 +68,17 @@ public class BSPrimDisplaced : BSPrim
         ClearDisplacement();
     }
 
+    // Clears any center-of-mass displacement introduced by linksets, etc.
+    // Does not clear the displacement set by the user.
     public void ClearDisplacement()
     {
-        PositionDisplacement = OMV.Vector3.Zero;
-        OrientationDisplacement = OMV.Quaternion.Identity;
+        SetEffectiveCenterOfMassDisplacement(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).
+    // Called at taint time.
     public virtual void SetEffectiveCenterOfMassDisplacement(Vector3 centerOfMassDisplacement)
     {
         Vector3 comDisp;
@@ -87,21 +87,17 @@ public class BSPrimDisplaced : BSPrim
         else
             comDisp = centerOfMassDisplacement;
 
+        if (comDisp.ApproxEquals(Vector3.Zero, 0.01f) )
+            comDisp = Vector3.Zero;
+
         DetailLog("{0},BSPrimDisplaced.SetEffectiveCenterOfMassDisplacement,userSet={1},comDisp={2}",
                                     LocalID, UserSetCenterOfMassDisplacement.HasValue, comDisp);
-        if (comDisp == Vector3.Zero)
+        if ( comDisp != PositionDisplacement )
         {
-            // If there is no diplacement. Things get reset.
-            PositionDisplacement = OMV.Vector3.Zero;
-            OrientationDisplacement = OMV.Quaternion.Identity;
-        }
-        else
-        {
-            // Remember the displacement from root as well as the origional rotation of the
-            //    new center-of-mass.
+            // Displacement setting is changing.
+            // The relationship between the physical object and simulated object must be aligned.
             PositionDisplacement = comDisp;
-            OrientationDisplacement = Quaternion.Normalize(RawOrientation);
-            OrientationDisplacementOrigInv = Quaternion.Inverse(OrientationDisplacement);
+            this.ForcePosition = RawPosition;
         }
     }
 
@@ -112,7 +108,11 @@ public class BSPrimDisplaced : BSPrim
         {
             if (PositionDisplacement != OMV.Vector3.Zero)
             {
-                OMV.Vector3 displacedPos = value - (PositionDisplacement * (OrientationDisplacementOrigInv * RawOrientation));
+                // 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;
             }
@@ -123,26 +123,12 @@ public class BSPrimDisplaced : BSPrim
         }
     }
 
-    public override Quaternion ForceOrientation
-    {
-        get { return base.ForceOrientation; }
-        set
-        {
-            // Changing orientation also changes the position of the center-of-mass since
-            //    the orientation passed is for a rotation around the root prim's center.
-            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?
+    // These are also overridden by BSPrimLinkable if the prim can be part of a linkset
     public override OMV.Vector3 CenterOfMass
     {
         get { return RawPosition; }
     }
 
-    // Is this used?
     public override OMV.Vector3 GeometricCenter
     {
         get { return RawPosition; }
@@ -151,18 +137,17 @@ public class BSPrimDisplaced : BSPrim
     public override void UpdateProperties(EntityProperties entprop)
     {
         // Undo any center-of-mass displacement that might have been done.
-        if (PositionDisplacement != OMV.Vector3.Zero || OrientationDisplacement != OMV.Quaternion.Identity)
+        if (PositionDisplacement != OMV.Vector3.Zero)
         {
-            // The origional shape was offset from 'zero' by PositionDisplacement and rotated by OrientationDisplacement.
-            // These physical locations and rotation must be back converted to be centered around the displaced
+            // The origional shape was offset from 'zero' by PositionDisplacement.
+            // 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 * OrientationDisplacementOrigInv));
-            
-            DetailLog("{0},BSPrimDisplaced.ForcePosition,physPos={1},disp={2},newPos={3}", LocalID, entprop.Position, PositionDisplacement, displacedPos);
+            OMV.Vector3 displacedPos = entprop.Position + (PositionDisplacement * entprop.Rotation);
+            DetailLog("{0},BSPrimDisplaced.UpdateProperties,physPos={1},disp={2},newPos={3}",
+                                    LocalID, entprop.Position, PositionDisplacement, displacedPos);
             entprop.Position = displacedPos;
-            // entprop.Rotation = something;
         }
 
         base.UpdateProperties(entprop);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
index 6d7de35..7058646 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
@@ -134,6 +134,16 @@ public class BSPrimLinkable : BSPrimDisplaced
         get { return Linkset.LinksetMass; }
     }
 
+    public override OMV.Vector3 CenterOfMass
+    {
+        get { return Linkset.CenterOfMass; }
+    }
+
+    public override OMV.Vector3 GeometricCenter
+    {
+        get { return Linkset.GeometricCenter; }
+    }
+
     // Refresh the linkset structure and parameters when the prim's physical parameters are changed.
     public override void UpdatePhysicalParameters()
     {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 1645c98..e56a6f6 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -785,7 +785,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             {
                 // The simulation of the time interval took less than realtime.
                 // Do a sleep for the rest of realtime.
-                DetailLog("{0},BulletSPluginPhysicsThread,sleeping={1}", BSScene.DetailLogZero, simulationTimeVsRealtimeDifferenceMS);
                 Thread.Sleep(simulationTimeVsRealtimeDifferenceMS);
             }
             else