Update linked SceneObject from Rigidbody

Maintain a priority sorted list of Rigidbodies

BansheeCore/Include/BsCorePrerequisites.h

@@ -423,7 +423,10 @@ namespace BansheeEngine
 
 namespace BansheeEngine
 {
-	// Resource handles
+	/** @addtogroup Resources
+	 *  @{
+	 */
+
 	typedef ResourceHandle<Resource> HResource;
 	typedef ResourceHandle<Texture> HTexture;
 	typedef ResourceHandle<Mesh> HMesh;
@@ -434,6 +437,25 @@ namespace BansheeEngine
 	typedef ResourceHandle<Prefab> HPrefab;
 	typedef ResourceHandle<StringTable> HStringTable;
 	typedef ResourceHandle<PhysicsMaterial> HPhysicsMaterial;
+
+	/** @} */
+}
+
+
+#include "BsGameObjectHandle.h"
+
+namespace BansheeEngine
+{
+	/** @addtogroup Scene
+	 *  @{
+	 */
+
+	// Game object handles
+	typedef GameObjectHandle<GameObject> HGameObject;
+	typedef GameObjectHandle<SceneObject> HSceneObject;
+	typedef GameObjectHandle<Component> HComponent;
+
+	/** @} */
 }
 
 namespace BansheeEngine

BansheeCore/Include/BsGameObject.h

@@ -13,19 +13,6 @@ namespace BansheeEngine
 
 	/** @cond INTERNAL */
 
-	/**	Contains instance data that is held by all GameObject handles. */
-	struct GameObjectInstanceData
-	{
-		GameObjectInstanceData()
-		:mInstanceId(0), object(nullptr)
-		{ }
-
-		std::shared_ptr<GameObject> object;
-		UINT64 mInstanceId;
-	};
-
-	typedef std::shared_ptr<GameObjectInstanceData> GameObjectInstanceDataPtr;
-
 	/** Flags used for notifying child scene object and components when a transform has been changed. */
 	enum TransformChangedFlags
 	{
@@ -128,21 +115,5 @@ namespace BansheeEngine
 		virtual RTTITypeBase* getRTTI() const override;
 	};
 
-	/** @} */
-}
-
-#include "BsGameObjectHandle.h"
-
-namespace BansheeEngine
-{
-	/** @addtogroup Scene
-	 *  @{
-	 */
-
-	// Game object handles
-	typedef GameObjectHandle<GameObject> HGameObject;
-	typedef GameObjectHandle<SceneObject> HSceneObject;
-	typedef GameObjectHandle<Component> HComponent;
-
 	/** @} */
 }

BansheeCore/Include/BsGameObjectHandle.h

@@ -15,6 +15,19 @@ namespace BansheeEngine
 
 	/** @cond INTERNAL */
 
+	/**	Contains instance data that is held by all GameObject handles. */
+	struct GameObjectInstanceData
+	{
+		GameObjectInstanceData()
+			:mInstanceId(0), object(nullptr)
+		{ }
+
+		std::shared_ptr<GameObject> object;
+		UINT64 mInstanceId;
+	};
+
+	typedef std::shared_ptr<GameObjectInstanceData> GameObjectInstanceDataPtr;
+
 	/**	Internal data shared between GameObject handles. */
 	struct BS_CORE_EXPORT GameObjectHandleData
 	{
@@ -54,11 +67,7 @@ namespace BansheeEngine
 		 *							completely inaccessible (fully destroyed). If this is true this method will return true
 		 *							if object is completely inaccessible or if it is just queued for destruction.
 		 */
-		bool isDestroyed(bool checkQueued = false) const
-		{
-			return mData->mPtr == nullptr || mData->mPtr->object == nullptr 
-				|| (checkQueued && mData->mPtr->object->_getIsDestroyed());
-		}
+		bool isDestroyed(bool checkQueued = false) const;
 
 		/**	Returns the instance ID of the object the handle is referencing. */
 		UINT64 getInstanceId() const { return mData->mPtr != nullptr ? mData->mPtr->mInstanceId : 0; }

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, UINT32 priority = 0) = 0;
+		virtual SPtr<Rigidbody> createRigidbody(const HSceneObject& linkedSO) = 0;
 
 		virtual SPtr<BoxCollider> createBoxCollider(float extentX, float extentY, float extentZ,
 			const Vector3& position, const Quaternion& rotation) = 0;
@@ -31,6 +31,8 @@ namespace BansheeEngine
 		void toggleCollision(UINT64 groupA, UINT64 groupB, bool enabled);
 		bool isCollisionEnabled(UINT64 groupA, UINT64 groupB) const;
 
+		bool _isUpdateInProgress() const { return mUpdateInProgress; }
+
 		static const UINT64 CollisionMapSize = 64;
 	protected:
 		friend class Rigidbody;
@@ -42,7 +44,8 @@ namespace BansheeEngine
 		mutable Mutex mMutex;
 		bool mCollisionMap[CollisionMapSize][CollisionMapSize];
 
