BulletSim: add parameters and API calls for setting ERP and CFM.

Set ERP and CFM in linkset constraints.
Reorder rebuilding of object bodies so they are not rebuilt everytime
   something is linked and unlinked.

6 files changed, 164 insertions, 29 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index fbb9e21..07f5a21 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -50,7 +50,8 @@ public class BSConstraint : IDisposable
         m_body2 = obj2;
         m_constraint = new BulletConstraint(BulletSimAPI.CreateConstraint2(m_world.Ptr, m_body1.Ptr, m_body2.Ptr,
                             frame1, frame1rot,
-                            frame2, frame2rot));
+                            frame2, frame2rot,
+                            true /*useLinearReferenceFrameA*/, true /*disableCollisionsBetweenLinkedBodies*/));
         m_enabled = true;
     }
 
@@ -83,6 +84,15 @@ public class BSConstraint : IDisposable
         return ret;
     }
 
+    public bool SetCFMAndERP(float cfm, float erp)
+    {
+        bool ret = false;
+        BulletSimAPI.SetConstraintParam2(m_constraint.Ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL);
+        BulletSimAPI.SetConstraintParam2(m_constraint.Ptr, ConstraintParams.BT_CONSTRAINT_STOP_ERP, erp, ConstraintParamAxis.AXIS_ALL);
+        BulletSimAPI.SetConstraintParam2(m_constraint.Ptr, ConstraintParams.BT_CONSTRAINT_CFM, cfm, ConstraintParamAxis.AXIS_ALL);
+        return ret;
+    }
+
     public bool UseFrameOffset(bool useOffset)
     {
         bool ret = false;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
index a2650fb..c88e645 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
@@ -83,7 +83,8 @@ public class BSConstraintCollection : IDisposable
         return true;
     }
 
-    // Get the constraint between two bodies. There can be only one the way we're using them.
+    // Get the constraint between two bodies. There can be only one.
+    // Return 'true' if a constraint was found.
     public bool TryGetConstraint(BulletBody body1, BulletBody body2, out BSConstraint returnConstraint)
     {
         bool found = false;
@@ -105,6 +106,9 @@ public class BSConstraintCollection : IDisposable
         return found;
     }
 
+    // Remove any constraint between the passed bodies.
+    // Presumed there is only one such constraint possible.
+    // Return 'true' if a constraint was found and destroyed.
     public bool RemoveAndDestroyConstraint(BulletBody body1, BulletBody body2)
     {
         // return BulletSimAPI.RemoveConstraint(m_world.ID, obj1.ID, obj2.ID);
@@ -125,6 +129,8 @@ public class BSConstraintCollection : IDisposable
         return ret;
     }
 
+    // Remove all constraints that reference the passed body.
+    // Return 'true' if any constraints were destroyed.
     public bool RemoveAndDestroyConstraint(BulletBody body1)
     {
         // return BulletSimAPI.RemoveConstraintByID(m_world.ID, obj.ID);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 3bc2100..6f8430c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -117,10 +117,50 @@ public class BSLinkset
     }
 
     // An existing linkset had one of its members rebuilt or something.
-    // Undo all the physical linking and rebuild the physical linkset.
-    public bool RefreshLinkset(BSPrim requestor)
+    // Go through the linkset and rebuild the pointers to the bodies of the linkset members.
+    public BSLinkset RefreshLinkset(BSPrim requestor)
     {
-        return true;
+        BSLinkset ret = requestor.Linkset;
+
+        lock (m_linksetActivityLock)
+        {
+            System.IntPtr aPtr = BulletSimAPI.GetBodyHandle2(m_scene.World.Ptr, m_linksetRoot.LocalID);
+            if (aPtr == System.IntPtr.Zero)
+            {
+                // That's odd. We can't find the root of the linkset.
+                // The linkset is somehow dead.  The requestor is now a member of a linkset of one.
+                DetailLog("{0},RefreshLinkset.RemoveRoot,child={1}", m_linksetRoot.LocalID, m_linksetRoot.LocalID);
+                ret = RemoveMeFromLinkset(m_linksetRoot);
+            }
+            else
+            {
+                // Reconstruct the pointer to the body of the linkset root.
+                DetailLog("{0},RefreshLinkset.RebuildRoot,rootID={1},ptr={2}", m_linksetRoot.LocalID, m_linksetRoot.LocalID, aPtr);
+                m_linksetRoot.Body = new BulletBody(m_linksetRoot.LocalID, aPtr);
+
+                List<BSPrim> toRemove = new List<BSPrim>();
+                foreach (BSPrim bsp in m_children)
+                {
+                    aPtr = BulletSimAPI.GetBodyHandle2(m_scene.World.Ptr, bsp.LocalID);
+                    if (aPtr == System.IntPtr.Zero)
+                    {
+                        toRemove.Add(bsp);
+                    }
+                    else
+                    {
+                        // Reconstruct the pointer to the body of the linkset root.
+                        DetailLog("{0},RefreshLinkset.RebuildChild,rootID={1},ptr={2}", bsp.LocalID, m_linksetRoot.LocalID, aPtr);
+                        bsp.Body = new BulletBody(bsp.LocalID, aPtr);
+                    }
+                }
+                foreach (BSPrim bsp in toRemove)
+                {
+                    RemoveChildFromLinkset(bsp);
+                }
+            }
+        }
+
+        return ret;
     }
 
 
