6 files changed, 146 insertions, 94 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
new file mode 100755
index 0000000..72df6b9
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+
+public class BS6DofConstraint : BSConstraint
+{
+    // Create a btGeneric6DofConstraint
+    public BS6DofConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
+                    Vector3 frame1, Quaternion frame1rot,
+                    Vector3 frame2, Quaternion frame2rot )
+    {
+        m_world = world;
+        m_body1 = obj1;
+        m_body2 = obj2;
+        m_constraint = new BulletConstraint(
+                            BulletSimAPI.Create6DofConstraint2(m_world.Ptr, m_body1.Ptr, m_body2.Ptr,
+                                frame1, frame1rot,
+                                frame2, frame2rot,
+                                true /*useLinearReferenceFrameA*/, true /*disableCollisionsBetweenLinkedBodies*/));
+        m_enabled = true;
+    }
+
+    public bool SetCFMAndERP(float cfm, float erp)
+    {
+        bool ret = true;
+        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;
+        float onOff = useOffset ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
+        if (m_enabled)
+            ret = BulletSimAPI.UseFrameOffset2(m_constraint.Ptr, onOff);
+        return ret;
+    }
+
+    public bool TranslationalLimitMotor(bool enable, float targetVelocity, float maxMotorForce)
+    {
+        bool ret = false;
+        float onOff = enable ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
+        if (m_enabled)
+            ret = BulletSimAPI.TranslationalLimitMotor2(m_constraint.Ptr, onOff, targetVelocity, maxMotorForce);
+        return ret;
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index ea3093a..a17efea 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -32,30 +32,19 @@ using OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 
-public class BSConstraint : IDisposable
+public abstract class BSConstraint : IDisposable
 {
-    private BulletSim m_world;
-    private BulletBody m_body1;
-    private BulletBody m_body2;
-    private BulletConstraint m_constraint;
-    private bool m_enabled = false;
+    protected BulletSim m_world;
+    protected BulletBody m_body1;
+    protected BulletBody m_body2;
+    protected BulletConstraint m_constraint;
+    protected bool m_enabled = false;
 
-    public BSConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
-                    Vector3 frame1, Quaternion frame1rot,
-                    Vector3 frame2, Quaternion frame2rot
-        )
+    public BSConstraint()
     {
-        m_world = world;
-        m_body1 = obj1;
-        m_body2 = obj2;
-        m_constraint = new BulletConstraint(BulletSimAPI.CreateConstraint2(m_world.Ptr, m_body1.Ptr, m_body2.Ptr,
-                            frame1, frame1rot,
-                            frame2, frame2rot,
-                            true /*useLinearReferenceFrameA*/, true /*disableCollisionsBetweenLinkedBodies*/));
-        m_enabled = true;
     }
 
-    public void Dispose()
+    public virtual void Dispose()
     {
         if (m_enabled)
         {
@@ -68,7 +57,7 @@ public class BSConstraint : IDisposable
     public BulletBody Body1 { get { return m_body1; } }
     public BulletBody Body2 { get { return m_body2; } }
 
-    public bool SetLinearLimits(Vector3 low, Vector3 high)
+    public virtual bool SetLinearLimits(Vector3 low, Vector3 high)
     {
         bool ret = false;
         if (m_enabled)
@@ -76,7 +65,7 @@ public class BSConstraint : IDisposable
         return ret;
     }
 
-    public bool SetAngularLimits(Vector3 low, Vector3 high)
+    public virtual bool SetAngularLimits(Vector3 low, Vector3 high)
     {
         bool ret = false;
         if (m_enabled)
@@ -84,34 +73,7 @@ public class BSConstraint : IDisposable
         return ret;
     }
 
-    public bool SetCFMAndERP(float cfm, float erp)
-    {
-        bool ret = true;
-        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;
-        float onOff = useOffset ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
-        if (m_enabled)
-            ret = BulletSimAPI.UseFrameOffset2(m_constraint.Ptr, onOff);
-        return ret;
-    }
-
-    public bool TranslationalLimitMotor(bool enable, float targetVelocity, float maxMotorForce)
-    {
-        bool ret = false;
-        float onOff = enable ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
-        if (m_enabled)
-            ret = BulletSimAPI.TranslationalLimitMotor2(m_constraint.Ptr, onOff, targetVelocity, maxMotorForce);
-        return ret;
-    }
-
-    public bool CalculateTransforms()
+    public virtual bool CalculateTransforms()
     {
         bool ret = false;
         if (m_enabled)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
index c88e645..397045a 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
@@ -63,16 +63,6 @@ public class BSConstraintCollection : IDisposable
         m_constraints.Clear();
     }
 
-    public BSConstraint CreateConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
-                    Vector3 frame1, Quaternion frame1rot,
-                    Vector3 frame2, Quaternion frame2rot)
-    {
-        BSConstraint constrain = new BSConstraint(world, obj1, obj2, frame1, frame1rot, frame2, frame2rot);
-
-        this.AddConstraint(constrain);
-        return constrain;
-    }
-
     public bool AddConstraint(BSConstraint cons)
     {
         // There is only one constraint between any bodies. Remove any old just to make sure.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index d19c4b8..e265d6d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -101,7 +101,7 @@ public class BSLinkset
                 {
                     // Note that we don't do a foreach because the remove routine
                     //    takes it out of the list.
-                    RemoveChildFromLinkset(m_children[0]);
+                    RemoveChildFromOtherLinkset(m_children[0]);
                 }
                 m_children.Clear(); // just to make sure
             }
@@ -124,6 +124,7 @@ public class BSLinkset
 
         lock (m_linksetActivityLock)
         {
+            // The body pointer is refetched in case anything has moved.
             System.IntPtr aPtr = BulletSimAPI.GetBodyHandle2(m_scene.World.Ptr, m_linksetRoot.LocalID);
             if (aPtr == System.IntPtr.Zero)
             {
@@ -155,7 +156,7 @@ public class BSLinkset
                 }
                 foreach (BSPrim bsp in toRemove)
                 {
-                    RemoveChildFromLinkset(bsp);
+                    RemoveChildFromOtherLinkset(bsp);
                 }
             }
         }
@@ -208,7 +209,8 @@ public class BSLinkset
             com += bp.Position * bp.MassRaw;
             totalMass += bp.MassRaw;
         }