-		Vector<Vector<Rigidbody*>> mRigidbodies;
+		bool mUpdateInProgress = false;
+		Vector<Vector<Rigidbody*>> mRigidbodies; // TODO: Unused for now, but keeping it here just in case I change the design. Remove later.
 
 		const static UINT32 MAX_PRIORITY = 128;
 	};

BansheeCore/Include/BsRigidbody.h

@@ -33,7 +33,7 @@ namespace BansheeEngine
 			AutoMass = 0x02,
 		};
 
-		Rigidbody(UINT32 priority);
+		Rigidbody(const HSceneObject& linkedSO);
 		virtual ~Rigidbody();
 
 		virtual void move(const Vector3& position) = 0;
@@ -100,7 +100,7 @@ namespace BansheeEngine
 
 		virtual Vector3 getVelocityAtPoint(const Vector3& point) const = 0;
 
-		static SPtr<Rigidbody> create(const Vector3& position, const Quaternion& rotation);
+		static SPtr<Rigidbody> create(const HSceneObject& linkedSO);
 
 		Event<void(const CollisionData&)> onCollisionBegin;
 		Event<void(const CollisionData&)> onCollisionStay;
@@ -110,6 +110,7 @@ namespace BansheeEngine
 
 		void _setPriority(UINT32 priority);
 		void _setPhysicsId(UINT32 id) { mPhysicsId = id; }
+		void _setTransform(const Vector3& position, const Quaternion& rotation);
 	protected:
 		friend class FCollider;
 
@@ -120,6 +121,7 @@ namespace BansheeEngine
 		Flag mFlags = Flag::None;
 		UINT32 mPriority = 0;
 		UINT32 mPhysicsId = 0;
+		HSceneObject mLinkedSO;
 		Vector<FCollider*> mColliders;
 	};
 }

BansheeCore/Include/BsUtility.h

@@ -36,6 +36,9 @@ namespace BansheeEngine
 		 */
 		static Vector<ResourceDependency> findResourceDependencies(IReflectable& object, bool recursive = true);
 
+		/** Calculates how deep in the scene object hierarchy is the provided object. Zero means root. */
+		static UINT32 getSceneObjectDepth(const HSceneObject& so);
+
 	private:
 		/**
 		 * Helper method for for recursion when finding resource dependencies.

BansheeCore/Source/BsGameObjectHandle.cpp

@@ -28,6 +28,12 @@ namespace BansheeEngine
 		mData = bs_shared_ptr_new<GameObjectHandleData>(nullptr);
 	}
 
+	bool GameObjectHandleBase::isDestroyed(bool checkQueued) const
+	{
+		return mData->mPtr == nullptr || mData->mPtr->object == nullptr
+			|| (checkQueued && mData->mPtr->object->_getIsDestroyed());
+	}
+
 	void GameObjectHandleBase::_resolve(const GameObjectHandleBase& object) 
 	{ 
 		mData->mPtr = object.mData->mPtr;

BansheeCore/Source/BsRigidbody.cpp

@@ -1,12 +1,15 @@
 #include "BsRigidbody.h"
 #include "BsPhysics.h"
+#include "BsSceneObject.h"
+#include "BsUtility.h"
 
 namespace BansheeEngine
 {
-	Rigidbody::Rigidbody(UINT32 priority)
-		:mPriority(priority)
+	Rigidbody::Rigidbody(const HSceneObject& linkedSO)
+		:mLinkedSO(linkedSO)
 	{
-		gPhysics().registerRigidbody(this, priority);
+		mPriority = Utility::getSceneObjectDepth(linkedSO);
+		gPhysics().registerRigidbody(this, mPriority);
 	}
 
 	Rigidbody::~Rigidbody()
@@ -48,8 +51,14 @@ namespace BansheeEngine
 		mPriority = priority;
 	}
 
-	SPtr<Rigidbody> Rigidbody::create(const Vector3& position, const Quaternion& rotation)
+	void Rigidbody::_setTransform(const Vector3& position, const Quaternion& rotation)
 	{
-		return gPhysics().createRigidbody(position, rotation);
+		mLinkedSO->setWorldPosition(position);
+		mLinkedSO->setWorldRotation(rotation);
+	}
+
+	SPtr<Rigidbody> Rigidbody::create(const HSceneObject& linkedSO)
+	{
+		return gPhysics().createRigidbody(linkedSO);
 	}
 }

BansheeCore/Source/BsUtility.cpp

@@ -2,7 +2,7 @@
 //**************** Copyright (c) 2016 Marko Pintera ([email protected]). All rights reserved. **********************//
 #include "BsUtility.h"
 #include "BsRTTIType.h"
-#include "BsFileSystem.h"
+#include "BsSceneObject.h"
 
 namespace BansheeEngine
 {
@@ -22,6 +22,20 @@ namespace BansheeEngine
 		return dependencyList;
 	}
 