@@ -256,10 +296,13 @@ public class BSLinkset
         DetailLog("{0},LinkAChildToMe,taint,root={1},child={2}", m_linksetRoot.LocalID, m_linksetRoot.LocalID, childPrim.LocalID);
         BSConstraint constrain = m_scene.Constraints.CreateConstraint(
                         m_scene.World, m_linksetRoot.Body, childPrim.Body,
-                        childRelativePosition,
-                        childRelativeRotation,
+                        // childRelativePosition,
+                        // childRelativeRotation,
+                        OMV.Vector3.Zero,
+                        OMV.Quaternion.Identity,
                         OMV.Vector3.Zero,
-                        OMV.Quaternion.Identity);
+                        OMV.Quaternion.Identity
+                        );
         constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
         constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
 
@@ -268,6 +311,7 @@ public class BSLinkset
         constrain.TranslationalLimitMotor(m_scene.BoolNumeric(m_scene.Params.linkConstraintEnableTransMotor),
                         m_scene.Params.linkConstraintTransMotorMaxVel,
                         m_scene.Params.linkConstraintTransMotorMaxForce);
+        constrain.SetCFMAndERP(m_scene.Params.linkConstraintCFM, m_scene.Params.linkConstraintERP);
 
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 7590d93..50d11e6 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -103,7 +103,10 @@ public sealed class BSPrim : PhysicsActor
     long _collidingGroundStep;
 
     private BulletBody m_body;
-    public BulletBody Body { get { return m_body; } }
+    public BulletBody Body { 
+        get { return m_body; }
+        set { m_body = value; }
+    }
 
     private BSDynamics _vehicle;
 
@@ -477,9 +480,11 @@ public sealed class BSPrim : PhysicsActor
     // Only called at taint time so it is save to call into Bullet.
     private void SetObjectDynamic()
     {
-        // m_log.DebugFormat("{0}: ID={1}, SetObjectDynamic: IsStatic={2}, IsSolid={3}", LogHeader, _localID, IsStatic, IsSolid);
-
-        RecreateGeomAndObject();
+        // RA: remove this for the moment.
+        // The problem is that dynamic objects are hulls so if we are becoming physical
+        //    the shape has to be checked and possibly built.
+        //    Maybe a VerifyCorrectPhysicalShape() routine?
+        // RecreateGeomAndObject();
 
         float mass = _mass;
         // Bullet wants static objects have a mass of zero
@@ -971,21 +976,23 @@ public sealed class BSPrim : PhysicsActor
                     {
                         DetailLog("{0},CreateGeom,sphere", LocalID);
                         _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE;
-                        ret = true;
                         // Bullet native objects are scaled by the Bullet engine so pass the size in
                         _scale = _size;
+                        // TODO: do we need to check for and destroy a mesh or hull that might have been left from before?
+                        ret = true;
                     }
                 }
             }
             else
             {
-                // m_log.DebugFormat("{0}: CreateGeom: Defaulting to box. lid={1}, size={2}", LogHeader, LocalID, _size);
+                // m_log.DebugFormat("{0}: CreateGeom: Defaulting to box. lid={1}, type={2}, size={3}", LogHeader, LocalID, _shapeType, _size);
                 if (_shapeType != ShapeData.PhysicsShapeType.SHAPE_BOX)
                 {
                     DetailLog("{0},CreateGeom,box", LocalID);
                     _shapeType = ShapeData.PhysicsShapeType.SHAPE_BOX;
-                    ret = true;
                     _scale = _size;
+                    // TODO: do we need to check for and destroy a mesh or hull that might have been left from before?
+                    ret = true;
                 }
             }
         }
