Rigidbody automatic mass distribution calculation added (WIP)

BansheeCore/Include/BsCollider.h

@@ -21,6 +21,9 @@ namespace BansheeEngine
 		inline void setIsTrigger(bool value);
 		inline bool getIsTrigger() const;
 
+		inline void setMass(float mass);
+		inline float getMass() const;
+
 		inline void setRigidbody(const SPtr<Rigidbody>& rigidbody);
 		inline SPtr<Rigidbody> getRigidbody() const;
 

BansheeCore/Include/BsFCollider.h

@@ -20,6 +20,10 @@ namespace BansheeEngine
 		virtual void setIsTrigger(bool value) = 0;
 		virtual bool getIsTrigger() const = 0;
 
+		// Not used for triggers, only relevant if parent rigidbody uses child mass
+		virtual void setMass(float mass) { mMass = mass; }
+		virtual float getMass() const { return mMass; }
+
 		virtual void setRigidbody(const SPtr<Rigidbody>& rigidbody);
 		virtual SPtr<Rigidbody> getRigidbody() const { return mRigidbody; }
 
@@ -40,6 +44,7 @@ namespace BansheeEngine
 	protected:
 		bool mIsActive = true;
 		UINT64 mLayer = 1;
+		float mMass = 1.0f;
 
 		SPtr<Rigidbody> mRigidbody;
 		HPhysicsMaterial mMaterial;

BansheeCore/Include/BsRigidbody.h

@@ -24,6 +24,15 @@ namespace BansheeEngine
 	class BS_CORE_EXPORT Rigidbody
 	{
 	public:
+		enum class Flag
+		{
+			None = 0x00,
+			/** Automatically calculate center of mass transform and inertia tensors from child shapes (colliders) */
+			AutoTensors = 0x01,
+			/** Calculate mass distribution from child shapes (colliders). Only relevant when auto-tensors is on. */
+			AutoMass = 0x02,
+		};
+
 		virtual ~Rigidbody();
 
 		virtual void move(const Vector3& position) = 0;
@@ -80,6 +89,9 @@ namespace BansheeEngine
 		virtual void setIsActive(bool value);
 		virtual bool getIsActive() const { return mIsActive; }
 
+		virtual void setFlags(Flag flags);
+		virtual Flag getFlags() const { return mFlags; }
+
 		virtual void addForce(const Vector3& force, ForceMode mode = ForceMode::Force) = 0;
 		virtual void addTorque(const Vector3& torque, ForceMode mode = ForceMode::Force) = 0;
 		virtual void addForceAtPoint(const Vector3& force, const Vector3& position, 
@@ -92,6 +104,8 @@ namespace BansheeEngine
 		Event<void(const CollisionData&)> onCollisionBegin;
 		Event<void(const CollisionData&)> onCollisionStay;
 		Event<void(const CollisionData&)> onCollisionEnd;
+
+		virtual void _updateMassDistribution() { }
 	protected:
 		friend class FCollider;
 
@@ -99,6 +113,7 @@ namespace BansheeEngine
 		virtual void removeCollider(FCollider* collider);
 
 		bool mIsActive = true;
+		Flag mFlags = Flag::None;
 		Vector<FCollider*> mColliders;
 	};
 }

BansheeCore/Source/BsCollider.cpp

@@ -23,6 +23,16 @@ namespace BansheeEngine
 		mInternal->setIsTrigger(value);
 	}
 
+	void Collider::setMass(float mass)
+	{
+		mInternal->setMass(mass);
+	}
+
+	float Collider::getMass() const
+	{
+		return mInternal->getMass();
+	}
+
 	bool Collider::getIsTrigger() const
 	{
 		return mInternal->getIsTrigger();

BansheeCore/Source/BsRigidbody.cpp

@@ -25,6 +25,15 @@ namespace BansheeEngine
 		mIsActive = value;
 	}
 
