Modified joint local transform calculations and ensured that rigidbodies notify parent joints when their transforms change

BansheeCore/Include/BsCJoint.h

@@ -88,12 +88,17 @@ namespace BansheeEngine
 		void onTransformChanged(TransformChangedFlags flags) override;
 
     protected:
+		friend class CRigidbody;
+
 		/** Creates the internal representation of the Joint for use by the component. */
 		virtual SPtr<Joint> createInternal() = 0;
 
 		/** Creates the internal representation of the Joint and restores the values saved by the Component. */
 		virtual void restoreInternal();
 
+		/** Notifies the joint that one of the attached rigidbodies moved and that its transform needs updating. */
+		void notifyRigidbodyMoved(const HRigidbody& body);
+
 		/** Updates the local transform for the specified body attached to the joint. */
 		void updateTransform(JointBody body);
 

BansheeCore/Include/BsCRigidbody.h

@@ -153,6 +153,9 @@ namespace BansheeEngine
 		/** Returns the Rigidbody implementation wrapped by this component. */
 		SPtr<Rigidbody> _getInternal() const { return mInternal; }
 
+		/** Sets that joint that this rigidbody is attached to. Allows the rigidbody to notify the joint when it moves. */
+		void _setJoint(const HJoint& joint) { mParentJoint = joint; }
+
 		/** @copydoc Rigidbody::_updateMassDistribution */
 		inline void _updateMassDistribution();
 
@@ -210,6 +213,7 @@ namespace BansheeEngine
 
 		SPtr<Rigidbody> mInternal;
 		Vector<HCollider> mChildren;
+		HJoint mParentJoint;
 
 		UINT32 mPositionSolverCount = 4;
 		UINT32 mVelocitySolverCount = 1;

BansheeCore/Include/BsCorePrerequisites.h

@@ -264,6 +264,13 @@ namespace BansheeEngine
 	class SliderJoint;
 	class D6Joint;
 	class CharacterController;
+	class CJoint;
+	class CHingeJoint;
+	class CDistanceJoint;
+	class CFixedJoint;
+	class CSphericalJoint;
+	class CSliderJoint;
+	class CD6Joint;
 	// Asset import
 	class SpecificImporter;
 	class Importer;
@@ -504,6 +511,13 @@ namespace BansheeEngine
 	typedef GameObjectHandle<CSphereCollider> HSphereCollider;
 	typedef GameObjectHandle<CCapsuleCollider> HCapsuleCollider;
 	typedef GameObjectHandle<CPlaneCollider> HPlaneCollider;
+	typedef GameObjectHandle<CJoint> HJoint;
+	typedef GameObjectHandle<CHingeJoint> HHingeJoint;
+	typedef GameObjectHandle<CSliderJoint> HSliderJoint;
+	typedef GameObjectHandle<CDistanceJoint> HDistanceJoint;
+	typedef GameObjectHandle<CSphericalJoint> HSphericalJoint;
+	typedef GameObjectHandle<CFixedJoint> HFixedJoint;
+	typedef GameObjectHandle<CD6Joint> HD6Joint;
 
 	/** @} */
 }

BansheeCore/Source/BsCJoint.cpp

@@ -3,6 +3,7 @@
 #include "BsCJoint.h"
 #include "BsCRigidbody.h"
 #include "BsSceneObject.h"
+#include "BsPhysics.h"
 #include "BsCJointRTTI.h"
 
 using namespace std::placeholders;
@@ -27,8 +28,14 @@ namespace BansheeEngine
 		if (mBodies[(int)body] == value)
 			return;
 
+		if (mBodies[(int)body] != nullptr)
+			mBodies[(int)body]->_setJoint(HJoint());
+
 		mBodies[(int)body] = value;
 
