BulletSim: Move construction of compound linkset from ShapeCollection

    into LinksetCompound where it should be.
Create meshes for native shapes when part of a compound linkset because
    scale is currently per object and not per collision shape.
Don't schedule a LinksetCompound refresh if just changing properties.

3 files changed, 120 insertions, 61 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 8b97ebb..adf4aff 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -34,7 +34,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public sealed class BSLinksetCompound : BSLinkset
 {
-    // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
+    private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]";
 
     public BSLinksetCompound(BSScene scene, BSPhysObject parent)
     {
@@ -59,6 +59,12 @@ public sealed class BSLinksetCompound : BSLinkset
     //   refresh will happen once after all the other taints are applied.
     public override void Refresh(BSPhysObject requestor)
     {
+        // External request for Refresh (from BSPrim) is not necessary
+        // InternalRefresh(requestor);
+    }
+
+    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()
@@ -135,13 +141,13 @@ public sealed class BSLinksetCompound : BSLinkset
     {
         bool ret = false;
 
-        DetailLog("{0},BSLinksetCompound.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2},isRoot={3}",
-                                    child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), IsRoot(child));
+        DetailLog("{0},BSLinksetCompound.RemoveBodyDependencies,refreshIfChild,rID={1},rBody={2},isRoot={3}",
+                        child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), IsRoot(child));
 
         if (!IsRoot(child))
         {
             // Cause the current shape to be freed and the new one to be built.
-            Refresh(LinksetRoot);
+            InternalRefresh(LinksetRoot);
             ret = true;
         }
 
@@ -169,7 +175,7 @@ public sealed class BSLinksetCompound : BSLinkset
             DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
 
             // Cause constraints and assorted properties to be recomputed before the next simulation step.
-            Refresh(LinksetRoot);
+            InternalRefresh(LinksetRoot);
         }
         return;
     }
@@ -196,34 +202,68 @@ public sealed class BSLinksetCompound : BSLinkset
             else
             {
                 // Schedule a rebuild of the linkset  before the next simulation tick.
-                Refresh(LinksetRoot);
+                InternalRefresh(LinksetRoot);
             }
         }
         return;
     }
 
-
-    // Call each of the constraints that make up this linkset and recompute the
-    //    various transforms and variables. Create constraints of not created yet.
-    // Called before the simulation step to make sure the constraint based linkset
+    // Called before the simulation step to make sure the compound based linkset
     //    is all initialized.
+    // 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 void RecomputeLinksetCompound()
     {
-        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},numChildren={2}",
-                        LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), NumberOfChildren);
-
+        // Cause the root shape to be rebuilt as a compound object with just the root in it
         LinksetRoot.ForceBodyShapeRebuild(true);
 
+        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
+                        LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
+
+        ForEachMember(delegate(BSPhysObject cPrim)
+        {
+            if (!IsRoot(cPrim))
+            {
+                OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
+                OMV.Vector3 displacementPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
+                OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
+
+                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}",
+                    LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, displacementPos, displacementRot);
+
+                if (cPrim.PhysShape.isNativeShape)
+                {
+                    // Native shapes are not shared so we need to create a new one.
+                    // A mesh or hull is created because scale is not available on a native shape.
+                    //     (TODO: Bullet does have a btScaledCollisionShape. Can that be used?)
+                    BulletShape saveShape = cPrim.PhysShape;
+                    PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
+                    BulletShape newShape = cPrim.PhysShape;
+                    cPrim.PhysShape = saveShape;
+                    BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot);
+                }
+                else
+                {
+                    // For the shared shapes (meshes and hulls) just use the shape in the child
+                    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);
+                }
+            }
+            return false;
+        });
+
+
         float linksetMass = LinksetMass;
         LinksetRoot.UpdatePhysicalMassProperties(linksetMass);
 
             // DEBUG: see of inter-linkset collisions are causing problems
         // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, 
         //                     (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
-        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,end,rBody={1},linksetMass={2}",
-                            LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), linksetMass);
-
 
     }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 13aa860..de35359 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -116,6 +116,10 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     // True if initialized and ready to do simulation steps
     private bool m_initialized = false;
 
+    // Flag which is true when processing taints.
+    // Not guaranteed to be correct all the time (don't depend on this) but good for debugging.
+    public bool InTaintTime { get; private set; }
+
     // Pinned memory used to pass step information between managed and unmanaged
     private int m_maxCollisionsPerFrame;
     private CollisionDesc[] m_collisionArray;
@@ -270,6 +274,9 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         TerrainManager = new BSTerrainManager(this);
         TerrainManager.CreateInitialGroundPlaneAndTerrain();
 
+        m_log.WarnFormat("{0} Linksets implemented with {1}", LogHeader, (BSLinkset.LinksetImplementation)Params.linksetImplementation);
+
+        InTaintTime = false;
         m_initialized = true;
     }
 
