Major rewrite of the constraints. Instead of using two frames that are defined independently, each constraint defines a single frame relative to the object it is attached to and the other frame is calculated for the other object.

Fixed problems with order of creation and destruction of constraints. Rigid bodies must exist and be added to the physics world before constraints are added.

Author: Phong13

UnityProject/Assets/BulletUnity/Examples/Scenes/Constraints/Constraints.unity

@@ -141,7 +141,7 @@ protected virtual void RemoveObjectFromBulletWorld()
             BPhysicsWorld.Get().RemoveCollisionObject(m_collisionObject);
         }
 
-        protected virtual void Start()
+        internal virtual void Start()
         {
             m_startHasBeenCalled = true;
             AddObjectToBulletWorld();

UnityProject/Assets/BulletUnity/Examples/Scenes/Constraints/Constraints.unity.meta

@@ -34,34 +34,6 @@ public override void GetWorldTransform(out Matrix worldTrans) {
 
         //Bullet calls this so I can copy bullet data to unity
         public override void SetWorldTransform(ref Matrix m) {
-            /*
-            BulletSharp.Math.Vector3 pos = m.Origin;
-            UnityEngine.Quaternion q = new UnityEngine.Quaternion();
-            q.w = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] + m[1, 1] + m[2, 2])) / 2;
-            q.x = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] - m[1, 1] - m[2, 2])) / 2;
-            q.y = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] + m[1, 1] - m[2, 2])) / 2;
-            q.z = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] - m[1, 1] + m[2, 2])) / 2;
-            q.x *= Mathf.Sign(q.x * (m[1, 2] - m[2, 1]));
-            q.y *= Mathf.Sign(q.y * (m[2, 0] - m[0, 2]));
-            q.z *= Mathf.Sign(q.z * (m[0, 1] - m[1, 2]));
-            */
-
-            //todo not very efficient
-            /*
-            Matrix4x4 mu = m.ToUnity();
-            UnityEngine.Vector3 p = BSExtensionMethods.ExtractTranslationFromMatrix(ref mu);
-            UnityEngine.Quaternion q = BSExtensionMethods.ExtractRotationFromMatrix(ref mu);
-            UnityEngine.Vector3 sc = BSExtensionMethods.ExtractScaleFromMatrix(ref mu);
-
-            UnityEngine.Vector3 p1 = BSExtensionMethods.ExtractTranslationFromMatrix(ref m);
-            UnityEngine.Quaternion q1 = BSExtensionMethods.ExtractRotationFromMatrix(ref m);
-            UnityEngine.Vector3 sc1 = BSExtensionMethods.ExtractScaleFromMatrix(ref m);
-
-            if (p != p1) Debug.Log("Dont match p " + p + " " + p1);
-            if (q != q1) Debug.Log("Dont match q " + q + " " + q1);
-            if (sc != sc1) Debug.Log("Dont match p " + sc + " " + sc1);
-            */
-
             transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
             transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
             transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref m);

UnityProject/Assets/BulletUnity/Examples/Scenes/SoftBody/RatherGoodPlayground.unity

@@ -7,6 +7,9 @@
 using BulletUnity.Debugging;
 
 
+// TODO order of destruction is an issue
+// TODO all constraints need to be removed from a rigid body before the rigid body is destroyed / removed
+
 namespace BulletUnity {
     public class BPhysicsWorld : MonoBehaviour, IDisposable {
 
@@ -337,7 +340,7 @@ public bool AddCollisionObject(BCollisionObject co)
         {
             if (!_isDisposed)
             {
-				BDebug.Log(debugType, "Adding collision object {0} to world", co);
+                if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Adding collision object {0} to world", co);
                 if (co._BuildCollisionObject())
                 {
                     m_world.AddCollisionObject(co.GetCollisionObject(), co.m_groupsIBelongTo, co.m_collisionMask);
@@ -361,7 +364,7 @@ public void RemoveCollisionObject(BulletSharp.CollisionObject co)
         {
             if (!_isDisposed)
             {
-				BDebug.Log(debugType, "Removing collisionObject {0} from world", co.UserObject);
+                if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Removing collisionObject {0} from world", co.UserObject);
                 m_world.RemoveCollisionObject(co);
                 if (co.UserObject is BCollisionObject) ((BCollisionObject)co.UserObject).isInWorld = false;
                 //TODO handle removing kinematic character controller action
@@ -376,7 +379,7 @@ public bool AddRigidBody(BRigidBody rb)
                 {
 					BDebug.LogError(debugType, "World type must not be collision only");
                 }
-				BDebug.Log(debugType, "Adding rigidbody {0} to world", rb);
+                if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Adding rigidbody {0} to world", rb);
                 if (rb._BuildCollisionObject())
                 {
                     ((DiscreteDynamicsWorld)m_world).AddRigidBody((RigidBody)rb.GetCollisionObject(), rb.m_groupsIBelongTo, rb.m_collisionMask);
@@ -395,7 +398,7 @@ public void RemoveRigidBody(BulletSharp.RigidBody rb)
                 {
 					BDebug.LogError(debugType, "World type must not be collision only");
                 }
-				BDebug.Log(debugType, "Removing rigidbody {0} from world", rb.UserObject);
+                if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Removing rigidbody {0} from world", rb.UserObject);
                 ((DiscreteDynamicsWorld)m_world).RemoveRigidBody(rb);
                 if (rb.UserObject is BCollisionObject) ((BCollisionObject)rb.UserObject).isInWorld = false;
             }
@@ -408,12 +411,13 @@ public bool AddConstraint(BTypedConstraint c)
                 if (m_worldType < WorldType.RigidBodyDynamics)
                 {
 					BDebug.LogError(debugType, "World type must not be collision only");
+                    return false;
                 }
-				BDebug.Log(debugType, "Adding constraint {0} to world", c);
+				if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Adding constraint {0} to world", c);
                 if (c._BuildConstraint())
                 {
-                    ((DiscreteDynamicsWorld)m_world).AddConstraint(c.GetConstraint(), c.disableCollisionsBetweenConstrainedBodies);
-                    c.isInWorld = true;
+                    ((DiscreteDynamicsWorld)m_world).AddConstraint(c.GetConstraint(), c.m_disableCollisionsBetweenConstrainedBodies);
+                    c.m_isInWorld = true;
                 }
                 return true;
             }
@@ -428,22 +432,22 @@ public void RemoveConstraint(BulletSharp.TypedConstraint c)
                 {
 					BDebug.LogError(debugType, "World type must not be collision only");
                 }
-				BDebug.Log(debugType, "Removing constraint {0} from world", c.Userobject);
+                if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Removing constraint {0} from world", c.Userobject);
                 ((DiscreteDynamicsWorld)m_world).RemoveConstraint(c);
-                if (c.Userobject is BTypedConstraint) ((BTypedConstraint)c.Userobject).isInWorld = false;
+                if (c.Userobject is BTypedConstraint) ((BTypedConstraint)c.Userobject).m_isInWorld = false;
             }
         }
 
         public bool AddSoftBody(BSoftBody softBody)
         {
             if (!(m_world is BulletSharp.SoftBody.SoftRigidDynamicsWorld))
             {
-				BDebug.LogError(debugType, "The Physics World must be a BSoftBodyWorld for adding soft bodies");
+                if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("The Physics World must be a BSoftBodyWorld for adding soft bodies");
                 return false;
             }
             if (!_isDisposed)
             {
-				BDebug.Log(debugType, "Adding softbody {0} to world", softBody);
+                if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Adding softbody {0} to world", softBody);
                 if (softBody._BuildCollisionObject())
                 {
                     ((BulletSharp.SoftBody.SoftRigidDynamicsWorld)m_world).AddSoftBody((SoftBody)softBody.GetCollisionObject());
@@ -458,7 +462,7 @@ public void RemoveSoftBody(BulletSharp.SoftBody.SoftBody softBody)
         {
             if (!_isDisposed && m_world is BulletSharp.SoftBody.SoftRigidDynamicsWorld)
             {
-				BDebug.Log(debugType, "Removing softbody {0} from world", softBody.UserObject);
+                if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Removing softbody {0} from world", softBody.UserObject);
                 ((BulletSharp.SoftBody.SoftRigidDynamicsWorld)m_world).RemoveSoftBody(softBody);
                 if (softBody.UserObject is BCollisionObject) ((BCollisionObject)softBody.UserObject).isInWorld = false;
             }
@@ -550,33 +554,36 @@ protected virtual void _InitializePhysicsWorld() {
         }
 
         protected void Dispose(bool disposing) {
-			BDebug.Log(debugType, "BDynamicsWorld Disposing physics.");
+                if (debugType <= BDebug.DebugType.Debug) Debug.Log("BDynamicsWorld Disposing physics.");
 
-            if (m_world != null) {
+                if (m_world != null) {
                 //remove/dispose constraints
                 int i;
                 if (_ddWorld != null)
                 {
+                    if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Removing Constraints {0}", _ddWorld.NumConstraints);
                     for (i = _ddWorld.NumConstraints - 1; i >= 0; i--)
                     {
                         TypedConstraint constraint = _ddWorld.GetConstraint(i);
                         _ddWorld.RemoveConstraint(constraint);
-                        if (constraint.Userobject is BTypedConstraint) ((BTypedConstraint)constraint.Userobject).isInWorld = false;
-                        if ((debugType & BDebug.DebugType.Debug) == BDebug.DebugType.Debug) Debug.LogFormat("Removed Constaint {0}", constraint.Userobject);
+                        if (constraint.Userobject is BTypedConstraint) ((BTypedConstraint)constraint.Userobject).m_isInWorld = false;
+                        if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Removed Constaint {0}", constraint.Userobject);
                         constraint.Dispose();
                     }
                 }
 
+                if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Removing Collision Objects {0}", _ddWorld.NumCollisionObjects);
                 //remove the rigidbodies from the dynamics world and delete them
                 for (i = m_world.NumCollisionObjects - 1; i >= 0; i--) {
                     CollisionObject obj = m_world.CollisionObjectArray[i];
                     RigidBody body = obj as RigidBody;
                     if (body != null && body.MotionState != null) {
+                        Debug.Assert(body.NumConstraintRefs == 0, "Rigid body still had constraints");
                         body.MotionState.Dispose();
                     }
                     m_world.RemoveCollisionObject(obj);
                     if (obj.UserObject is BCollisionObject) ((BCollisionObject)obj.UserObject).isInWorld = false;
-                    if ((debugType & BDebug.DebugType.Debug) == BDebug.DebugType.Debug) Debug.LogFormat("Removed CollisionObject {0}", obj.UserObject);
+                    if (debugType <= BDebug.DebugType.Debug) Debug.LogFormat("Removed CollisionObject {0}", obj.UserObject);
                     obj.Dispose();
                 }
 

UnityProject/Assets/BulletUnity/Scripts/BCollisionObject.cs

@@ -354,20 +354,46 @@ protected override void Awake() {
             }
         }
 
+        protected override void OnDisable()
+        {
+            if (m_rigidBody != null && isInWorld) {
+                //all constraints using RB must be disabled before rigid body is disabled
+                for (int i = m_rigidBody.NumConstraintRefs - 1; i >= 0; i--)
+                {
+                    BTypedConstraint btc = (BTypedConstraint) m_rigidBody.GetConstraintRef(i).Userobject;
+                    Debug.Assert(btc != null);
+                    btc.enabled = false; //should remove it from the scene
+                }
+            }
+            base.OnDisable();
+        }
+
         protected override void AddObjectToBulletWorld()
         {
             BPhysicsWorld.Get().AddRigidBody(this);
         }
 
         protected override void RemoveObjectFromBulletWorld()
         {
-            BPhysicsWorld.Get().RemoveRigidBody((RigidBody)m_collisionObject);
+            BPhysicsWorld pw = BPhysicsWorld.Get();
+            if (pw != null && m_rigidBody != null && isInWorld)
+            {
+                Debug.Assert(m_rigidBody.NumConstraintRefs == 0, "Removing rigid body that still had constraints. Remove constraints first.");
+                //constraints must be removed before rigid body is removed
+                pw.RemoveRigidBody((RigidBody)m_collisionObject);
+            }
         }
 
         protected override void Dispose(bool isdisposing) {
             if (isInWorld && isdisposing && m_rigidBody != null) {
                 BPhysicsWorld pw = BPhysicsWorld.Get();
                 if (pw != null && pw.world != null) {
+                    //constraints must be removed before rigid body is removed
+                    for (int i = m_rigidBody.NumConstraintRefs; i > 0; i--)
+                    {
+                        BTypedConstraint tc = (BTypedConstraint) m_rigidBody.GetConstraintRef(i - 1).Userobject;
+                        ((DiscreteDynamicsWorld)pw.world).RemoveConstraint(tc.GetConstraint());
+                    }
                     ((DiscreteDynamicsWorld) pw.world).RemoveRigidBody(m_rigidBody);
                 }
             }