Physics now keeps a priority sorted list of all rigidbodies in the scene

BansheeCore/Include/BsPhysics.h

@@ -18,7 +18,7 @@ namespace BansheeEngine
 		virtual void update() = 0;
 
 		virtual SPtr<PhysicsMaterial> createMaterial(float staticFriction, float dynamicFriction, float restitution) = 0;
-		virtual SPtr<Rigidbody> createRigidbody(const Vector3& position, const Quaternion& rotation) = 0;
+		virtual SPtr<Rigidbody> createRigidbody(const Vector3& position, const Quaternion& rotation, UINT32 priority = 0) = 0;
 
 		virtual SPtr<BoxCollider> createBoxCollider(float extentX, float extentY, float extentZ,
 			const Vector3& position, const Quaternion& rotation) = 0;
@@ -33,8 +33,18 @@ namespace BansheeEngine
 
 		static const UINT64 CollisionMapSize = 64;
 	protected:
+		friend class Rigidbody;
+
+		void registerRigidbody(Rigidbody* body, UINT32 priority);
+		void unregisterRigidbody(UINT32 id, UINT32 priority);
+		void updatePriority(UINT32 id, UINT32 oldPriority, UINT32 newPriority);
+
 		mutable Mutex mMutex;
 		bool mCollisionMap[CollisionMapSize][CollisionMapSize];
+
+		Vector<Vector<Rigidbody*>> mRigidbodies;
+
+		const static UINT32 MAX_PRIORITY = 128;
 	};
 
 	/** Provides easier access to Physics. */

BansheeCore/Include/BsRigidbody.h

@@ -33,6 +33,7 @@ namespace BansheeEngine
 			AutoMass = 0x02,
 		};
 
+		Rigidbody(UINT32 priority);
 		virtual ~Rigidbody();
 
 		virtual void move(const Vector3& position) = 0;
@@ -106,6 +107,9 @@ namespace BansheeEngine
 		Event<void(const CollisionData&)> onCollisionEnd;
 
 		virtual void _updateMassDistribution() { }
+
+		void _setPriority(UINT32 priority);
+		void _setPhysicsId(UINT32 id) { mPhysicsId = id; }
 	protected:
 		friend class FCollider;
 
@@ -114,6 +118,8 @@ namespace BansheeEngine
 
 		bool mIsActive = true;
 		Flag mFlags = Flag::None;
+		UINT32 mPriority = 0;
+		UINT32 mPhysicsId = 0;
 		Vector<FCollider*> mColliders;
 	};
 }

BansheeCore/Source/BsPhysics.cpp

@@ -1,5 +1,5 @@
 #include "BsPhysics.h"
-#include "BsPhysicsMaterial.h"
+#include "BsRigidbody.h"
 
 namespace BansheeEngine
 {
@@ -24,6 +24,50 @@ namespace BansheeEngine
 		return mCollisionMap[groupA][groupB];
 	}
 
