BulletSim: Move constraint tracking from C++ code to C# code

for more flexibility.

2 files changed, 83 insertions, 18 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index a19d6d7..ff87955 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -97,6 +97,9 @@ public sealed class BSPrim : PhysicsActor
     long _collidingStep;
     long _collidingGroundStep;
 
+    private BulletBody m_body;
+    public BulletBody Body { get { return m_body; } }
+
     private BSDynamics _vehicle;
 
     private OMV.Vector3 _PIDTarget;
@@ -138,6 +141,11 @@ public sealed class BSPrim : PhysicsActor
         _scene.TaintedObject(delegate()
         {
             RecreateGeomAndObject();
+
+            // Get the pointer to the physical body for this object.
+            // At the moment, we're still letting BulletSim manage the creation and destruction
+            //    of the object. Someday we'll move that into the C# code.
+            m_body = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID));
         });
     }
 
@@ -161,7 +169,7 @@ public sealed class BSPrim : PhysicsActor
                 _parentPrim = null;
             }
 
-            // make sure there are no possible children depending on me
+            // make sure there are no other prims are linked to me
             UnlinkAllChildren();
 
             // everything in the C# world will get garbage collected. Tell the C++ world to free stuff.
@@ -333,11 +341,11 @@ public sealed class BSPrim : PhysicsActor
         _rotationalVelocity = OMV.Vector3.Zero;
 
         // Zero some other properties directly into the physics engine
-        IntPtr obj = BulletSimAPI.GetBodyHandleWorldID2(_scene.WorldID, LocalID);
-        BulletSimAPI.SetVelocity2(obj, OMV.Vector3.Zero);
-        BulletSimAPI.SetAngularVelocity2(obj, OMV.Vector3.Zero);
-        BulletSimAPI.SetInterpolation2(obj, OMV.Vector3.Zero, OMV.Vector3.Zero);
-        BulletSimAPI.ClearForces2(obj);
+        BulletBody obj = new BulletBody(LocalID, BulletSimAPI.GetBodyHandleWorldID2(_scene.WorldID, LocalID));
+        BulletSimAPI.SetVelocity2(obj.Ptr, OMV.Vector3.Zero);
+        BulletSimAPI.SetAngularVelocity2(obj.Ptr, OMV.Vector3.Zero);
+        BulletSimAPI.SetInterpolation2(obj.Ptr, OMV.Vector3.Zero, OMV.Vector3.Zero);
+        BulletSimAPI.ClearForces2(obj.Ptr);
     }
 
     public override void LockAngularMotion(OMV.Vector3 axis)
@@ -383,7 +391,8 @@ public sealed class BSPrim : PhysicsActor
             _scene.TaintedObject(delegate()
             {
                 DetailLog("{0},SetForce,taint,force={1}", LocalID, _force);
-                BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
+                // BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
+                BulletSimAPI.SetObjectForce2(Body.Ptr, _force);
             });
         } 
     }
@@ -407,8 +416,7 @@ public sealed class BSPrim : PhysicsActor
                     _scene.TaintedObject(delegate()
                     {
                         // Tell the physics engine to clear state
-                        IntPtr obj = BulletSimAPI.GetBodyHandleWorldID2(_scene.WorldID, LocalID);
-                        BulletSimAPI.ClearForces2(obj);
+                        BulletSimAPI.ClearForces2(this.Body.Ptr);
                     });
 
                     // make it so the scene will call us each tick to do vehicle things