+		if (value != nullptr)
+			mBodies[(int)body]->_setJoint(mThisHandle);
+
 		if(mInternal != nullptr)
 		{
 			Rigidbody* rigidbody = nullptr;
@@ -117,6 +124,12 @@ namespace BansheeEngine
 
 	void CJoint::onDestroyed()
 	{
+		if (mBodies[0] != nullptr)
+			mBodies[0]->_setJoint(HJoint());
+
+		if (mBodies[1] != nullptr)
+			mBodies[1]->_setJoint(HJoint());
+
 		// This should release the last reference and destroy the internal joint
 		mInternal = nullptr;
 	}
@@ -137,7 +150,15 @@ namespace BansheeEngine
 		if (!SO()->getActive())
 			return;
 
-		// TODO - This might get called during physics update, perhaps avoid that?
+		// We're ignoring this during physics update because it would cause problems if the joint itself was moved by physics
+		// Note: This isn't particularily correct because if the joint is being moved by physics but the rigidbodies
+		// themselves are not parented to the joint, the transform will need updating. However I'm leaving it up to the
+		// user to ensure rigidbodies are always parented to the joint in such a case (It's an unlikely situation that
+		// I can't think of an use for - joint transform will almost always be set as an initialization step and not a 
+		// physics response).
+		if (gPhysics()._isUpdateInProgress())
+			return;
+
 		updateTransform(JointBody::A);
 		updateTransform(JointBody::B);
 	}
@@ -174,29 +195,46 @@ namespace BansheeEngine
 		updateTransform(JointBody::B);
 	}
 
+	void CJoint::notifyRigidbodyMoved(const HRigidbody& body)
+	{
+		// If physics update is in progress do nothing, as its the joint itself that's probably moving the body
+		if (gPhysics()._isUpdateInProgress())
+			return;
+
+		if (mBodies[0] == body)
+			updateTransform(JointBody::A);
+		else if (mBodies[1] == body)
+			updateTransform(JointBody::B);
+		else
+			assert(false); // Not allowed to happen
+	}
+
 	void CJoint::updateTransform(JointBody body)
 	{
-		Vector3 localPos;
-		Quaternion localRot;
+		Vector3 parentPos = SO()->getWorldPosition();
+		Quaternion parentRot = SO()->getWorldRotation();
+		
+		// Add local position/rotation offset to the joint's transform
+		Vector3 worldPos = parentPos + parentRot.rotate(mPositions[(int)body]);
+		Quaternion worldRot = parentRot * mRotations[(int)body];
 
-		localPos = mPositions[(int)body];
-		localRot = mRotations[(int)body];
+		// Transform body's world position/rotation into space local to the joint + offset space
+		Vector3 bodyPos;
+		Quaternion bodyRot;
 
-		// Transform to world space of the related body
 		HRigidbody rigidbody = mBodies[(int)body];
 		if(rigidbody != nullptr)
 		{
-			localRot = rigidbody->SO()->getWorldRotation() * localRot;
-			localPos = localRot.rotate(localPos) + rigidbody->SO()->getWorldPosition();
+			bodyPos = rigidbody->SO()->getWorldPosition(); 
+			bodyRot = rigidbody->SO()->getWorldRotation();
 		}
 
-		// Transform to space local to the joint
-		Quaternion invRotation = SO()->getWorldRotation().inverse();
+		Quaternion invRotation = worldRot.inverse();
 
-		localPos = invRotation.rotate(localPos - SO()->getWorldPosition());
-		localRot = invRotation * localRot;
+		bodyPos = invRotation.rotate(bodyPos - worldPos);
+		bodyRot = invRotation * bodyRot;
 
-		mInternal->setTransform(body, localPos, localRot);
+		mInternal->setTransform(body, bodyPos, bodyRot);
 	}
 
 	void CJoint::triggerOnJointBroken()

BansheeCore/Source/BsCRigidbody.cpp

@@ -3,8 +3,7 @@
 #include "BsCRigidbody.h"
 #include "BsSceneObject.h"
 #include "BsCCollider.h"
-#include "BsCMeshCollider.h"
-#include "BsPhysicsMesh.h"
+#include "BsCJoint.h"
 #include "BsCRigidbodyRTTI.h"
 
 using namespace std::placeholders;
@@ -423,6 +422,9 @@ namespace BansheeEngine
 		}
 
 		mInternal->setTransform(SO()->getWorldPosition(), SO()->getWorldRotation());
+
+		if (mParentJoint != nullptr)
+			mParentJoint->notifyRigidbodyMoved(mThisHandle);
 	}
 
 	RTTITypeBase* CRigidbody::getRTTIStatic()