+	void Physics::registerRigidbody(Rigidbody* body, UINT32 priority)
+	{
+		assert(priority <= MAX_PRIORITY && "Priority value too high");
+
+		if (mRigidbodies.size() <= priority)
+			mRigidbodies.resize(priority + 1);
+
+		UINT32 nextId = (UINT32)mRigidbodies[priority].size();
+		mRigidbodies[priority].push_back(body);
+		body->_setPhysicsId(nextId);
+	}
+
+	void Physics::unregisterRigidbody(UINT32 id, UINT32 priority)
+	{
+		assert(mRigidbodies.size() >= priority);
+
+		auto& rigidbodies = mRigidbodies[priority];
+
+		UINT32 lastId = (UINT32)rigidbodies.size();
+		if (id != lastId)
+		{
+			rigidbodies[id] = rigidbodies[lastId];
+			rigidbodies[id]->_setPhysicsId(id);
+		}
+
+		rigidbodies.erase(rigidbodies.begin() + lastId);
+	}
+
+	void Physics::updatePriority(UINT32 id, UINT32 oldPriority, UINT32 newPriority)
+	{
+		assert(newPriority <= MAX_PRIORITY && "Priority value too high");
+
+		if (oldPriority == newPriority)
+			return;
+
+		assert(mRigidbodies.size() >= oldPriority);
+
+		auto& rigidbodies = mRigidbodies[oldPriority];
+		Rigidbody* rigidbody = rigidbodies[id];
+
+		unregisterRigidbody(id, oldPriority);
+		registerRigidbody(rigidbody, newPriority);
+	}
+
 	Physics& gPhysics()
 	{
 		return Physics::instance();

BansheeCore/Source/BsRigidbody.cpp

@@ -3,9 +3,16 @@
 
 namespace BansheeEngine
 {
+	Rigidbody::Rigidbody(UINT32 priority)
+		:mPriority(priority)
+	{
+		gPhysics().registerRigidbody(this, priority);
+	}
+
 	Rigidbody::~Rigidbody()
 	{
 		// It is assumed that child colliders will keep the parent Rigidbody alive, so we don't need to clear their parents
+		gPhysics().unregisterRigidbody(mPhysicsId, mPriority);
 	}
 
 	void Rigidbody::addCollider(FCollider* collider)
@@ -34,6 +41,13 @@ namespace BansheeEngine
 		_updateMassDistribution();
 	}
 
+	void Rigidbody::_setPriority(UINT32 priority)
+	{
+		gPhysics().updatePriority(mPhysicsId, mPriority, priority);
+
+		mPriority = priority;
+	}
+
 	SPtr<Rigidbody> Rigidbody::create(const Vector3& position, const Quaternion& rotation)
 	{
 		return gPhysics().createRigidbody(position, rotation);

BansheePhysX/Include/BsPhysX.h

@@ -43,7 +43,7 @@ namespace BansheeEngine
 
 		SPtr<PhysicsMaterial> createMaterial(float staticFriction, float dynamicFriction, float restitution) override;
 		SPtr<Rigidbody> createRigidbody(const Vector3& position = Vector3::ZERO,
-			const Quaternion& rotation = Quaternion::IDENTITY) override;
+			const Quaternion& rotation = Quaternion::IDENTITY, UINT32 priority = 0) override;
 
 		SPtr<BoxCollider> createBoxCollider(float extentX, float extentY, float extentZ, const Vector3& position, 
 			const Quaternion& rotation) override;

BansheePhysX/Include/BsPhysXRigidbody.h

@@ -13,7 +13,8 @@ namespace BansheeEngine
 	class PhysXRigidbody : public Rigidbody
 	{
 	public:
-		PhysXRigidbody(physx::PxPhysics* physx, physx::PxScene* scene, const Vector3& position, const Quaternion& rotation);
+		PhysXRigidbody(physx::PxPhysics* physx, physx::PxScene* scene, const Vector3& position, 
+			const Quaternion& rotation, UINT32 priority);
 		~PhysXRigidbody();
 
 		void move(const Vector3& position) override;

BansheePhysX/Source/BsPhysX.cpp

@@ -442,9 +442,9 @@ namespace BansheeEngine
 		return bs_shared_ptr_new<PhysXMaterial>(mPhysics, staticFriction, dynamicFriction, restitution);
 	}
 
-	SPtr<Rigidbody> PhysX::createRigidbody(const Vector3& position, const Quaternion& rotation)
+	SPtr<Rigidbody> PhysX::createRigidbody(const Vector3& position, const Quaternion& rotation, UINT32 priority)
 	{
-		return bs_shared_ptr_new<PhysXRigidbody>(mPhysics, mScene, position, rotation);
+		return bs_shared_ptr_new<PhysXRigidbody>(mPhysics, mScene, position, rotation, priority);
 	}
 
 	SPtr<BoxCollider> PhysX::createBoxCollider(float extentX, float extentY, float extentZ, const Vector3& position, 

BansheePhysX/Source/BsPhysXRigidbody.cpp

@@ -38,7 +38,9 @@ namespace BansheeEngine
 		return PxForceMode::eFORCE;
 	}
 
-	PhysXRigidbody::PhysXRigidbody(PxPhysics* physx, PxScene* scene, const Vector3& position, const Quaternion& rotation)
+	PhysXRigidbody::PhysXRigidbody(PxPhysics* physx, PxScene* scene, const Vector3& position, const Quaternion& rotation, 
+		UINT32 priority)
+		:Rigidbody(priority)
 	{
 		PxTransform tfrm = toPxTransform(position, rotation);