@@ -1203,11 +1210,9 @@ public sealed class BSPrim : PhysicsActor
         FillShapeInfo(out shape);
         // m_log.DebugFormat("{0}: CreateObject: lID={1}, shape={2}", LogHeader, _localID, shape.Type);
         BulletSimAPI.CreateObject(_scene.WorldID, shape);
+
         // the CreateObject() may have recreated the rigid body. Make sure we have the latest.
         m_body.Ptr = BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID);
-
-        // The root object could have been recreated. Make sure everything linksety is up to date.
-        _linkset.RefreshLinkset(this);
     }
 
     // Copy prim's info into the BulletSim shape description structure
@@ -1236,8 +1241,8 @@ public sealed class BSPrim : PhysicsActor
     private void RecreateGeomAndObject()
     {
         // m_log.DebugFormat("{0}: RecreateGeomAndObject. lID={1}", LogHeader, _localID);
-        CreateGeom(true);
-        CreateObject();
+        if (CreateGeom(true))
+            CreateObject();
         return;
     }
 
@@ -1322,6 +1327,15 @@ public sealed class BSPrim : PhysicsActor
 
             base.RequestPhysicsterseUpdate();
         }
+            /*
+        else
+        {
+            // For debugging, we can also report the movement of children
+            DetailLog("{0},UpdateProperties,child,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+                    LocalID, entprop.Position, entprop.Rotation, entprop.Velocity, 
+                    entprop.Acceleration, entprop.RotationalVelocity);
+        }
+             */
     }
 
     // I've collided with something
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index c6d622b..28d5cb5 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -315,6 +315,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, bool isFlying)
     {
         // m_log.DebugFormat("{0}: AddAvatar: {1}", LogHeader, avName);
+
+        if (!m_initialized) return null;
+
         BSCharacter actor = new BSCharacter(localID, avName, this, position, size, isFlying);
         lock (m_avatars) m_avatars.Add(localID, actor);
         return actor;
@@ -323,6 +326,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     public override void RemoveAvatar(PhysicsActor actor)
     {
         // m_log.DebugFormat("{0}: RemoveAvatar", LogHeader);
+
+        if (!m_initialized) return;
+
         BSCharacter bsactor = actor as BSCharacter;
         if (bsactor != null)
         {
@@ -341,6 +347,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     public override void RemovePrim(PhysicsActor prim)
     {
+        if (!m_initialized) return;
+
         BSPrim bsprim = prim as BSPrim;
         if (bsprim != null)
         {
@@ -366,6 +374,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                                               Vector3 size, Quaternion rotation, bool isPhysical, uint localID)
     {
         // m_log.DebugFormat("{0}: AddPrimShape2: {1}", LogHeader, primName);
+
+        if (!m_initialized) return null;
+
         BSPrim prim = new BSPrim(localID, primName, this, position, size, rotation, pbs, isPhysical);
         lock (m_prims) m_prims.Add(localID, prim);
         return prim;
@@ -807,6 +818,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     // List of all of the externally visible parameters.
     // For each parameter, this table maps a text name to getter and setters.
+    // To add a new externally referencable/settable parameter, add the paramter storage
+    //    location somewhere in the program and make an entry in this table with the
+    //    getters and setters.
+    // To add a new variable, it is easiest to find an existing definition and copy it.
+    // Parameter values are floats. Booleans are converted to a floating value.
+    // 
     // A ParameterDefn() takes the following parameters:
     //    -- the text name of the parameter. This is used for console input and ini file.
     //    -- a short text description of the parameter. This shows up in the console listing.
@@ -815,7 +832,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     //    -- a delegate for getting the value as a float
     //    -- a delegate for setting the value from a float
     //
-    // To add a new variable, it is best to find an existing definition and copy it.
+    // The single letter parameters for the delegates are:
+    //    s = BSScene
+    //    p = string parameter name
+    //    l = localID of referenced object
+    //    v = float value
+    //    cf = parameter configuration class (for fetching values from ini file)
     private ParameterDefn[] ParameterDefinitions =
     {
         new ParameterDefn("MeshSculptedPrim", "Whether to create meshes for sculpties",
@@ -1048,6 +1070,16 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.m_params[0].linkConstraintTransMotorMaxForce = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linkConstraintTransMotorMaxForce; },
             (s,p,l,v) => { s.m_params[0].linkConstraintTransMotorMaxForce = v; } ),
+            new ParameterDefn("LinkConstraintCFM", "Amount constraint can be violated. 0=none, 1=all. Default=0",
+            0.0f,
+            (s,cf,p,v) => { s.m_params[0].linkConstraintCFM = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].linkConstraintCFM; },
+            (s,p,l,v) => { s.m_params[0].linkConstraintCFM = v; } ),
+            new ParameterDefn("LinkConstraintERP", "Amount constraint is corrected each tick. 0=none, 1=all. Default = 0.2",
+            0.2f,
+            (s,cf,p,v) => { s.m_params[0].linkConstraintERP = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].linkConstraintERP; },
+            (s,p,l,v) => { s.m_params[0].linkConstraintERP = v; } ),
 
         new ParameterDefn("DetailedStats", "Frames between outputting detailed phys stats. (0 is off)",
             0f,
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 65e3145..fe705cc 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -66,13 +66,14 @@ public struct ShapeData
 {
     public enum PhysicsShapeType
     {
-                SHAPE_AVATAR = 0,
-                SHAPE_BOX = 1,
-                SHAPE_CONE = 2,
-                SHAPE_CYLINDER = 3,
-                SHAPE_SPHERE = 4,
-                SHAPE_MESH = 5,
-                SHAPE_HULL = 6
+                SHAPE_UNKNOWN   = 0,
+                SHAPE_AVATAR    = 1,
+                SHAPE_BOX       = 2,
+                SHAPE_CONE      = 3,
+                SHAPE_CYLINDER  = 4,
+                SHAPE_SPHERE    = 5,
+                SHAPE_MESH      = 6,
+                SHAPE_HULL      = 7
     };
     public uint ID;
     public PhysicsShapeType Type;
@@ -168,6 +169,8 @@ public struct ConfigurationParameters
     public float linkConstraintEnableTransMotor;
     public float linkConstraintTransMotorMaxVel;
     public float linkConstraintTransMotorMaxForce;
+    public float linkConstraintERP;
+    public float linkConstraintCFM;
 
     public const float numericTrue = 1f;
     public const float numericFalse = 0f;
@@ -189,6 +192,28 @@ public enum CollisionFlags : uint
     PHYSICAL_OBJECT               = 1 << 12,
 };
 
+// CFM controls the 'hardness' of the constraint. 0=fixed, 0..1=violatable. Default=0
+// ERP controls amount of correction per tick. Usable range=0.1..0.8. Default=0.2.
+public enum ConstraintParams : int
+{
+    BT_CONSTRAINT_ERP = 1,  // this one is not used in Bullet as of 20120730
+    BT_CONSTRAINT_STOP_ERP,
+    BT_CONSTRAINT_CFM,
+    BT_CONSTRAINT_STOP_CFM,
+};
+public enum ConstraintParamAxis : int
+{
+    AXIS_LINEAR_X = 0,
+    AXIS_LINEAR_Y,
+    AXIS_LINEAR_Z,
+    AXIS_ANGULAR_X,
+    AXIS_ANGULAR_Y,
+    AXIS_ANGULAR_Z,
+    AXIS_LINEAR_ALL = 20,    // these last three added by BulletSim so we don't have to do zillions of calls
+    AXIS_ANGULAR_ALL,
+    AXIS_ALL
+};
+
 // ===============================================================================
 static class BulletSimAPI {
 
@@ -380,7 +405,8 @@ public static extern IntPtr CreateObject2(IntPtr sim, ShapeData shapeData);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr CreateConstraint2(IntPtr sim, IntPtr obj1, IntPtr obj2,
                     Vector3 frame1loc, Quaternion frame1rot,
-                    Vector3 frame2loc, Quaternion frame2rot);
+                    Vector3 frame2loc, Quaternion frame2rot,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool SetLinearLimits2(IntPtr constrain, Vector3 low, Vector3 hi);
@@ -398,6 +424,9 @@ public static extern bool TranslationalLimitMotor2(IntPtr constrain, float enabl
 public static extern bool CalculateTransforms2(IntPtr constrain);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetConstraintParam2(IntPtr constrain, ConstraintParams paramIndex, float value, ConstraintParamAxis axis);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool DestroyConstraint2(IntPtr sim, IntPtr constrain);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]