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]