BulletSim: add ResetBroadphasePool and ResetConstraintSolver diagnostic

functions. If values set from console, the functions are called. Looking
for why the collision pools fill up with unnecessary stuff.

4 files changed, 73 insertions, 50 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
index 8c6e7d6..45ecf56 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
@@ -530,12 +530,12 @@ public override void SetForceUpdateAllAabbs(BulletWorld world, bool force)
 // btDynamicsWorld entries
 public override bool AddObjectToWorld(BulletWorld world, BulletBody obj)
 {
-    // Bullet resets several variables when an object is added to the world.
-    //   Gravity is reset to world default depending on the static/dynamic
-    //   type. Of course, the collision flags in the broadphase proxy are initialized to default.
     BulletWorldUnman worldu = world as BulletWorldUnman;
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
 
+    // Bullet resets several variables when an object is added to the world.
+    //   Gravity is reset to world default depending on the static/dynamic
+    //   type. Of course, the collision flags in the broadphase proxy are initialized to default.
     Vector3 origGrav = BSAPICPP.GetGravity2(bodyu.ptr);
 
     bool ret = BSAPICPP.AddObjectToWorld2(worldu.ptr, bodyu.ptr);
@@ -1259,6 +1259,16 @@ public override void DumpPhysicsStatistics(BulletWorld world)
     BulletWorldUnman worldu = world as BulletWorldUnman;
     BSAPICPP.DumpPhysicsStatistics2(worldu.ptr);
 }
+public override void ResetBroadphasePool(BulletWorld world)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.ResetBroadphasePool(worldu.ptr);
+}
+public override void ResetConstraintSolver(BulletWorld world)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.ResetConstraintSolver(worldu.ptr);
+}
 
 // =====================================================================================
 // =====================================================================================
@@ -1832,6 +1842,12 @@ public static extern void DumpAllInfo2(IntPtr sim);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void DumpPhysicsStatistics2(IntPtr sim);
 
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ResetBroadphasePool(IntPtr sim);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ResetConstraintSolver(IntPtr sim);
+
 }
 
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
index 30a7bee..0c7f315 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
@@ -232,21 +232,25 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override bool AddObjectToWorld(BulletWorld pWorld, BulletBody pBody)
     {
+        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        CollisionObject cbody = ((BulletBodyXNA)pBody).body;
+        RigidBody rbody = cbody as RigidBody;
+
         // Bullet resets several variables when an object is added to the world. In particular,
         //   BulletXNA resets position and rotation. Gravity is also reset depending on the static/dynamic
         //   type. Of course, the collision flags in the broadphase proxy are initialized to default.
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-
-        IndexedMatrix origPos = body.GetWorldTransform();
-        IndexedVector3 origGrav = body.GetGravity();
-
-        //if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE))
-
-        world.AddRigidBody(body);
-
-        body.SetWorldTransform(origPos);
-        body.SetGravity(origGrav);
+        IndexedMatrix origPos = cbody.GetWorldTransform();
+        if (rbody != null)
+        {
+            IndexedVector3 origGrav = rbody.GetGravity();
+            world.AddRigidBody(rbody);
+            rbody.SetGravity(origGrav);
+        }
+        else
+        {
+            world.AddCollisionObject(rbody);
+        }
+        cbody.SetWorldTransform(origPos);
 
         pBody.ApplyCollisionMask(pWorld.physicsScene);
 
@@ -773,35 +777,6 @@ private sealed class BulletConstraintXNA : BulletConstraint
         body.ApplyTorqueImpulse(ref fSum);
     }
 
-    public override void DumpRigidBody(BulletWorld p, BulletBody p_2)
-    {
-        //TODO:
-    }
-
-    public override void DumpCollisionShape(BulletWorld p, BulletShape p_2)
-    {
-        //TODO:
-    }
-    public override void DumpConstraint(BulletWorld world, BulletConstraint constrain)
-    {
-        //TODO:
-    }
-
-    public override void DumpActivationInfo(BulletWorld world)
-    {
-        //TODO:
-    }
-
-    public override void DumpAllInfo(BulletWorld world)
-    {
-        //TODO:
-    }
-
-    public override void DumpPhysicsStatistics(BulletWorld world)
-    {
-        //TODO:
-    }
-
     public override void DestroyObject(BulletWorld p, BulletBody p_2)
     {
         //TODO:
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
index 8ad78ca..befb076 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
@@ -646,17 +646,21 @@ public abstract float GetMargin(BulletShape shape);
 
 // =====================================================================================
 // Debugging
-public abstract void DumpRigidBody(BulletWorld sim, BulletBody collisionObject);
+public virtual void DumpRigidBody(BulletWorld sim, BulletBody collisionObject) { }
 
-public abstract void DumpCollisionShape(BulletWorld sim, BulletShape collisionShape);
+public virtual void DumpCollisionShape(BulletWorld sim, BulletShape collisionShape) { }
 
-public abstract void DumpConstraint(BulletWorld sim, BulletConstraint constrain);
+public virtual void DumpConstraint(BulletWorld sim, BulletConstraint constrain) { }
 
-public abstract void DumpActivationInfo(BulletWorld sim);
+public virtual void DumpActivationInfo(BulletWorld sim) { }
 
-public abstract void DumpAllInfo(BulletWorld sim);
+public virtual void DumpAllInfo(BulletWorld sim) { }
 
-public abstract void DumpPhysicsStatistics(BulletWorld sim);
+public virtual void DumpPhysicsStatistics(BulletWorld sim) { }
+
+public virtual void ResetBroadphasePool(BulletWorld sim) { }
+
+public virtual void ResetConstraintSolver(BulletWorld sim) { }
 
 };
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index 69ac8cd..b9bd0bf 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -497,6 +497,16 @@ public static class BSParam
             (s,cf,p,v) => { s.PhysicsMetricDumpFrames = cf.GetFloat(p, (int)v); },
             (s) => { return (float)s.PhysicsMetricDumpFrames; },
             (s,p,l,v) => { s.PhysicsMetricDumpFrames = (int)v; } ),
+        new ParameterDefn("ResetBroadphasePool", "Setting this is any value resets the broadphase collision pool",
+            0f,
+            (s,cf,p,v) => { ; },
+            (s) => { return 0f; },
+            (s,p,l,v) => { BSParam.ResetBroadphasePoolTainted(s, v); } ),
+        new ParameterDefn("ResetConstraintSolver", "Setting this is any value resets the constraint solver",
+            0f,
+            (s,cf,p,v) => { ; },
+            (s) => { return 0f; },
+            (s,p,l,v) => { BSParam.ResetConstraintSolverTainted(s, v); } ),
     };
 
     // Convert a boolean to our numeric true and false values
@@ -511,6 +521,24 @@ public static class BSParam
         return (b == ConfigurationParameters.numericTrue ? true : false);
     }
 
+    private static void ResetBroadphasePoolTainted(BSScene pPhysScene, float v)
+    {
+        BSScene physScene = pPhysScene;
+        physScene.TaintedObject("BSParam.ResetBroadphasePoolTainted", delegate()
+        {
+            physScene.PE.ResetBroadphasePool(physScene.World);
+        });
+    }
+
+    private static void ResetConstraintSolverTainted(BSScene pPhysScene, float v)
+    {
+        BSScene physScene = pPhysScene;
+        physScene.TaintedObject("BSParam.ResetConstraintSolver", delegate()
+        {
+            physScene.PE.ResetConstraintSolver(physScene.World);
+        });
+    }
+
     // Search through the parameter definitions and return the matching
     //    ParameterDefn structure.
     // Case does not matter as names are compared after converting to lower case.