-        com /= totalMass;
+        if (totalMass != 0f)
+            com /= totalMass;
 
         return com;
     }
@@ -221,7 +223,7 @@ public class BSLinkset
         {
             com += bp.Position * bp.MassRaw;
         }
-        com /= m_children.Count + 1;
+        com /= (m_children.Count + 1);
 
         return com;
     }
@@ -237,13 +239,24 @@ public class BSLinkset
             m_scene.TaintedObject(delegate()
             {
                 DebugLog("{0}: AddChildToLinkset: adding child {1} to {2}", LogHeader, child.LocalID, m_linksetRoot.LocalID);
-                DetailLog("{0},AddChildToLinkset,child={1}", m_linksetRoot.LocalID, pchild.LocalID);
+                DetailLog("{0},AddChildToLinkset,taint,child={1}", m_linksetRoot.LocalID, pchild.LocalID);
                 PhysicallyLinkAChildToRoot(pchild);     // build the physical binding between me and the child
             });
         }
         return;
     }
 
+    // Forcefully removing a child from a linkset.
+    // This is not being called by the child so we have to make sure the child doesn't think
+    //    it's still connected to the linkset.
+    // Normal OpenSimulator operation will never do this because other SceneObjectPart information
+    //    has to be updated also (like pointer to prim's parent).
+    public void RemoveChildFromOtherLinkset(BSPrim pchild)
+    {
+        pchild.Linkset = new BSLinkset(m_scene, pchild);
+        RemoveChildFromLinkset(pchild);
+    }
+
     // I am the root of a linkset and one of my children is being removed.
     // Safe to call even if the child is not really in my linkset.
     public void RemoveChildFromLinkset(BSPrim pchild)