+	UINT32 Utility::getSceneObjectDepth(const HSceneObject& so)
+	{
+		HSceneObject parent = so->getParent();
+		
+		UINT32 depth = 0;
+		while (parent != nullptr)
+		{
+			depth++;
+			parent = parent->getParent();
+		}
+
+		return depth;
+	}
+
 	void Utility::findResourceDependenciesInternal(IReflectable& obj, bool recursive, Map<String, ResourceDependency>& dependencies)
 	{
 		RTTITypeBase* rtti = obj.getRTTI();

BansheePhysX/Include/BsPhysX.h

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

BansheePhysX/Include/BsPhysXRigidbody.h

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

BansheePhysX/Source/BsPhysX.cpp

@@ -301,6 +301,7 @@ namespace BansheeEngine
 		sceneDesc.cpuDispatcher = &gPhysXCPUDispatcher;
 		sceneDesc.filterShader = PhysXFilterShader;
 		sceneDesc.simulationEventCallback = &gPhysXEventCallback;
+		sceneDesc.flags = PxSceneFlag::eENABLE_ACTIVETRANSFORMS;
 
 		mScene = mPhysics->createScene(sceneDesc);
 		mSimulationStep = input.timeStep;
@@ -320,6 +321,8 @@ namespace BansheeEngine
 
 	void PhysX::update()
 	{
+		mUpdateInProgress = true;
+
 		float nextFrameTime = mLastSimulationTime + mSimulationStep;
 		float curFrameTime = gTime().getTime();
 		if(curFrameTime < nextFrameTime)
@@ -332,13 +335,26 @@ namespace BansheeEngine
 		float simulationAmount = curFrameTime - mLastSimulationTime;
 		while (simulationAmount >= mSimulationStep) // In case we're running really slow multiple updates might be needed
 		{
-			// TODO - Consider delaying fetchResults one frame. This could improve performance but at a cost to input latency.
+			// Note: Consider delaying fetchResults one frame. This could improve performance because Physics update would be
+			//       able to run parallel to the simulation thread, but at a cost to input latency.
 			// TODO - Provide a scratch buffer for the simulation (use the frame allocator, but I must extend it so it allocates
 			//	      on a 16 byte boundary).
 			mScene->simulate(mSimulationStep);
 			mScene->fetchResults(true);
 
-			// TODO - Update all rigidbody transfroms from their PhsyX state
+			// Update rigidbodies with new transforms
+			PxU32 numActiveTransforms;
+			const PxActiveTransform* activeTransforms = mScene->getActiveTransforms(numActiveTransforms);
+
+			for (PxU32 i = 0; i < numActiveTransforms; i++)
+			{
+				Rigidbody* rigidbody = static_cast<Rigidbody*>(activeTransforms[i].userData);
+				const PxTransform& transform = activeTransforms[i].actor2World;
+
+				// Note: Make this faster, avoid dereferencing Rigidbody and attempt to access pos/rot destination directly,
+				//       use non-temporal writes
+				rigidbody->_setTransform(fromPxVector(transform.p), fromPxQuaternion(transform.q));
+			}
 
 			simulationAmount -= mSimulationStep;
 		}
@@ -346,6 +362,7 @@ namespace BansheeEngine
 		// TODO - Consider extrapolating for the remaining "simulationAmount" value
 
 		mLastSimulationTime = curFrameTime; 
+		mUpdateInProgress = false;
 
 		triggerEvents();
 	}
@@ -442,9 +459,9 @@ namespace BansheeEngine
 		return bs_shared_ptr_new<PhysXMaterial>(mPhysics, staticFriction, dynamicFriction, restitution);
 	}
 
-	SPtr<Rigidbody> PhysX::createRigidbody(const Vector3& position, const Quaternion& rotation, UINT32 priority)
+	SPtr<Rigidbody> PhysX::createRigidbody(const HSceneObject& linkedSO)
 	{
-		return bs_shared_ptr_new<PhysXRigidbody>(mPhysics, mScene, position, rotation, priority);
+		return bs_shared_ptr_new<PhysXRigidbody>(mPhysics, mScene, linkedSO);
 	}
 
 	SPtr<BoxCollider> PhysX::createBoxCollider(float extentX, float extentY, float extentZ, const Vector3& position, 

BansheePhysX/Source/BsPhysXRigidbody.cpp

@@ -1,5 +1,6 @@
 #include "BsPhysXRigidbody.h"
 #include "BsCollider.h"
+#include "BsSceneObject.h"
 #include "PxRigidDynamic.h"
 #include "PxScene.h"
 #include "extensions\PxRigidBodyExt.h"
@@ -38,13 +39,14 @@ namespace BansheeEngine
 		return PxForceMode::eFORCE;
 	}
 
-	PhysXRigidbody::PhysXRigidbody(PxPhysics* physx, PxScene* scene, const Vector3& position, const Quaternion& rotation, 
-		UINT32 priority)
-		:Rigidbody(priority)
+	PhysXRigidbody::PhysXRigidbody(PxPhysics* physx, PxScene* scene, const HSceneObject& linkedSO)
+		:Rigidbody(linkedSO)
 	{
-		PxTransform tfrm = toPxTransform(position, rotation);
+		PxTransform tfrm = toPxTransform(linkedSO->getWorldPosition(), linkedSO->getWorldRotation());
 
 		mInternal = physx->createRigidDynamic(tfrm);
+		mInternal->userData = this;
+
 		scene->addActor(*mInternal);
 	}