BulletSim: add calls for creating all the different Bullet constraint types.

Updated the DLLs and SOs and code for BulletXNA to create the types.
All the detailed control calls are not all in place yet.

1 files changed, 186 insertions, 41 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
index 15fa52b..6fc10e9 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
@@ -559,8 +559,9 @@ private sealed class BulletConstraintXNA : BulletConstraint
     }
 
 
-    //BulletSimAPI.Create6DofConstraint(m_world.ptr, m_body1.ptr, m_body2.ptr,frame1, frame1rot,frame2, frame2rot,useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
-    public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+    public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, 
+                                    Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, 
+                                    bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
 
     {
         DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
@@ -584,7 +585,24 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return new BulletConstraintXNA(consttr);
     }
 
-    
+    public override BulletConstraint Create6DofConstraintFixed(BulletWorld pWorld, BulletBody pBody1,
+                                        Vector3 pframe1, Quaternion pframe1rot,
+                                        bool pUseLinearReferenceFrameB, bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
+        IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
+        IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
+        IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
+        frame1._origin = frame1v;
+
+        Generic6DofConstraint consttr = new Generic6DofConstraint(body1, ref frame1, pUseLinearReferenceFrameB);
+        consttr.CalculateTransforms();
+        world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);
+
+        return new BulletConstraintXNA(consttr);
+    }
+
     /// <summary>
     /// 
     /// </summary>
@@ -1443,129 +1461,130 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public BSPhysicsShapeType BSShapeTypeFromBroadPhaseNativeType(BroadphaseNativeTypes pin)
     {
+        BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
         switch (pin)
         {
             case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_BOX;
+                ret =  BSPhysicsShapeType.SHAPE_BOX;
                 break;
             case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
 
             case BroadphaseNativeTypes.TETRAHEDRAL_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_MESH;
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
                 break;
             case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_HULL;
+                ret =  BSPhysicsShapeType.SHAPE_HULL;
                 break;
             case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.CUSTOM_POLYHEDRAL_SHAPE_TYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             //implicit convex shapes
             case BroadphaseNativeTypes.IMPLICIT_CONVEX_SHAPES_START_HERE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_SPHERE;
+                ret =  BSPhysicsShapeType.SHAPE_SPHERE;
                 break;
             case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_CAPSULE;
+                ret =  BSPhysicsShapeType.SHAPE_CAPSULE;
                 break;
             case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_CONE;
+                ret =  BSPhysicsShapeType.SHAPE_CONE;
                 break;
             case BroadphaseNativeTypes.CONVEX_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_CYLINDER;
+                ret =  BSPhysicsShapeType.SHAPE_CYLINDER;
                 break;
             case BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.MINKOWSKI_SUM_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.BOX_2D_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.CONVEX_2D_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.CUSTOM_CONVEX_SHAPE_TYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             //concave shape
             case BroadphaseNativeTypes.CONCAVE_SHAPES_START_HERE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
             case BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_MESH;
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
                 break;
             case BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_MESH;
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
                 break;
             ///used for demo integration FAST/Swift collision library and Bullet
             case BroadphaseNativeTypes.FAST_CONCAVE_MESH_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_MESH;
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
                 break;
             //terrain
             case BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_HEIGHTMAP;
+                ret =  BSPhysicsShapeType.SHAPE_HEIGHTMAP;
                 break;
             ///Used for GIMPACT Trimesh integration
             case BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_MESH;
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
                 break;
             ///Multimaterial mesh
             case BroadphaseNativeTypes.MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_MESH;
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
                 break;
 
             case BroadphaseNativeTypes.EMPTY_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_GROUNDPLANE;
+                ret =  BSPhysicsShapeType.SHAPE_GROUNDPLANE;
                 break;
             case BroadphaseNativeTypes.CUSTOM_CONCAVE_SHAPE_TYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.CONCAVE_SHAPES_END_HERE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
 
             case BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_COMPOUND;
+                ret =  BSPhysicsShapeType.SHAPE_COMPOUND;
                 break;
 
             case BroadphaseNativeTypes.SOFTBODY_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_MESH;
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
                 break;
             case BroadphaseNativeTypes.HFFLUID_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
             case BroadphaseNativeTypes.INVALID_SHAPE_PROXYTYPE:
-                return BSPhysicsShapeType.SHAPE_UNKNOWN;
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
                 break;
         }
-        return BSPhysicsShapeType.SHAPE_UNKNOWN;
+        return ret;
     }
 
     public override void RemoveChildShapeFromCompoundShape(BulletShape cShape, BulletShape removeShape) { /* TODO */ }