@@ -255,7 +268,7 @@ public class BSLinkset
             m_scene.TaintedObject(delegate()
             {
                 DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
-                DetailLog("{0},RemoveChildFromLinkset,child={1}", m_linksetRoot.LocalID, pchild.LocalID);
+                DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, pchild.LocalID);
 
                 if (m_children.Count == 0)
                 {
@@ -294,13 +307,14 @@ public class BSLinkset
         // 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}", m_linksetRoot.LocalID, m_linksetRoot.LocalID, childPrim.LocalID);
-        BSConstraint constrain = m_scene.Constraints.CreateConstraint(
+        BS6DofConstraint constrain = new BS6DofConstraint(
                         m_scene.World, m_linksetRoot.Body, childPrim.Body,
                         childRelativePosition,
                         childRelativeRotation,
                         OMV.Vector3.Zero,
                         -childRelativeRotation
                         );
+        m_scene.Constraints.AddConstraint(constrain);
 
         // zero linear and angular limits makes the objects unable to move in relation to each other
         constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
@@ -319,11 +333,13 @@ public class BSLinkset
     // Called at taint time!
     private void PhysicallyUnlinkAChildFromRoot(BSPrim childPrim)
     {
-        DebugLog("{0}: PhysicallyUnlinkAChildFromRoot: RemoveConstraint between root prim {1} and child prim {2}", 
-                    LogHeader, m_linksetRoot.LocalID, childPrim.LocalID);
+        // DebugLog("{0}: PhysicallyUnlinkAChildFromRoot: RemoveConstraint between root prim {1} and child prim {2}", 
+        //             LogHeader, m_linksetRoot.LocalID, childPrim.LocalID);
         DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},child={2}", m_linksetRoot.LocalID, m_linksetRoot.LocalID, childPrim.LocalID);
-        // BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, childPrim.LocalID);
+
         m_scene.Constraints.RemoveAndDestroyConstraint(m_linksetRoot.Body, childPrim.Body);
+        // Make the child refresh its location
+        BulletSimAPI.PushUpdate2(childPrim.Body.Ptr);
     }
 
     // Remove linkage between myself and any possible children I might have
@@ -332,8 +348,8 @@ public class BSLinkset
     {
         // DebugLog("{0}: PhysicallyUnlinkAllChildren:", LogHeader);
         DetailLog("{0},PhysicallyUnlinkAllChildren,taint", m_linksetRoot.LocalID);
+
         m_scene.Constraints.RemoveAndDestroyConstraint(m_linksetRoot.Body);
-        // BulletSimAPI.RemoveConstraintByID(_scene.WorldID, LocalID);
     }
 
     // Invoke the detailed logger and output something if it's enabled.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 98b69b1..e0f6ed2 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -224,10 +224,12 @@ public sealed class BSPrim : PhysicsActor
     // link me to the specified parent
     public override void link(PhysicsActor obj) {
         BSPrim parent = obj as BSPrim;
-        DebugLog("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, obj.LocalID);
-        DetailLog("{0},link,parent={1}", LocalID, obj.LocalID);
-
-        _linkset = _linkset.AddMeToLinkset(this, parent);
+        if (parent != null)
+        {
+            DebugLog("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, parent.LocalID);
+            DetailLog("{0},link,parent={1}", LocalID, parent.LocalID);
+            _linkset = _linkset.AddMeToLinkset(this, parent);
+        }
         return; 
     }
 
@@ -478,7 +480,6 @@ public sealed class BSPrim : PhysicsActor
 
     // Make gravity work if the object is physical and not selected
     // No locking here because only called when it is safe
-    // Only called at taint time so it is save to call into Bullet.
     private void SetObjectDynamic()
     {
         // RA: remove this for the moment.
@@ -982,7 +983,7 @@ public sealed class BSPrim : PhysicsActor
                     // m_log.DebugFormat("{0}: CreateGeom: Defaulting to sphere of size {1}", LogHeader, _size);
                     if (forceRebuild || (_shapeType != ShapeData.PhysicsShapeType.SHAPE_SPHERE))
                     {
-                        DetailLog("{0},CreateGeom,sphere", LocalID);
+                        DetailLog("{0},CreateGeom,sphere (force={1}", LocalID, forceRebuild);
                         _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE;
                         // Bullet native objects are scaled by the Bullet engine so pass the size in
                         _scale = _size;
@@ -996,7 +997,7 @@ public sealed class BSPrim : PhysicsActor
                 // m_log.DebugFormat("{0}: CreateGeom: Defaulting to box. lid={1}, type={2}, size={3}", LogHeader, LocalID, _shapeType, _size);
                 if (forceRebuild || (_shapeType != ShapeData.PhysicsShapeType.SHAPE_BOX))
                 {
-                    DetailLog("{0},CreateGeom,box", LocalID);
+                    DetailLog("{0},CreateGeom,box (force={1})", LocalID, forceRebuild);
                     _shapeType = ShapeData.PhysicsShapeType.SHAPE_BOX;
                     _scale = _size;
                     // TODO: do we need to check for and destroy a mesh or hull that might have been left from before?
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 86fc9d2..c016402 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -363,7 +363,7 @@ public static extern IntPtr GetSimHandle2(uint worldID);
 public static extern IntPtr GetBodyHandleWorldID2(uint worldID, uint id);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetBodyHandle2(IntPtr sim, uint id);
+public static extern IntPtr GetBodyHandle2(IntPtr world, uint id);
 
 // ===============================================================================
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -372,40 +372,43 @@ public static extern IntPtr Initialize2(Vector3 maxPosition, IntPtr parms,
                                                                                         int maxUpdates, IntPtr updateArray);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool UpdateParameter2(IntPtr sim, uint localID, String parm, float value);
+public static extern bool UpdateParameter2(IntPtr world, uint localID, String parm, float value);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetHeightmap2(IntPtr sim, float[] heightmap);
+public static extern void SetHeightmap2(IntPtr world, float[] heightmap);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void Shutdown2(IntPtr sim);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern int PhysicsStep2(IntPtr sim, float timeStep, int maxSubSteps, float fixedTimeStep,
+public static extern int PhysicsStep2(IntPtr world, float timeStep, int maxSubSteps, float fixedTimeStep,
                         out int updatedEntityCount, 
                         out IntPtr updatedEntitiesPtr,
                         out int collidersCount,
                         out IntPtr collidersPtr);
 
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool PushUpdate2(IntPtr obj);
+
 /*
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateMesh2(IntPtr sim, int indicesCount, int* indices, int verticesCount, float* vertices );
+public static extern IntPtr CreateMesh2(IntPtr world, int indicesCount, int* indices, int verticesCount, float* vertices );
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool BuildHull2(IntPtr sim, IntPtr mesh);
+public static extern bool BuildHull2(IntPtr world, IntPtr mesh);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool ReleaseHull2(IntPtr sim, IntPtr mesh);
+public static extern bool ReleaseHull2(IntPtr world, IntPtr mesh);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyMesh2(IntPtr sim, IntPtr mesh);
+public static extern bool DestroyMesh2(IntPtr world, IntPtr mesh);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateObject2(IntPtr sim, ShapeData shapeData);
+public static extern IntPtr CreateObject2(IntPtr world, ShapeData shapeData);
 */
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateConstraint2(IntPtr sim, IntPtr obj1, IntPtr obj2,
+public static extern IntPtr Create6DofConstraint2(IntPtr world, IntPtr obj1, IntPtr obj2,
                     Vector3 frame1loc, Quaternion frame1rot,
                     Vector3 frame2loc, Quaternion frame2rot,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
@@ -429,7 +432,13 @@ public static extern bool CalculateTransforms2(IntPtr constrain);
 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);
+public static extern bool DestroyConstraint2(IntPtr world, IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 AddObjectToWorld2(IntPtr world, IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 RemoveObjectFromWorld2(IntPtr world, IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern Vector3 GetPosition2(IntPtr obj);
@@ -510,12 +519,6 @@ public static extern bool SetMargin2(IntPtr obj, float val);
 public static extern bool UpdateSingleAabb2(IntPtr world, IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool AddObjectToWorld2(IntPtr world, IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool RemoveObjectFromWorld2(IntPtr world, IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool DestroyObject2(IntPtr world, uint id);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]