1 files changed, 96 insertions, 17 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index bc9f9be..fe5b5e3 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -32,6 +32,31 @@ 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 OMV.Vector3 OffsetPos;
+    public OMV.Quaternion OffsetRot;
+    public BSLinksetCompoundInfo(OMV.Vector3 p, OMV.Quaternion r)
+    {
+        OffsetPos = p;
+        OffsetRot = r;
+    }
+    public override string ToString()
+    {
+        StringBuilder buff = new StringBuilder();
+        buff.Append("<p=");
+        buff.Append(OffsetPos.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]";
@@ -44,6 +69,7 @@ public sealed class BSLinksetCompound : BSLinkset
     // For compound implimented linksets, if there are children, use compound shape for the root.
     public override BSPhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
     { 
+        // Returning 'unknown' means we don't have a preference.
         BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
         if (IsRoot(requestor) && HasAnyChildren)
         {
@@ -63,10 +89,10 @@ public sealed class BSLinksetCompound : BSLinkset
         // InternalRefresh(requestor);
     }
 
+    // Schedule a refresh to happen after all the other taint processing.
     private void InternalRefresh(BSPhysObject requestor)
     {
         DetailLog("{0},BSLinksetCompound.Refresh,schedulingRefresh,requestor={1}", LinksetRoot.LocalID, requestor.LocalID);
-        // Queue to happen after all the other taint processing
         PhysicsScene.PostTaintObject("BSLinksetCompound.Refresh", requestor.LocalID, delegate()
         {
             if (IsRoot(requestor) && HasAnyChildren)
@@ -108,7 +134,7 @@ public sealed class BSLinksetCompound : BSLinkset
         {
             // The non-physical children can come back to life.
             BulletSimAPI.RemoveFromCollisionFlags2(child.PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
-            // Don't force activation so setting of DISABLE_SIMULATION can stay.
+            // Don't force activation so setting of DISABLE_SIMULATION can stay if used.
             BulletSimAPI.Activate2(child.PhysBody.ptr, false);
             ret = true;
         }
@@ -146,6 +172,10 @@ public sealed class BSLinksetCompound : BSLinkset
 
         if (!IsRoot(child))
         {
+            // Because it is a convenient time, recompute child world position and rotation based on
+            //    its position in the linkset.
+            RecomputeChildWorldPosition(child, true);
+
             // Cause the current shape to be freed and the new one to be built.
             InternalRefresh(LinksetRoot);
             ret = true;
@@ -154,6 +184,42 @@ public sealed class BSLinksetCompound : BSLinkset
         return ret;
     }
 
+    // When the linkset is built, the child shape is added
+    //    to the compound shape relative to the root shape. The linkset then moves around but
+    //    this does not move the actual child prim. The child prim's location must be recomputed
+    //    based on the location of the root shape.
+    private void RecomputeChildWorldPosition(BSPhysObject child, bool inTaintTime)
+    {
+        BSLinksetCompoundInfo lci = child.LinksetInfo as BSLinksetCompoundInfo;
+        if (lci != null)
+        {
+            if (inTaintTime)
+            {
+                OMV.Vector3 oldPos = child.RawPosition;
+                child.ForcePosition = LinksetRoot.RawPosition + lci.OffsetPos;
+                child.ForceOrientation = LinksetRoot.RawOrientation * lci.OffsetRot;
+                DetailLog("{0},BSLinksetCompound.RecomputeChildWorldPosition,oldPos={1},lci={2},newPos={3}",
+                                            child.LocalID, oldPos, lci, child.RawPosition);
+            }
+            else
+            {
+                // TaintedObject is not used here so the raw position is set now and not at taint-time.
+                child.Position = LinksetRoot.RawPosition + lci.OffsetPos;
+                child.Orientation = LinksetRoot.RawOrientation * lci.OffsetRot;
+            }
+        }
+        else
+        {
+            // This happens when children have been added to the linkset but the linkset
+            //     has not been constructed yet. So like, at taint time, adding children to a linkset
+            //     and then changing properties of the children (makePhysical, for instance)
+            //     but the post-print action of actually rebuilding the linkset has not yet happened.
+            // PhysicsScene.Logger.WarnFormat("{0} Restoring linkset child position failed because of no relative position computed. ID={1}",
+            //                                 LogHeader, child.LocalID);
+            DetailLog("{0},BSLinksetCompound.recomputeChildWorldPosition,noRelativePositonInfo", child.LocalID);
+        }
+    }
+
     // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
     // this routine will restore the removed constraints.
     // Called at taint-time!!
@@ -192,8 +258,7 @@ public sealed class BSLinksetCompound : BSLinkset
                             child.LocalID, child.PhysBody.ptr.ToString("X"));
 
             // Cause the child's body to be rebuilt and thus restored to normal operation
-            // TODO: position and rotation must be restored because the child could have moved
-            //    based on the linkset.
+            RecomputeChildWorldPosition(child, false);
             child.ForceBodyShapeRebuild(false);
 
             if (!HasAnyChildren)
@@ -228,43 +293,57 @@ public sealed class BSLinksetCompound : BSLinkset
         {
             if (!IsRoot(cPrim))
             {
-                // Each child position and rotation is given relative to the root.
-                OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
-                OMV.Vector3 displacementPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
-                OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
+                // Compute the displacement of the child from the root of the linkset.
+                // This info is saved in the child prim so the relationship does not
+                //    change over time and the new child position can be computed
+                //    when the linkset is being disassembled (the linkset may have moved).
+                BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo;
+                if (lci == null)
+                {
+                    // Each child position and rotation is given relative to the root.
+                    OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
+                    OMV.Vector3 displacementPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
+                    OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
+
+                    // Save relative position for recomputing child's world position after moving linkset.
+                    lci = new BSLinksetCompoundInfo(displacementPos, displacementRot);
+                    cPrim.LinksetInfo = lci;
+                    DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci);
+                }
 
                 DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}",
-                    LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, displacementPos, displacementRot);
+                    LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, lci.OffsetPos, lci.OffsetRot);
 
                 if (cPrim.PhysShape.isNativeShape)
                 {
-                    // A native shape is turning into a null collision shape because native
+                    // A native shape is turning into a hull collision shape because native
                     //    shapes are not shared so we have to hullify it so it will be tracked
                     //    and freed at the correct time. This also solves the scaling problem
                     //    (native shapes scaled but hull/meshes are assumed to not be).
+                    // TODO: decide of the native shape can just be used in the compound shape.
+                    //    Use call to CreateGeomNonSpecial().
                     BulletShape saveShape = cPrim.PhysShape;
-                    cPrim.PhysShape.ptr = IntPtr.Zero;  // Don't let the create free the child's shape
+                    cPrim.PhysShape.Clear();        // Don't let the create free the child's shape
+                    // PhysicsScene.Shapes.CreateGeomNonSpecial(true, cPrim, null);
                     PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
                     BulletShape newShape = cPrim.PhysShape;
                     cPrim.PhysShape = saveShape;
-                    BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot);
+                    BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, newShape.ptr, lci.OffsetPos , lci.OffsetRot);
                 }
                 else
                 {
                     // For the shared shapes (meshes and hulls), just use the shape in the child.
+                    // The reference count added here will be decremented when the compound shape
+                    //     is destroyed in BSShapeCollection (the child shapes are looped over and dereferenced).
                     if (PhysicsScene.Shapes.ReferenceShape(cPrim.PhysShape))
                     {
                         PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}",
                                             LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape);
                     }
-                    BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, cPrim.PhysShape.ptr, displacementPos, displacementRot);
+                    BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, cPrim.PhysShape.ptr, lci.OffsetPos , lci.OffsetRot);
                 }
             }
 
-            // TODO: need to phantomize the child prims left behind.
-            //     Maybe just destroy the children bodies and shapes and have them rebuild on unlink.
-            //     Selection/deselection might cause way too many build/destructions esp. for LARGE linksets.
-
             return false;   // 'false' says to move onto the next child in the list
         });