@@ -707,8 +714,10 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     // here just before the physics engine is called to step the simulation.
     public void ProcessTaints()
     {
+        InTaintTime = true;
         ProcessRegularTaints();
         ProcessPostTaintTaints();
+        InTaintTime = false;
     }
 
     private void ProcessRegularTaints()
@@ -851,6 +860,17 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         }
     }
 
+    public bool AssertInTaintTime(string whereFrom)
+    {
+        if (!InTaintTime)
+        {
+            DetailLog("{0},BSScene.AssertInTaintTime,NOT IN TAINT TIME,Region={1},Where={2}", DetailLogZero, RegionName, whereFrom);
+            m_log.ErrorFormat("{0} NOT IN TAINT TIME!! Region={1}, Where={2}", LogHeader, RegionName, whereFrom);
+            Util.PrintCallStack();
+        }
+        return InTaintTime;
+    }
+
     #endregion // Taints
 
     #region Vehicles
@@ -1214,8 +1234,8 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             (s) => { return s.m_params[0].numberOfSolverIterations; },
             (s,p,l,v) => { s.m_params[0].numberOfSolverIterations = v; } ),
 
-            new ParameterDefn("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound)",
-            (float)BSLinkset.LinksetImplementation.Constraint,
+            new ParameterDefn("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)",
+            (float)BSLinkset.LinksetImplementation.Compound,
             (s,cf,p,v) => { s.m_params[0].linksetImplementation = cf.GetFloat(p,v); },
             (s) => { return s.m_params[0].linksetImplementation; },
             (s,p,l,v) => { s.m_params[0].linksetImplementation = v; } ),
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index 107befe..662b19d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -92,6 +92,8 @@ public sealed class BSShapeCollection : IDisposable
     public bool GetBodyAndShape(bool forceRebuild, BulletSim sim, BSPhysObject prim,
                     ShapeDestructionCallback shapeCallback, BodyDestructionCallback bodyCallback)
     {
+        PhysicsScene.AssertInTaintTime("BSShapeCollection.GetBodyAndShape");
+
         bool ret = false;
 
         // This lock could probably be pushed down lower but building shouldn't take long
@@ -121,7 +123,7 @@ public sealed class BSShapeCollection : IDisposable
     {
         lock (m_collectionActivityLock)
         {
-            DetailLog("{0},BSShapeCollection.ReferenceBody,newBody", body.ID, body);
+            DetailLog("{0},BSShapeCollection.ReferenceBody,newBody,body={1}", body.ID, body);
             PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.ReferenceBody", delegate()
             {
                 if (!BulletSimAPI.IsInWorld2(body.ptr))
@@ -165,7 +167,7 @@ public sealed class BSShapeCollection : IDisposable
     // Meshes and hulls for the same shape have the same hash key.
     // NOTE that native shapes are not added to the mesh list or removed.
     // Returns 'true' if this is the initial reference to the shape. Otherwise reused.
-    private bool ReferenceShape(BulletShape shape)
+    public bool ReferenceShape(BulletShape shape)
     {
         bool ret = false;
         switch (shape.type)
@@ -276,8 +278,8 @@ public sealed class BSShapeCollection : IDisposable
             if (shapeCallback != null) shapeCallback(shape);
             meshDesc.lastReferenced = System.DateTime.Now;
             Meshes[shape.shapeKey] = meshDesc;
-            DetailLog("{0},BSShapeCollection.DereferenceMesh,key={1},refCnt={2}",
-                    BSScene.DetailLogZero, shape.shapeKey.ToString("X"), meshDesc.referenceCount);
+            DetailLog("{0},BSShapeCollection.DereferenceMesh,shape={1},refCnt={2}",
+                                BSScene.DetailLogZero, shape, meshDesc.referenceCount);
 
         }
     }
@@ -297,8 +299,8 @@ public sealed class BSShapeCollection : IDisposable
 
             hullDesc.lastReferenced = System.DateTime.Now;
             Hulls[shape.shapeKey] = hullDesc;
-            DetailLog("{0},BSShapeCollection.DereferenceHull,key={1},refCnt={2}",
-                    BSScene.DetailLogZero, shape.shapeKey.ToString("X"), hullDesc.referenceCount);
+            DetailLog("{0},BSShapeCollection.DereferenceHull,shape={1},refCnt={2}",
+                    BSScene.DetailLogZero, shape, hullDesc.referenceCount);
         }
     }
 
@@ -319,8 +321,11 @@ public sealed class BSShapeCollection : IDisposable
                                         BSScene.DetailLogZero, shape.type, shape.ptr.ToString("X"));
             return;
         }
+
         int numChildren = BulletSimAPI.GetNumberOfCompoundChildren2(shape.ptr);