@@ -420,7 +428,6 @@ public sealed class BSPrim : PhysicsActor
     }
     public override void VehicleFloatParam(int param, float value) 
     {
-        m_log.DebugFormat("{0} VehicleFloatParam. {1} <= {2}", LogHeader, param, value);
         _scene.TaintedObject(delegate()
         {
             _vehicle.ProcessFloatVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
@@ -428,7 +435,6 @@ public sealed class BSPrim : PhysicsActor
     }
     public override void VehicleVectorParam(int param, OMV.Vector3 value) 
     {
-        m_log.DebugFormat("{0} VehicleVectorParam. {1} <= {2}", LogHeader, param, value);
         _scene.TaintedObject(delegate()
         {
             _vehicle.ProcessVectorVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
@@ -436,7 +442,6 @@ public sealed class BSPrim : PhysicsActor
     }
     public override void VehicleRotationParam(int param, OMV.Quaternion rotation) 
     {
-        m_log.DebugFormat("{0} VehicleRotationParam. {1} <= {2}", LogHeader, param, rotation);
         _scene.TaintedObject(delegate()
         {
             _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
@@ -444,7 +449,6 @@ public sealed class BSPrim : PhysicsActor
     }
     public override void VehicleFlags(int param, bool remove) 
     {
-        m_log.DebugFormat("{0} VehicleFlags. {1}. Remove={2}", LogHeader, param, remove);
         _scene.TaintedObject(delegate()
         {
             _vehicle.ProcessVehicleFlags(param, remove);
@@ -1296,7 +1300,7 @@ public sealed class BSPrim : PhysicsActor
 
         // remove any constraints that might be in place
         DebugLog("{0}: CreateLinkset: RemoveConstraints between me and any children", LogHeader, LocalID);
-        BulletSimAPI.RemoveConstraintByID(_scene.WorldID, LocalID);
+        UnlinkAllChildren();
 
         // create constraints between the root prim and each of the children
         foreach (BSPrim prim in _childrenPrims)
@@ -1323,6 +1327,7 @@ public sealed class BSPrim : PhysicsActor
         // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
         DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
         DetailLog("{0},LinkAChildToMe,taint,root={1},child={2}", LocalID, LocalID, childPrim.LocalID);
+        /*
         BulletSimAPI.AddConstraint(_scene.WorldID, LocalID, childPrim.LocalID, 
             childRelativePosition,
             childRelativeRotation,
@@ -1330,6 +1335,20 @@ public sealed class BSPrim : PhysicsActor
             OMV.Quaternion.Identity,
             OMV.Vector3.Zero, OMV.Vector3.Zero,
             OMV.Vector3.Zero, OMV.Vector3.Zero);
+         */
+        // BSConstraint constrain = new BSConstraint(_scene.World, this.Body, childPrim.Body,
+        BSConstraint constrain = _scene.Constraints.CreateConstraint(
+                        _scene.World, this.Body, childPrim.Body,
+                        childRelativePosition,
+                        childRelativeRotation,
+                        OMV.Vector3.Zero,
+                        OMV.Quaternion.Identity);
+        constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
+        constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
+
+        // tweek the constraint to increase stability
+        constrain.UseFrameOffset(true);
+        constrain.TranslationalLimitMotor(true, 5f, 0.1f);
     }
 
     // Remove linkage between myself and a particular child
@@ -1339,7 +1358,8 @@ public sealed class BSPrim : PhysicsActor
         DebugLog("{0}: UnlinkAChildFromMe: RemoveConstraint between root prim {1} and child prim {2}", 
                     LogHeader, LocalID, childPrim.LocalID);
         DetailLog("{0},UnlinkAChildFromMe,taint,root={1},child={2}", LocalID, LocalID, childPrim.LocalID);
-        BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, childPrim.LocalID);
+        // BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, childPrim.LocalID);
+        _scene.Constraints.RemoveAndDestroyConstraint(this.Body, childPrim.Body);
     }
 
     // Remove linkage between myself and any possible children I might have
@@ -1348,7 +1368,8 @@ public sealed class BSPrim : PhysicsActor
     {
         DebugLog("{0}: UnlinkAllChildren:", LogHeader);
         DetailLog("{0},UnlinkAllChildren,taint", LocalID);
-        BulletSimAPI.RemoveConstraintByID(_scene.WorldID, LocalID);
+        _scene.Constraints.RemoveAndDestroyConstraint(this.Body);
+        // BulletSimAPI.RemoveConstraintByID(_scene.WorldID, LocalID);
     }
 
     #endregion // Linkset creation and destruction
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 54a8cfd..89fd9b7 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -32,6 +32,28 @@ using OpenMetaverse;
 
 namespace OpenSim.Region.Physics.BulletSPlugin {
 
+// Classes to allow some type checking for the API
+public struct BulletSim
+{
+    public BulletSim(uint id, IntPtr xx) { ID = id;  Ptr = xx; }
+    public IntPtr Ptr;
+    public uint ID;
+}
+
+public struct BulletBody
+{
+    public BulletBody(uint id, IntPtr xx) { ID = id;  Ptr = xx; }
+    public IntPtr Ptr;
+    public uint ID;
+}
+
+public struct BulletConstraint
+{
+    public BulletConstraint(IntPtr xx) { Ptr = xx; }
+    public IntPtr Ptr;
+}
+
+// ===============================================================================
 [StructLayout(LayoutKind.Sequential)]
 public struct ConvexHull 
 {
@@ -142,6 +164,11 @@ public struct ConfigurationParameters
         public float shouldEnableFrictionCaching;
         public float numberOfSolverIterations;
 
+    public float linkConstraintUseFrameOffset;
+    public float linkConstraintEnableTransMotor;
+    public float linkConstraintTransMotorMaxVel;
+    public float linkConstraintTransMotorMaxForce;
+
     public const float numericTrue = 1f;
     public const float numericFalse = 0f;
 }
@@ -162,6 +189,7 @@ public enum CollisionFlags : uint
     PHYSICAL_OBJECT               = 1 << 12,
 };
 
+// ===============================================================================
 static class BulletSimAPI {
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -214,6 +242,7 @@ public static extern bool CreateObject(uint worldID, ShapeData shapeData);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void CreateLinkset(uint worldID, int objectCount, ShapeData[] shapeDatas);
 
+/*  Remove old functionality
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void AddConstraint(uint worldID, uint id1, uint id2, 
                         Vector3 frame1, Quaternion frame1rot,
@@ -225,6 +254,7 @@ public static extern bool RemoveConstraintByID(uint worldID, uint id1);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool RemoveConstraint(uint worldID, uint id1, uint id2);
+ */
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern Vector3 GetObjectPosition(uint WorldID, uint id);
@@ -350,8 +380,22 @@ 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 lowLinear, Vector3 hiLinear, Vector3 lowAngular, Vector3 hiAngular);
+                    Vector3 frame2loc, Quaternion frame2rot);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetLinearLimits2(IntPtr constrain, Vector3 low, Vector3 hi);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetAngularLimits2(IntPtr constrain, Vector3 low, Vector3 hi);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool UseFrameOffset2(IntPtr constrain, float enable);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool TranslationalLimitMotor2(IntPtr constrain, float enable, float targetVel, float maxMotorForce);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool CalculateTransforms2(IntPtr constrain);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool DestroyConstraint2(IntPtr sim, IntPtr constrain);