+	void Rigidbody::setFlags(Flag flags)
+	{
+		if (mFlags == flags)
+			return;
+
+		mFlags = flags;
+		_updateMassDistribution();
+	}
+
 	SPtr<Rigidbody> Rigidbody::create(const Vector3& position, const Quaternion& rotation)
 	{
 		return gPhysics().createRigidbody(position, rotation);

BansheePhysX/Include/BsFPhysXCollider.h

@@ -22,6 +22,8 @@ namespace BansheeEngine
 		void setIsTrigger(bool value) override;
 		bool getIsTrigger() const override;
 
+		void setMass(float mass) override;
+
 		/**
 		 * Determines how far apart do two shapes need to be away from each other before the physics runtime starts 
 		 * generating repelling impulse for them. This distance will be the sum of contact offsets of the two interacting

BansheePhysX/Include/BsPhysXRigidbody.h

@@ -76,6 +76,7 @@ namespace BansheeEngine
 
 		Vector3 getVelocityAtPoint(const Vector3& point) const override;
 
+		void _updateMassDistribution() override;
 	private:
 		physx::PxRigidDynamic* mInternal;
 	};

BansheePhysX/Source/BsFPhysXCollider.cpp

@@ -70,6 +70,14 @@ namespace BansheeEngine
 		mShape->setContactOffset(value);
 	}
 
+	void FPhysXCollider::setMass(float mass)
+	{
+		FCollider::setMass(mass);
+
+		if (mRigidbody != nullptr)
+			mRigidbody->_updateMassDistribution(); // Note: Perhaps I can just mark mass distribution as dirty and recalculate it delayed, in case a lot of colliders change
+	}
+
 	float FPhysXCollider::getContactOffset()
 	{
 		return mShape->getContactOffset();

BansheePhysX/Source/BsPhysXRigidbody.cpp

@@ -1,6 +1,8 @@
 #include "BsPhysXRigidbody.h"
+#include "BsCollider.h"
 #include "PxRigidDynamic.h"
 #include "PxScene.h"
+#include "extensions\PxRigidBodyExt.h"
 
 using namespace physx;
 
@@ -87,6 +89,12 @@ namespace BansheeEngine
 
 	void PhysXRigidbody::setMass(float mass)
 	{
+		if(((UINT32)mFlags & (UINT32)Flag::AutoMass) != 0)
+		{
+			LOGWRN("Attempting to set Rigidbody mass, but it has automatic mass calculation turned on.");
+			return;
+		}
+
 		mInternal->setMass(mass);
 	}
 
@@ -182,6 +190,12 @@ namespace BansheeEngine
 
 	void PhysXRigidbody::setInertiaTensor(const Vector3& tensor)
 	{
+		if (((UINT32)mFlags & (UINT32)Flag::AutoTensors) != 0)
+		{
+			LOGWRN("Attempting to set Rigidbody inertia tensor, but it has automatic tensor calculation turned on.");
+			return;
+		}
+
 		mInternal->setMassSpaceInertiaTensor(toPxVector(tensor));
 	}
 
@@ -202,6 +216,12 @@ namespace BansheeEngine
 
 	void PhysXRigidbody::setCenterOfMass(const Vector3& position, const Quaternion& rotation)
 	{
+		if (((UINT32)mFlags & (UINT32)Flag::AutoTensors) != 0)
+		{
+			LOGWRN("Attempting to set Rigidbody center of mass, but it has automatic tensor calculation turned on.");
+			return;
+		}
+
 		mInternal->setCMassLocalPose(toPxTransform(position, rotation));
 	}
 
@@ -288,4 +308,31 @@ namespace BansheeEngine
 
 		return fromPxVector(velocity);
 	}
+
+	void PhysXRigidbody::_updateMassDistribution() 
+	{
+		if (((UINT32)mFlags & (UINT32)Flag::AutoTensors) == 0)
+			return;
+
+		if (((UINT32)mFlags & (UINT32)Flag::AutoMass) == 0)
+		{
+			PxRigidBodyExt::setMassAndUpdateInertia(*mInternal, mInternal->getMass());
+		}
+		else
+		{
+			UINT32 numShapes = mInternal->getNbShapes();
+
+			PxShape** shapes = (PxShape**)bs_stack_alloc(sizeof(PxShape*) * numShapes);
+			mInternal->getShapes(shapes, numShapes);
+
+			float* masses = (float*)bs_stack_alloc(sizeof(float) * numShapes);
+			for (UINT32 i = 0; i < numShapes; i++)
+				masses[i] = ((Collider*)shapes[i]->userData)->getMass();
+
+			PxRigidBodyExt::setMassAndUpdateInertia(*mInternal, masses, numShapes);
+
+			bs_stack_free(masses);
+			bs_stack_free(shapes);
+		}
+	}
 }