-        for (int ii = 0; ii < numChildren; ii++)
+        DetailLog("{0},BSShapeCollection.DereferenceCompound,shape={1},children={2}", BSScene.DetailLogZero, shape, numChildren);
+
+        for (int ii = numChildren - 1; ii >= 0; ii--)
         {
             IntPtr childShape = BulletSimAPI.RemoveChildShapeFromCompoundShapeIndex2(shape.ptr, ii);
             DereferenceAnonCollisionShape(childShape);
@@ -343,6 +348,7 @@ public sealed class BSShapeCollection : IDisposable
             shapeInfo.isNativeShape = true;
             shapeInfo.type = ShapeData.PhysicsShapeType.SHAPE_BOX; // (technically, type doesn't matter)
         }
+        DetailLog("{0},BSShapeCollection.DereferenceAnonCollisionShape,shape={1}", BSScene.DetailLogZero, shapeInfo);
 
         DereferenceShape(shapeInfo, true, null);
     }
@@ -440,23 +446,32 @@ public sealed class BSShapeCollection : IDisposable
         }
 
         // If a simple shape is not happening, create a mesh and possibly a hull.
+        if (!haveShape && pbs != null)
+        {
+            ret = CreateGeomMeshOrHull(prim, shapeCallback);
+        }
+
+        return ret;
+    }
+
+    public bool CreateGeomMeshOrHull(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
+    {
+
+        bool ret = false;
         // Note that if it's a native shape, the check for physical/non-physical is not
         //     made. Native shapes work in either case.
-        if (!haveShape && pbs != null)
+        if (prim.IsPhysical && PhysicsScene.ShouldUseHullsForPhysicalObjects)
         {
-            if (prim.IsPhysical && PhysicsScene.ShouldUseHullsForPhysicalObjects)
-            {
-                // Update prim.BSShape to reference a hull of this shape.
-                ret = GetReferenceToHull(prim,shapeCallback);
-                DetailLog("{0},BSShapeCollection.CreateGeom,hull,shape={1},key={2}",
-                                        prim.LocalID, prim.PhysShape, prim.PhysShape.shapeKey.ToString("X"));
-            }
-            else
-            {
-                ret = GetReferenceToMesh(prim, shapeCallback);
-                DetailLog("{0},BSShapeCollection.CreateGeom,mesh,shape={1},key={2}",
-                                        prim.LocalID, prim.PhysShape, prim.PhysShape.shapeKey.ToString("X"));
-            }
+            // Update prim.BSShape to reference a hull of this shape.
+            ret = GetReferenceToHull(prim,shapeCallback);
+            DetailLog("{0},BSShapeCollection.CreateGeom,hull,shape={1},key={2}",
+                                    prim.LocalID, prim.PhysShape, prim.PhysShape.shapeKey.ToString("X"));
+        }
+        else
+        {
+            ret = GetReferenceToMesh(prim, shapeCallback);
+            DetailLog("{0},BSShapeCollection.CreateGeom,mesh,shape={1},key={2}",
+                                    prim.LocalID, prim.PhysShape, prim.PhysShape.shapeKey.ToString("X"));
         }
         return ret;
     }
@@ -743,32 +758,16 @@ public sealed class BSShapeCollection : IDisposable
     // This shouldn't be to bad since most of the parts will be meshes that had been built previously.
     private bool GetReferenceToCompoundShape(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
     {
+        // Remove reference to the old shape
+        // Don't need to do this as the shape is freed when we create the new root shape below.
+        // DereferenceShape(prim.PhysShape, true, shapeCallback);
+
         BulletShape cShape = new BulletShape(
             BulletSimAPI.CreateCompoundShape2(PhysicsScene.World.ptr), ShapeData.PhysicsShapeType.SHAPE_COMPOUND);
 
-        // The prim's linkset is the source of the children.
-        // TODO: there is too much knowledge here about the internals of linksets and too much
-        //     dependency on the relationship of compound shapes and linksets (what if we want to use
-        //     compound shapes for something else?). Think through this and clean up so the
-        //     appropriate knowledge is used at the correct software levels.
-
-        // Recreate the geometry of the root prim (might have been a linkset root in the past)
+        // Create the shape for the root prim and add it to the compound shape
         CreateGeomNonSpecial(true, prim, null);
-
-        BSPhysObject rootPrim = prim.Linkset.LinksetRoot;
-
-        prim.Linkset.ForEachMember(delegate(BSPhysObject cPrim)
-        {
-            OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(rootPrim.RawOrientation);
-            OMV.Vector3 displacementPos = (cPrim.RawPosition - rootPrim.RawPosition) * invRootOrientation;
-            OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
-
-            DetailLog("{0},BSShapeCollection.GetReferenceToCompoundShape,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}",
-                prim.LocalID, cPrim.LocalID, cPrim.PhysShape.ptr.ToString("X"), displacementPos, displacementRot);
-
-            BulletSimAPI.AddChildShapeToCompoundShape2(cShape.ptr, cPrim.PhysShape.ptr, displacementPos, displacementRot);
-            return false;
-        });
+        BulletSimAPI.AddChildShapeToCompoundShape2(cShape.ptr, prim.PhysShape.ptr, OMV.Vector3.Zero, OMV.Quaternion.Identity);
 
         prim.PhysShape = cShape;