@@ -1579,7 +1598,39 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return new BulletShapeXNA(m_planeshape, BSPhysicsShapeType.SHAPE_GROUNDPLANE);
     }
 
-    public override BulletConstraint CreateHingeConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+    public override BulletConstraint Create6DofSpringConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, 
+                                    Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, 
+                                    bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+
+    {
+        Generic6DofSpringConstraint constrain = null;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
+        if (body1 != null && body2 != null)
+        {
+            IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
+            IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
+            IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
+            frame1._origin = frame1v;
+
+            IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
+            IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
+            IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
+            frame2._origin = frame1v;
+
+            constrain = new Generic6DofSpringConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
+            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
+
+            constrain.CalculateTransforms();
+        }
+
+        return new BulletConstraintXNA(constrain);
+    }
+
+    public override BulletConstraint CreateHingeConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                        Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB,
+                        bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
     {
         HingeConstraint constrain = null;
         DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
@@ -1591,6 +1642,100 @@ private sealed class BulletConstraintXNA : BulletConstraint
             IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z);
             IndexedVector3 axisInA = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z);
             IndexedVector3 axisInB = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z);
+            constrain = new HingeConstraint(rb1, rb2, ref pivotInA, ref pivotInB, ref axisInA, ref axisInB, puseLinearReferenceFrameA);
+            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
+        }
+        return new BulletConstraintXNA(constrain);
+    }
+
+    public override BulletConstraint CreateSliderConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                        Vector3 pframe1, Quaternion pframe1rot,
+                        Vector3 pframe2, Quaternion pframe2rot,
+                        bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        SliderConstraint constrain = null;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
+        if (rb1 != null && rb2 != null)
+        {
+            IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
+            IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
+            IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
+            frame1._origin = frame1v;
+
+            IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
+            IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
+            IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
+            frame2._origin = frame1v;
+
+            constrain = new SliderConstraint(rb1, rb2, ref frame1, ref frame2, puseLinearReferenceFrameA);
+            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
+        }
+        return new BulletConstraintXNA(constrain);
+    }
+
+    public override BulletConstraint CreateConeTwistConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                        Vector3 pframe1, Quaternion pframe1rot,
+                        Vector3 pframe2, Quaternion pframe2rot,
+                        bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        ConeTwistConstraint constrain = null;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
+        if (rb1 != null && rb2 != null)
+        {
+            IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
+            IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
+            IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
+            frame1._origin = frame1v;
+
+            IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
+            IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
+            IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
+            frame2._origin = frame1v;
+
+            constrain = new ConeTwistConstraint(rb1, rb2, ref frame1, ref frame2);
+            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
+        }
+        return new BulletConstraintXNA(constrain);
+    }
+
+    public override BulletConstraint CreateGearConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                        Vector3 paxisInA, Vector3 paxisInB,
+                        float pratio, bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        Generic6DofConstraint constrain = null;
+        /*   BulletXNA does not have a gear constraint
+        GearConstraint constrain = null;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
+        if (rb1 != null && rb2 != null)
+        {
+            IndexedVector3 axis1 = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z);
+            IndexedVector3 axis2 = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z);
+            constrain = new GearConstraint(rb1, rb2, ref axis1, ref axis2, pratio);
+            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
+        }
+        */
+        return new BulletConstraintXNA(constrain);
+    }
+
+    public override BulletConstraint CreatePoint2PointConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                        Vector3 ppivotInA, Vector3 ppivotInB,
+                        bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        Point2PointConstraint constrain = null;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
+        if (rb1 != null && rb2 != null)
+        {
+            IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z);
+            IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z);
+            constrain = new Point2PointConstraint(rb1, rb2, ref pivotInA, ref pivotInB);
             world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
         }
         return new BulletConstraintXNA(constrain);