Physics work

include/anki/core/Counters.h

@@ -115,7 +115,7 @@ class TraceManager
 {
 public:
 	Array<ThreadTraceManager*, 32> m_threadData;
-	AtomicU32 m_threadCount = {0};
+	Atomic<U32> m_threadCount = {0};
 	File m_traceFile;
 	Mutex m_fileMtx;
 

include/anki/gl/GlHandle.h

@@ -72,7 +72,7 @@ public:
 	
 		if(b.m_cb)
 		{
-			auto count = b.m_cb->m_refcount.fetch_add(1);
+			auto count = b.m_cb->m_refcount.fetchAdd(1);
 			ANKI_ASSERT(count > 0);
 			(void)count;
 
@@ -142,7 +142,7 @@ public:
 			{
 				cb->m_alloc = alloc;
 				cb->m_del = del;
-				cb->m_refcount = 1;
+				cb->m_refcount.store(1);
 				cb->m_ptr = ptr;
 
 				m_cb = cb;
@@ -188,11 +188,11 @@ public:
 
 			if(cb != nullptr)
 			{
-				cb->m_state = GlHandleState::NEW;
+				cb->m_state.store(GlHandleState::NEW);
 				cb->m_gl = dev;
 				cb->m_alloc = alloc;
 				cb->m_del = del;
-				cb->m_refcount = 1;
+				cb->m_refcount.store(1);
 				cb->m_ptr = ptr;
 
 				m_cb = cb;
@@ -266,7 +266,7 @@ private:
 	{
 	public:
 		Y* m_ptr;
-		AtomicI32 m_refcount;
+		Atomic<I32> m_refcount;
 
 		virtual ~CtrlBlockBase()
 		{}
@@ -376,7 +376,7 @@ private:
 	{
 		if(m_cb)
 		{
-			auto count = m_cb->m_refcount.fetch_sub(1);
+			auto count = m_cb->m_refcount.fetchSub(1);
 			if(count == 1)
 			{
 				m_cb->deletePtr();

include/anki/math/Quat.h

@@ -16,19 +16,18 @@ namespace anki {
 
 /// Quaternion. Used in rotations
 template<typename T>
-class alignas(16) TQuat: public TVec4<T>
+class alignas(16) TQuat: 
+	public TVec<T, 4, typename TVec4Simd<T>::Type, TQuat<T>>
 {
 public:
-	using Base = TVec4<T>;
+	using Base = TVec<T, 4, typename TVec4Simd<T>::Type, TQuat<T>>;
 	
-	using Base::Base;
 	using Base::x;
 	using Base::y;
 	using Base::z;
 	using Base::w;
-	using Base::operator=;
-	using Base::getLengthSquared;
-	using Base::normalize;
+	using Base::normalize; // Shortcut
+	using Base::getLengthSquared; // Shortcut
 
 	/// @name Constructors
 	/// @{
@@ -65,7 +64,7 @@ public:
 	{}
 
 	explicit TQuat(const TVec4<T>& v)
-	:	Base(v)
+	:	Base(v.x(), v.y(), v.z(), v.w())
 	{}
 
 	explicit TQuat(const TMat3<T>& m3)
@@ -109,7 +108,7 @@ public:
 	}
 
 	explicit TQuat(const TMat3x4<T>& m)
-		: TQuat(m.getRotationPart())
+	:	TQuat(m.getRotationPart())
 	{
 		ANKI_ASSERT(isZero<T>(m(0, 3)) && isZero<T>(m(1, 3)) 
 			&& isZero<T>(m(2, 3)));

include/anki/math/Vec3.h

@@ -41,27 +41,27 @@ public:
 	/// @name Constructors
 	/// @{
 	explicit TVec3()
-		: Base()
+	:	Base()
 	{}
 
 	TVec3(const TVec3& b)
-		: Base(b)
+	:	Base(b)
 	{}
 
 	explicit TVec3(const T x_, const T y_, const T z_)
-		: Base(x_, y_, z_)
+	:	Base(x_, y_, z_)
 	{}
 
 	explicit TVec3(const T f)
-		: Base(f)
+	:	Base(f)
 	{}
 
 	explicit TVec3(const T arr[])
-		: Base(arr)
+	:	Base(arr)
 	{}
 
 	explicit TVec3(const TVec2<T>& v, const T z_)
-		: Base(v.x(), v.y(), z_)
+	:	Base(v.x(), v.y(), z_)
 	{}
 	/// @}
 

include/anki/math/Vec4.h

@@ -65,35 +65,35 @@ public:
 	/// @name Constructors
 	/// @{
 	explicit TVec4()
-		: Base()
+	:	Base()
 	{}
 
 	TVec4(const TVec4& b)
-		: Base(b)
+	:	Base(b)
 	{}
 
 	explicit TVec4(const T x_, const T y_, const T z_, const T w_)
-		: Base(x_, y_, z_, w_)
+	:	Base(x_, y_, z_, w_)
 	{}
 
 	explicit TVec4(const T f)
-		: Base(f)
+	:	Base(f)
 	{}
 
 	explicit TVec4(const T arr[])
-		: Base(arr)
+	:	Base(arr)
 	{}
 
 	explicit TVec4(const typename Base::Simd& simd)
-		: Base(simd)
+	:	Base(simd)
 	{}
 
 	explicit TVec4(const TVec2<T>& v, const T z_, const T w_)
-		: Base(v.x(), v.y(), z_, w_)
+	:	Base(v.x(), v.y(), z_, w_)
 	{}
 
 	explicit TVec4(const TVec3<T>& v, const T w_)
-		: Base(v.x(), v.y(), v.z(), w_)
+	:	Base(v.x(), v.y(), v.z(), w_)
 	{}
 	/// @}
 

include/anki/physics/Common.h

@@ -30,6 +30,12 @@ enum class PhysicsMaterialBit: U16
 	PARTICLES = 1 << 3
 };
 ANKI_ENUM_ALLOW_NUMERIC_OPERATIONS(PhysicsMaterialBit, inline)
+
+/// Convert newton quat to AnKi.
+inline void toAnki(Quat& q)
+{
+	q = Quat(q.y(), q.z(), q.w(), q.x());
+}
 /// @}
 
 } // end namespace anki

include/anki/physics/PhysicsPlayerController.h

@@ -35,14 +35,33 @@ public:
 
 	ANKI_USE_RESULT Error create(const Initializer& init);
 
+	// Update the state machine
 	void setVelocity(F32 forwardSpeed, F32 strafeSpeed, F32 jumpSpeed, 
-		const Vec3& forwardDir, const Vec3& gravity, F32 dt);
+		const Vec4& forwardDir)
+	{
+		m_forwardSpeed = forwardSpeed;
+		m_strafeSpeed = strafeSpeed;
+		m_jumpSpeed = jumpSpeed;
+		m_forwardDir = forwardDir;
+	}
+
+	void moveToPosition(const Vec4& position);
+
+	/// @privatesection
+	/// @{
+
+	/// Called by Newton thread to update the controller.
+	static void postUpdateKernelCallback(
+		NewtonWorld* const world, 
+		void* const context, 
+		int threadIndex);
+	/// @}
 
 private:
 	Vec4 m_upDir;
 	Vec4 m_frontDir;
 	Vec4 m_groundPlane;
-	Vec4 m_groundVeloc;
+	Vec4 m_groundVelocity;
 	F32 m_innerRadius;
 	F32 m_outerRadius;
 	F32 m_height;
@@ -56,7 +75,19 @@ private:
 	NewtonCollision* m_upperBodyShape;
 	NewtonBody* m_body;
 
+	// State
+	F32 m_forwardSpeed = 0.0;
+	F32 m_strafeSpeed = 0.0;
+	F32 m_jumpSpeed = 0.0;
+	Vec4 m_forwardDir = Vec4(0.0, 0.0, -1.0, 0.0);
+	Vec4 m_gravity;
+
 	static constexpr F32 MIN_RESTRAINING_DISTANCE = 1.0e-2;
+	static constexpr U DESCRETE_MOTION_STEPS = 8;
+	static constexpr U MAX_CONTACTS = 32;
+	static constexpr U MAX_INTERGRATION_STEPS = 8;
+	static constexpr F32 CONTACT_SKIN_THICKNESS = 0.025;
+	static constexpr U MAX_SOLVER_ITERATIONS = 16;
 
 	void setClimbSlope(F32 ang)
 	{
@@ -64,13 +95,20 @@ private:
 		m_maxSlope = cos(ang);
 	}
 
-	void postUpdate();
+	Vec4 calculateDesiredOmega(const Vec4& headingAngle, F32 dt) const;
 
-	/// Called by Newton thread to update the controller.
-	static void postUpdateKernelCallback(
-		NewtonWorld* const world, 
-		void* const context, 
-		int threadIndex);
+	Vec4 calculateDesiredVelocity(F32 forwardSpeed, F32 strafeSpeed, 
+		F32 verticalSpeed, const Vec4& gravity, F32 dt) const;
+
+	void calculateVelocity(F32 dt);
+
+	F32 calculateContactKinematics(const Vec4& veloc, 
+		const NewtonWorldConvexCastReturnInfo* contactInfo) const;
+
+	void updateGroundPlane(Mat4& matrix, const Mat4& castMatrix, 
+		const Vec4& dst, int threadIndex);
+
+	void postUpdate(F32 dt, int threadIndex);
 };
 /// @}
 

include/anki/physics/PhysicsWorld.h

@@ -47,7 +47,7 @@ public:
 	}
 
 	/// Start asynchronous update.
-	Error updateAsync(F32 timestep);
+	Error updateAsync(F32 dt);
 
 	/// End asynchronous update.
 	void waitUpdate();
@@ -61,6 +61,16 @@ public:
 	}
 
 	void _increaseObjectsMarkedForDeletion(PhysicsObject::Type type);
+
+	const Vec4& getGravity() const
+	{
+		return m_gravity;
+	}
+
+	F32 getDeltaTime() const
+	{
+		return m_dt;
+	}
 	/// @}
 
 private:
@@ -68,16 +78,33 @@ private:
 	List<PhysicsCollisionShape*> m_collisions;
 	List<PhysicsBody*> m_bodies;
 	List<PhysicsPlayerController*> m_playerControllers;
-	Array<AtomicU32, static_cast<U>(PhysicsObject::Type::COUNT)> 
+	Array<Atomic<U32>, static_cast<U>(PhysicsObject::Type::COUNT)> 
 		m_forDeletionCount = {{{0}, {0}, {0}}};
 	mutable NewtonWorld* m_world = nullptr;
+	Vec4 m_gravity = Vec4(0.0, -9.8, 0.0, 0.0);
+	F32 m_dt = 0.0;
 
 	template<typename T, typename TContainer, typename... TArgs>
 	T* newObjectInternal(TContainer& cont, TArgs&&... args);
 
 	template<typename T>
 	void cleanupMarkedForDeletion(
-		List<T*>& container, AtomicU32& count);
+		List<T*>& container, Atomic<U32>& count);
+
+	/// Custom update
+	static void postUpdateCallback(
+		const NewtonWorld* const world, 
+		void* const listenerUserData, F32 dt)
+	{
+		static_cast<PhysicsWorld*>(listenerUserData)->postUpdate(dt);
+	}
+
+	void postUpdate(F32 dt);
+
+	static void destroyCallback(
+		const NewtonWorld* const world, 
+		void* const listenerUserData)
+	{}
 };
 
 //==============================================================================

include/anki/resource/ResourcePointer.h

@@ -129,7 +129,7 @@ private:
 		{}
 
 		Type m_resource;
-		AtomicU32 m_refcount = {1};
+		Atomic<U32> m_refcount = {1};
 		TResourceManager* m_resources = nullptr;
 		char m_uuid[1]; ///< This is part of the UUID
 	};

include/anki/resource/ResourcePointer.inl.h

@@ -114,7 +114,7 @@ void ResourcePointer<T, TResourceManager>::reset()
 {
 	if(m_cb != nullptr)
 	{
-		auto count = m_cb->m_refcount.fetch_sub(1);
+		auto count = m_cb->m_refcount.fetchSub(1);
 		if(count == 2)
 		{
 			m_cb->m_resources->_unregisterResource(*this);
@@ -136,7 +136,7 @@ void ResourcePointer<T, TResourceManager>::copy(const ResourcePointer& b)
 	
 	if(b.m_cb != nullptr)
 	{
-		auto count = b.m_cb->m_refcount.fetch_add(1);
+		auto count = b.m_cb->m_refcount.fetchAdd(1);
 		ANKI_ASSERT(count > 0);
 		(void)count;
 

include/anki/scene/SceneGraph.h

@@ -148,7 +148,7 @@ public:
 
 	void increaseObjectsMarkedForDeletion()
 	{
-		++m_objectsMarkedForDeletionCount;
+		m_objectsMarkedForDeletionCount.fetchAdd(1);
 	}
 
 	/// @privatesection
@@ -189,7 +189,7 @@ private:
 
 	EventManager m_events;
 
-	AtomicU32 m_objectsMarkedForDeletionCount;
+	Atomic<U32> m_objectsMarkedForDeletionCount;
 
 	/// Put a node in the appropriate containers
 	ANKI_USE_RESULT Error registerNode(SceneNode* node);

include/anki/util/Allocator.h

@@ -100,7 +100,7 @@ public:
 			ANKI_LOGF("Initialization failed");
 		}
 
-		m_pool->getRefcount() = 1;
+		m_pool->getRefcount().store(1);
 	}
 
 	/// Destructor
@@ -324,7 +324,7 @@ private:
 		if(b.m_pool)
 		{
 			m_pool = b.m_pool;
-			++m_pool->getRefcount();
+			m_pool->getRefcount().fetchAdd(1);
 		}
 	}
 
@@ -332,8 +332,8 @@ private:
 	{
 		if(m_pool)
 		{
-			auto count = --m_pool->getRefcount();
-			if(count == 0)
+			auto count = m_pool->getRefcount().fetchSub(1);
+			if(count == 1)
 			{
 				auto allocCb = m_pool->getAllocationCallback();
 				auto ud = m_pool->getAllocationCallbackUserData();

include/anki/util/Atomic.h

@@ -7,24 +7,114 @@
 #define ANKI_UTIL_ATOMIC_H
 
 #include "anki/util/StdTypes.h"
-#include <atomic>
+#include "anki/util/NonCopyable.h"
 
 namespace anki {
 
 /// @addtogroup util_other
 /// @{
 
-// 32bit
-using AtomicI32 = std::atomic<I32>;
-using AtomicU32 = std::atomic<U32>;
+enum class AtomicMemoryOrder
+{
+#if defined(__GNUC__)
+	RELAXED = __ATOMIC_RELAXED,
+	CONSUME = __ATOMIC_CONSUME,
+	ACQUIRE = __ATOMIC_ACQUIRE,
+	RELEASE = __ATOMIC_RELEASE,
+	ACQ_REL = __ATOMIC_ACQ_REL,
+	SEQ_CST = __ATOMIC_SEQ_CST
+#else
+#	error "TODO"
+#endif
+};
+
+/// Atomic template.
+template<typename T, AtomicMemoryOrder tmemOrd = AtomicMemoryOrder::RELAXED>
+class Atomic: public NonCopyable
+{
+public:
+	using Value = T;
+	static constexpr AtomicMemoryOrder MEMORY_ORDER = tmemOrd;
+
+	Atomic()
+	:	m_val(static_cast<Value>(0))
+	{}
+
+	Atomic(const Value a)
+	:	m_val(a)
+	{}
+
+	Value load(AtomicMemoryOrder memOrd = MEMORY_ORDER) const
+	{
+#if defined(__GNUC__)
+		return __atomic_load_n(&m_val, static_cast<int>(memOrd));
+#else
+#	error "TODO"
+#endif
+	}
+
+	void store(const Value a, AtomicMemoryOrder memOrd = MEMORY_ORDER)
+	{
+#if defined(__GNUC__)
+		__atomic_store_n(&m_val, a, static_cast<int>(memOrd));
+#else
+#	error "TODO"
+#endif
+	}
+
+	template<typename Y>
+	Value fetchAdd(const Y a, AtomicMemoryOrder memOrd = MEMORY_ORDER)
+	{
+#if defined(__GNUC__)
+		return __atomic_fetch_add(&m_val, a, static_cast<int>(memOrd));
+#else
+#	error "TODO"
+#endif
+	}
 
-// 64bit
-using AtomicI64 = std::atomic<I64>;
-using AtomicU64 = std::atomic<U64>;
+	template<typename Y>
+	Value fetchSub(const Y a, AtomicMemoryOrder memOrd = MEMORY_ORDER)
+	{
+#if defined(__GNUC__)
+		return __atomic_fetch_sub(&m_val, a, static_cast<int>(memOrd));
+#else
+#	error "TODO"
+#endif
+	}
 
-template<typename T>
-using Atomic = std::atomic<T>;
+	/// @code
+	/// if(m_val == expected) {
+	/// 	m_val = desired;
+	/// 	return true;
+	/// } else {
+	/// 	expected = m_val;
+	/// 	return false;
+	/// }
+	/// @endcode
+	Bool compareExchange(Value& expected, const Value desired, 
+		AtomicMemoryOrder memOrd = MEMORY_ORDER)
+	{
+#if defined(__GNUC__)
+		return __atomic_compare_exchange_n(&m_val, &expected, desired,
+			false, static_cast<int>(memOrd), __ATOMIC_RELAXED);
+#else
+#	error "TODO"
+#endif
+	}
+
+	/// Set @a a to the atomic and return the previous value.
+	Value exchange(const Value a, AtomicMemoryOrder memOrd = MEMORY_ORDER)
+	{
+#if defined(__GNUC__)
+		return __atomic_exchange_n(&m_val, a, static_cast<int>(memOrd));
+#else
+#	error "TODO"
+#endif
+	}
 
+private:
+	Value m_val;
+};
 /// @}
 
 } // end namespace anki

include/anki/util/Memory.h

@@ -81,7 +81,7 @@ public:
 	void free(void* ptr);
 
 	/// Get refcount.
-	AtomicU32& getRefcount()
+	Atomic<U32>& getRefcount()
 	{
 		return m_refcount;
 	}
@@ -118,7 +118,7 @@ protected:
 	void* m_allocCbUserData = nullptr;
 
 	/// Allocations count.
-	AtomicU32 m_allocationsCount = {0};
+	Atomic<U32> m_allocationsCount = {0};
 
 	/// Check if already created.
 	Bool isCreated() const;
@@ -128,7 +128,7 @@ private:
 	Type m_type = Type::NONE;
 
 	/// Refcount.
-	AtomicU32 m_refcount = {0};
+	Atomic<U32> m_refcount = {0};
 };
 
 /// A dummy interface to match the StackMemoryPool and ChainMemoryPool 

src/physics/PhysicsPlayerController.cpp

@@ -8,6 +8,133 @@
 
 namespace anki {
 
+//==============================================================================
+// Static                                                                      =
+//==============================================================================
+
+//==============================================================================
+struct CustomControllerConvexRayFilter
+{
+	const NewtonBody* m_me = nullptr;
+	Vec4 m_hitContact = Vec4(0.0); 
+	Vec4 m_hitNormal = Vec4(0.0);
+	const NewtonBody* m_hitBody = nullptr;
+	const NewtonCollision* m_shapeHit = nullptr;
+	U32 m_collisionId = 0;
+	F32 m_intersectParam = 0.0;
+
+	static F32 filterCallback(
+		const NewtonBody* const body, 
+		const NewtonCollision* const shapeHit, 
+		const dFloat* const hitContact, 
+		const dFloat* const hitNormal, 
+		dLong collisionId,
+		void* const userData, 
+		dFloat intersectParam)
+	{
+		CustomControllerConvexRayFilter* filter = 
+			static_cast<CustomControllerConvexRayFilter*>(userData);
+
+		ANKI_ASSERT(body != filter->m_me);
+
+		if(intersectParam < filter->m_intersectParam) 
+		{
+			filter->m_hitBody = body;	
+			filter->m_shapeHit = shapeHit;
+			filter->m_collisionId = collisionId;
+			filter->m_intersectParam = intersectParam;
+			filter->m_hitContact = Vec4(
+				hitContact[0], hitContact[1], hitContact[2], 0.0);
+			filter->m_hitNormal = Vec4(
+				hitNormal[0], hitNormal[1], hitNormal[2], 0.0); 
+		}
+
+		return intersectParam;
+	}
+
+	static unsigned prefilterCallback(
+		const NewtonBody* const body, 
+		const NewtonCollision* const myCollision, 
+		void* const userData)
+	{
+		CustomControllerConvexRayFilter* filter = 
+			static_cast<CustomControllerConvexRayFilter*>(userData);
+
+		return (body != filter->m_me) ? 1 : 0;
+	}
+};
+
+//==============================================================================
+struct CustomControllerConvexCastPreFilter
+{
+	const NewtonBody* m_me = nullptr;
+
+	CustomControllerConvexCastPreFilter(NewtonBody* body)
+	:	m_me(body)
+	{
+		ANKI_ASSERT(m_me != nullptr);
+	}
+
+	static unsigned prefilterCallback(
+		const NewtonBody* const body, 
+		const NewtonCollision* const myCollision, 
+		void* const userData)
+	{
+		CustomControllerConvexCastPreFilter* filter = 
+			static_cast<CustomControllerConvexCastPreFilter*>(userData);
+
+		return (body != filter->m_me) ? 1 : 0;
+	}
+};
+
+//==============================================================================
+static Vec4 calcAverageOmega(Quat q0, const Quat& q1, F32 invdt)
+{
+	if(q0.dot(q1) < 0.0)
+	{
+		q0 *= -1.0f;
+	}
+
+	Quat dq(q0.getConjugated().combineRotations(q1));
+	Vec4 omegaDir(dq.x(), dq.y(), dq.z(), 0.0);
+
+	F32 dirMag2 = omegaDir.getLengthSquared();
+	if(dirMag2 < 1.0e-5 * 1.0e-5) 
+	{
+		return Vec4(0.0);
+	}
+
+	F32 dirMagInv = 1.0 / sqrt(dirMag2);
+	F32 dirMag = dirMag2 * dirMagInv;
+
+	F32 omegaMag = 2.0 * atan2(dirMag, dq.w()) * invdt;
+	return omegaDir * (dirMagInv * omegaMag);
+}
+
+//==============================================================================
+static Quat integrateOmega(const Quat& rot, const Vec4& omega, F32 dt)
+{
+	ANKI_ASSERT(omega.w() == 0.0);
+	Quat rotation(rot);
+	F32 omegaMag2 = omega.dot(omega);
+	F32 errAngle2 = toRad(0.0125) * toRad(0.0125);
+	if(omegaMag2 > errAngle2) 
+	{
+		F32 invOmegaMag = 1.0 / sqrt(omegaMag2);
+		Vec4 omegaAxis(omega * invOmegaMag);
+		F32 omegaAngle = invOmegaMag * omegaMag2 * dt;
+		Quat deltaRotation(Axisang(omegaAngle, omegaAxis.xyz()));
+		rotation = rotation.combineRotations(deltaRotation);
+		rotation = rotation * (1.0 / sqrt(rotation.dot(rotation)));
+	}
+
+	return rotation;
+}
+
+//==============================================================================
+// PhysicsPlayerController                                                     =
+//==============================================================================
+
 //==============================================================================
 Error PhysicsPlayerController::create(const Initializer& init)
 {
@@ -20,6 +147,8 @@ Error PhysicsPlayerController::create(const Initializer& init)
 	m_stepHeight = init.m_stepHeight;
 	m_isJumping = false;
 
+	m_gravity = m_world->getGravity();
+
 	setClimbSlope(toRad(45.0));
 
 	Mat4 localAxis(Mat4::getIdentity());
@@ -28,7 +157,7 @@ Error PhysicsPlayerController::create(const Initializer& init)
 	m_frontDir = localAxis.getColumn(2);
 
 	m_groundPlane = Vec4(0.0);
-	m_groundVeloc = Vec4(0.0);
+	m_groundVelocity = Vec4(0.0);
 
 	const U steps = 12;
 	Array2d<Vec4, 2, steps> convexPoints;
@@ -148,11 +277,416 @@ Error PhysicsPlayerController::create(const Initializer& init)
 }
 
 //==============================================================================
-void PhysicsPlayerController::setVelocity(
-	F32 forwardSpeed, F32 strafeSpeed, F32 jumpSpeed, 
-	const Vec3& forwardDir, const Vec3& gravity, F32 dt)
+Vec4 PhysicsPlayerController::calculateDesiredOmega(
+	const Vec4& frontDir, F32 dt) const
 {
-	
+	Vec4 playerRotationNt;
+	NewtonBodyGetRotation(m_body, &playerRotationNt[0]);
+	Quat playerRotation(playerRotationNt[1], playerRotationNt[2],
+		playerRotationNt[3], playerRotationNt[0]);
+
+	Quat targetRotation;
+	targetRotation.setFrom2Vec3(m_frontDir.xyz(), frontDir.xyz());
+	return calcAverageOmega(playerRotation, targetRotation, 0.5 / dt);
+}
+
+//==============================================================================
+Vec4 PhysicsPlayerController::calculateDesiredVelocity(
+	F32 forwardSpeed, F32 strafeSpeed, 
+	F32 verticalSpeed, const Vec4& gravity, F32 dt) const
+{
+	Mat4 matrix;
+	NewtonBodyGetMatrix(m_body, &matrix[0]);
+	matrix.transpose();
+	matrix.setTranslationPart(Vec4(0.0, 0.0, 0.0, 1.0));
+
+	Vec4 updir(matrix * m_upDir);
+	Vec4 frontDir(matrix * m_frontDir);
+	Vec4 rightDir(frontDir.cross(updir));
+
+	Vec4 veloc(0.0);
+	if((verticalSpeed <= 0.0) && m_groundPlane.getLengthSquared() > 0.0) 
+	{
+		// Plane is supported by a ground plane, apply the player input velocity
+		if(m_groundPlane.dot(updir) >= m_maxSlope) 
+		{
+			// Player is in a legal slope, he is in full control of his movement
+			Vec4 bodyVeloc(0.0);
+			NewtonBodyGetVelocity(m_body, &bodyVeloc[0]);
+			veloc = updir * bodyVeloc.dot(updir) 
+				+ gravity * dt + frontDir * forwardSpeed 
+				+ rightDir * strafeSpeed 
+				+ updir * verticalSpeed;
+
+			veloc += m_groundVelocity - updir * updir.dot(m_groundVelocity);
+
+			F32 speedLimitMag2 = forwardSpeed * forwardSpeed 
+				+ strafeSpeed * strafeSpeed 
+				+ verticalSpeed * verticalSpeed 
+				+ m_groundVelocity.dot(m_groundVelocity) + 0.1;
+
+			F32 speedMag2 = veloc.dot(veloc);
+			if(speedMag2 > speedLimitMag2)
+			{
+				veloc = veloc * sqrt(speedLimitMag2 / speedMag2);
+			}
+
+			F32 normalVeloc = m_groundPlane.dot(veloc - m_groundVelocity);
+			if(normalVeloc < 0.0) 
+			{
+				veloc -= m_groundPlane * normalVeloc;
+			}
+		} 
+		else 
+		{
+			// Player is in an illegal ramp, he slides down hill an loses 
+			// control of his movement 
+			NewtonBodyGetVelocity(m_body, &veloc[0]);
+			veloc += updir * verticalSpeed;
+			veloc += gravity * dt;
+			F32 normalVeloc = m_groundPlane.dot(veloc - m_groundVelocity);
+			if(normalVeloc < 0.0) 
+			{
+				veloc -= m_groundPlane * normalVeloc;
+			}
+		}
+	} 
+	else 
+	{
+		// Player is on free fall, only apply the gravity
+		NewtonBodyGetVelocity(m_body, &veloc[0]);
+		veloc += updir * verticalSpeed;
+		veloc += gravity * dt;
+	}
+
+	return veloc;
+}
+
+//==============================================================================
+void PhysicsPlayerController::calculateVelocity(F32 dt)
+{
+	Vec4 omega(calculateDesiredOmega(m_forwardDir, dt));
+	Vec4 veloc(calculateDesiredVelocity(
+		m_forwardSpeed, m_strafeSpeed, m_jumpSpeed, m_gravity, dt));			
+
+	NewtonBodySetOmega(m_body, &omega[0]);
+	NewtonBodySetVelocity(m_body, &veloc[0]);
+
+	if(m_jumpSpeed > 0.0)
+	{
+		m_isJumping = true;
+	}
+}
+
+//==============================================================================
+F32 PhysicsPlayerController::calculateContactKinematics(const Vec4& veloc, 
+	const NewtonWorldConvexCastReturnInfo* contactInfo) const
+{
+	Vec4 contactVeloc(0.0) ;
+	if(contactInfo->m_hitBody) 
+	{
+		NewtonBodyGetPointVelocity(contactInfo->m_hitBody, 
+			contactInfo->m_point, &contactVeloc[0]);
+	}
+
+	const F32 restitution = 0.0;
+	Vec4 normal(contactInfo->m_normal[0], contactInfo->m_normal[1], 
+		contactInfo->m_normal[2], 0.0);
+	F32 reboundVelocMag = -((veloc - contactVeloc).dot(normal)) 
+		* (1.0 + restitution);
+	return max(reboundVelocMag, 0.0f);
+}
+
+//==============================================================================
+void PhysicsPlayerController::updateGroundPlane(Mat4& matrix, 
+	const Mat4& castMatrix, const Vec4& dst, int threadIndex)
+{
+	NewtonWorld* world = m_world->_getNewtonWorld();
+
+	CustomControllerConvexRayFilter filter;
+	filter.m_me = m_body;
+
+	NewtonWorldConvexRayCast(world, 
+		m_castingShape, 
+		&castMatrix.getTransposed()[0], 
+		reinterpret_cast<const F32*>(&dst), 
+		CustomControllerConvexRayFilter::filterCallback, 
+		&filter, 
+		CustomControllerConvexRayFilter::prefilterCallback, 
+		threadIndex);
+
+	m_groundPlane = Vec4(0.0);
+	m_groundVelocity = Vec4(0.0);
+
+	if(filter.m_hitBody) 
+	{
+		m_isJumping = false;
+
+		Vec4 castMatrixTransl = castMatrix.getTranslationPart();
+
+		Vec4 supportPoint(castMatrixTransl
+			+ (dst - castMatrixTransl) * filter.m_intersectParam);
+
+		m_groundPlane = filter.m_hitNormal;
+		m_groundPlane.w() = -supportPoint.dot(filter.m_hitNormal);
+
+		NewtonBodyGetPointVelocity(
+			filter.m_hitBody, &supportPoint[0], &m_groundVelocity[0]);
+
+		matrix.setTranslationPart(Vec4(supportPoint.xyz(), 1.0));
+	}
+}
+
+//==============================================================================
+void PhysicsPlayerController::postUpdate(F32 dt, int threadIndex)
+{
+	Mat4 matrix;
+	Quat bodyRotation;
+	Vec4 veloc(0.0);
+	Vec4 omega(0.0);
+
+	NewtonWorld* world = m_world->_getNewtonWorld();
+
+	calculateVelocity(dt);
+
+	// Get the body motion state 
+	NewtonBodyGetMatrix(m_body, &matrix[0]);
+	matrix.transpose();
+	NewtonBodyGetVelocity(m_body, &veloc[0]);
+	NewtonBodyGetOmega(m_body, &omega[0]);
+
+	// Integrate body angular velocity
+	NewtonBodyGetRotation(m_body, &bodyRotation[0]);
+	toAnki(bodyRotation);
+	bodyRotation = integrateOmega(bodyRotation, omega, dt);
+	matrix.setRotationPart(Mat3(bodyRotation));
+
+	// Integrate linear velocity
+	F32 normalizedTimeLeft = 1.0; 
+	F32 step = dt * sqrt(veloc.dot(veloc));
+	F32 descreteTimeStep = dt * (1.0 / DESCRETE_MOTION_STEPS);
+	U prevContactCount = 0;
+	CustomControllerConvexCastPreFilter castFilterData(m_body);
+	Array<NewtonWorldConvexCastReturnInfo, MAX_CONTACTS> prevInfo;
+
+	Vec4 updir(matrix.getRotationPart() * m_upDir.xyz(), 0.0);
+
+	Vec4 scale(0.0);
+	NewtonCollisionGetScale(
+		m_upperBodyShape, &scale.x(), &scale.y(), &scale.z());
+	F32 radio = (m_outerRadius + m_restrainingDistance) * 4.0;
+	NewtonCollisionSetScale(m_upperBodyShape, 
+		m_height - m_stepHeight, radio, radio);
+
+	NewtonWorldConvexCastReturnInfo upConstraint;
+	memset(&upConstraint, 0, sizeof(upConstraint));
+	upConstraint.m_normal[0] = m_upDir.x();
+	upConstraint.m_normal[1] = m_upDir.y();
+	upConstraint.m_normal[2] = m_upDir.z();
+	upConstraint.m_normal[3] = m_upDir.w();
+
+	for(U j = 0; 
+		(j < MAX_INTERGRATION_STEPS) && (normalizedTimeLeft > 1.0e-5f); 
+		++j) 
+	{
+		if((veloc.dot(veloc)) < 1.0e-6) 
+		{
+			break;
+		}
+
+		F32 timetoImpact;
+		Array<NewtonWorldConvexCastReturnInfo, MAX_CONTACTS> info;
+
+		Vec4 destPosit(matrix.getTranslationPart() + veloc * dt);
+		U contactCount = NewtonWorldConvexCast(
+			world, 
+			&matrix.getTransposed()[0], 
+			&destPosit[0], 
+			m_upperBodyShape, 
+			&timetoImpact, 
+			&castFilterData, 
+			CustomControllerConvexCastPreFilter::prefilterCallback, 
+			&info[0], 
+			info.getSize(), 
+			threadIndex);
+
+		if(contactCount > 0)
+		{
+			matrix.setTranslationPart(
+				matrix.getTranslationPart() + veloc * (timetoImpact * dt));
+
+			if(timetoImpact > 0.0) 
+			{
+				matrix.setTranslationPart(matrix.getTranslationPart() -
+					veloc * (CONTACT_SKIN_THICKNESS / veloc.getLength()));
+			}
+
+			normalizedTimeLeft -= timetoImpact;
+
+			Array<F32, MAX_CONTACTS * 2> speed;
+			Array<F32, MAX_CONTACTS * 2> bounceSpeed;
+			Array<Vec4, MAX_CONTACTS * 2> bounceNormal;
+
+			for(U i = 1; i < contactCount; ++i) 
+			{
+				Vec4 n0(info[i - 1].m_normal);
+
+				for(U j = 0; j < i; ++j) 
+				{
+					Vec4 n1(info[j].m_normal);
+					if((n0.dot(n1)) > 0.9999) 
+					{
+						info[i] = info[contactCount - 1];
+						--i;
+						--contactCount;
+						break;
+					}
+				}
+			}
+
+			U count = 0;
+			if(!m_isJumping) 
+			{
+				Vec4 matTls = matrix.getTranslationPart();
+				upConstraint.m_point[0] = matTls.x();
+				upConstraint.m_point[1] = matTls.y();
+				upConstraint.m_point[2] = matTls.z();
+				upConstraint.m_point[3] = matTls.w();
+
+				speed[count] = 0.0;
+				bounceNormal[count] = Vec4(upConstraint.m_normal);
+				bounceSpeed[count] = 
+					calculateContactKinematics(veloc, &upConstraint);
+				++count;
+			}
+
+			for(U i = 0; i < contactCount; ++i) 
+			{
+				speed[count] = 0.0;
+				bounceNormal[count] = Vec4(info[i].m_normal);
+				bounceSpeed[count] = 
+					calculateContactKinematics(veloc, &info[i]);
+				++count;
+			}
+
+			for(U i = 0; i < prevContactCount; ++i) 
+			{
+				speed[count] = 0.0;
+				bounceNormal[count] = Vec4(prevInfo[i].m_normal);
+				bounceSpeed[count] = 
+					calculateContactKinematics(veloc, &prevInfo[i]);
+				++count;
+			}
+
+			F32 residual = 10.0;
+			Vec4 auxBounceVeloc (0.0);
+			for(U i = 0; 
+				(i < MAX_SOLVER_ITERATIONS) && (residual > 1.0e-3); 
+				++i) 
+			{
+				residual = 0.0;
+				for(U k = 0; k < count; ++k) 
+				{
+					Vec4 normal(bounceNormal[k]);
+					F32 v = bounceSpeed[k] - normal.dot(auxBounceVeloc);
+					F32 x = speed[k] + v;
+					if(x < 0.0) 
+					{
+						v = 0.0;
+						x = 0.0;
+					}
+
+					if(abs(v) > residual) 
+					{
+						residual = abs(v);
+					}
+
+					auxBounceVeloc += normal * (x - speed[k]);
+					speed[k] = x;
+				}
+			}
+
+			Vec4 velocStep (0.0);
+			for(U i = 0; i < count; ++i) 
+			{
+				Vec4 normal(bounceNormal[i]);
+				velocStep += normal * speed[i];
+			}
+
+			veloc += velocStep;
+
+			F32 velocMag2 = velocStep.getLengthSquared();
+			if(velocMag2 < 1.0e-6) 
+			{
+				F32 advanceTime = min(
+					descreteTimeStep, normalizedTimeLeft * dt);
+
+				matrix.setTranslationPart(
+					matrix.getTranslationPart() + veloc * advanceTime);
+
+				normalizedTimeLeft -= advanceTime / dt;
+			}
+
+			prevContactCount = contactCount;
+			memcpy(&prevInfo[0], &info[0], 
+				prevContactCount * sizeof(NewtonWorldConvexCastReturnInfo));
+
+		} 
+		else 
+		{
+			matrix.setTranslationPart(Vec4(destPosit.xyz(), 1.0));
+			break;
+		}
+	}
+
+	NewtonCollisionSetScale(m_upperBodyShape, scale.x(), scale.y(), scale.z());
+
+	// determine if player is standing on some plane
+	Mat4 supportMatrix(matrix);
+	supportMatrix.setTranslationPart(
+		supportMatrix.getTranslationPart() + updir * m_sphereCastOrigin);
+	supportMatrix(3, 3) = 1.0;
+
+	if(m_isJumping) 
+	{
+		Vec4 dst(matrix.getTranslationPart().xyz0());
+		updateGroundPlane(matrix, supportMatrix, dst, threadIndex);
+	} 
+	else 
+	{
+		step = abs(updir.dot(veloc * dt));
+		F32 castDist = 
+			(m_groundPlane.getLengthSquared() > 0.0) ? m_stepHeight : step;
+		Vec4 dst(matrix.getTranslationPart() - updir * (castDist * 2.0));
+		dst.w() = 0.0;
+
+		updateGroundPlane(matrix, supportMatrix, dst, threadIndex);
+	}
+
+	// set player velocity, position and orientation
+	NewtonBodySetVelocity(m_body, &veloc[0]);
+	NewtonBodySetMatrix(m_body, &matrix[0]);
+}
+
+//==============================================================================
+void PhysicsPlayerController::postUpdateKernelCallback(
+	NewtonWorld* const world, 
+	void* const context, 
+	int threadIndex)
+{
+	PhysicsPlayerController* x = 
+		static_cast<PhysicsPlayerController*>(context);
+	x->postUpdate(x->m_world->getDeltaTime(), threadIndex);
+}
+
+//==============================================================================
+void PhysicsPlayerController::moveToPosition(const Vec4& position)
+{
+	Mat4 trf;
+	NewtonBodyGetMatrix(m_body, &trf[0]);
+	trf.transpose();
+	trf.setTranslationPart(position.xyz1());
+	NewtonBodySetMatrix(m_body, &trf[0]);
 }
 
 } // end namespace anki

src/physics/PhysicsWorld.cpp

@@ -63,18 +63,24 @@ Error PhysicsWorld::create(AllocAlignedCallback allocCb, void* allocCbData)
 	// Set the simplified solver mode (faster but less accurate)
 	NewtonSetSolverModel(m_world, 1);
 
+	// Set the post update listener
+	NewtonWorldAddPostListener(m_world, "world", this, postUpdateCallback, 
+		destroyCallback);
+
 	return err;
 }
 
 //==============================================================================
 void PhysicsWorld::_increaseObjectsMarkedForDeletion(PhysicsObject::Type type)
 {
-	++m_forDeletionCount[static_cast<U>(type)];
+	m_forDeletionCount[static_cast<U>(type)].fetchAdd(1);
 }
 
 //==============================================================================
-Error PhysicsWorld::updateAsync(F32 timestep)
+Error PhysicsWorld::updateAsync(F32 dt)
 {
+	m_dt = dt;
+
 	// Do cleanup of marked for deletion
 	cleanupMarkedForDeletion(m_bodies, 
 		m_forDeletionCount[static_cast<U>(PhysicsObject::Type::BODY)]);
@@ -82,7 +88,7 @@ Error PhysicsWorld::updateAsync(F32 timestep)
 		static_cast<U>(PhysicsObject::Type::COLLISION_SHAPE)]);
 
 	// Update
-	NewtonUpdateAsync(m_world, timestep);
+	NewtonUpdateAsync(m_world, dt);
 
 	return ErrorCode::NONE;
 }
@@ -96,9 +102,9 @@ void PhysicsWorld::waitUpdate()
 //==============================================================================
 template<typename T>
 void PhysicsWorld::cleanupMarkedForDeletion(
-	List<T*>& container, AtomicU32& count)
+	List<T*>& container, Atomic<U32>& count)
 {
-	while(count > 0)
+	while(count.load() > 0)
 	{
 		Bool found = false;
 		auto it = container.begin();
@@ -120,4 +126,14 @@ void PhysicsWorld::cleanupMarkedForDeletion(
 	}
 }
 
+//==============================================================================
+void PhysicsWorld::postUpdate(F32 dt)
+{
+	for(PhysicsPlayerController* player : m_playerControllers)
+	{
+		NewtonDispachThreadJob(m_world, 
+			PhysicsPlayerController::postUpdateKernelCallback, player);
+	}
+}
+
 } // end namespace anki

src/renderer/Is.cpp

@@ -77,11 +77,11 @@ public:
 	VisibilityTestResults::Container::ConstIterator m_lightsBegin;
 	VisibilityTestResults::Container::ConstIterator m_lightsEnd;
 
-	AtomicU32* m_pointLightsCount = nullptr;
-	AtomicU32* m_spotLightsCount = nullptr;
-	AtomicU32* m_spotTexLightsCount = nullptr;
+	Atomic<U32>* m_pointLightsCount = nullptr;
+	Atomic<U32>* m_spotLightsCount = nullptr;
+	Atomic<U32>* m_spotTexLightsCount = nullptr;
 		
-	Array2d<AtomicU32, 
+	Array2d<Atomic<U32>, 
 		ANKI_RENDERER_MAX_TILES_Y, 
 		ANKI_RENDERER_MAX_TILES_X>
 		* m_tilePointLightsCount = nullptr,
@@ -150,7 +150,7 @@ public:
 	I doPointLight(const LightComponent& light, const MoveComponent& move)
 	{
 		// Get GPU light
-		I i = m_pointLightsCount->fetch_add(1);
+		I i = m_pointLightsCount->fetchAdd(1);
 		if(i >= (I)m_is->m_maxPointLights)
 		{
 			return -1;
@@ -183,7 +183,7 @@ public:
 		{
 			// Spot tex light
 
-			i = m_spotTexLightsCount->fetch_add(1);
+			i = m_spotTexLightsCount->fetchAdd(1);
 			if(i >= (I)m_is->m_maxSpotTexLights)
 			{
 				return -1;
@@ -211,7 +211,7 @@ public:
 		{
 			// Spot light without texture
 
-			i = m_spotLightsCount->fetch_add(1);
+			i = m_spotLightsCount->fetchAdd(1);
 			if(i >= (I)m_is->m_maxSpotLights)
 			{
 				return -1;
@@ -286,7 +286,7 @@ public:
 			U x = t % m_is->m_r->getTilesCount().x();
 			U y = t / m_is->m_r->getTilesCount().x();
 
-			U tilePos = (*m_tilePointLightsCount)[y][x].fetch_add(1);
+			U tilePos = (*m_tilePointLightsCount)[y][x].fetchAdd(1);
 
 			if(tilePos < m_is->m_maxPointLightsPerTile)
 			{
@@ -321,7 +321,7 @@ public:
 
 			if(light.getShadowEnabled())
 			{
-				U tilePos = (*m_tileSpotTexLightsCount)[y][x].fetch_add(1);
+				U tilePos = (*m_tileSpotTexLightsCount)[y][x].fetchAdd(1);
 
 				if(tilePos < m_is->m_maxSpotTexLightsPerTile)
 				{
@@ -330,7 +330,7 @@ public:
 			}
 			else
 			{
-				U tilePos = (*m_tileSpotLightsCount)[y][x].fetch_add(1);
+				U tilePos = (*m_tileSpotLightsCount)[y][x].fetchAdd(1);
 
 				if(tilePos < m_is->m_maxSpotLightsPerTile)
 				{
@@ -639,11 +639,11 @@ Error Is::lightPass(GlCommandBufferHandle& cmdBuff)
 	err = tilesClientBuff.create(cmdBuff, m_tilesBuff.getSize(), nullptr);
 	if(err) return err;
 
-	AtomicU32 pointLightsAtomicCount(0);
-	AtomicU32 spotLightsAtomicCount(0);
-	AtomicU32 spotTexLightsAtomicCount(0);
+	Atomic<U32> pointLightsAtomicCount(0);
+	Atomic<U32> spotLightsAtomicCount(0);
+	Atomic<U32> spotTexLightsAtomicCount(0);
 
-	Array2d<AtomicU32, 
+	Array2d<Atomic<U32>, 
 		ANKI_RENDERER_MAX_TILES_Y, 
 		ANKI_RENDERER_MAX_TILES_X> 
 		tilePointLightsCount,

src/scene/SceneGraph.cpp

@@ -218,7 +218,7 @@ void SceneGraph::deleteNodesMarkedForDeletion()
 {
 	/// Delete all nodes pending deletion. At this point all scene threads 
 	/// should have finished their tasks
-	while(m_objectsMarkedForDeletionCount > 0)
+	while(m_objectsMarkedForDeletionCount.load() > 0)
 	{
 		Bool found = false;
 		auto it = m_nodes.begin();

src/util/Memory.cpp

@@ -140,7 +140,7 @@ void* allocAligned(
 //==============================================================================
 BaseMemoryPool::~BaseMemoryPool()
 {
-	ANKI_ASSERT(m_refcount == 0 && "Refcount should be zero");
+	ANKI_ASSERT(m_refcount.load() == 0 && "Refcount should be zero");
 }
 
 //==============================================================================
@@ -205,7 +205,7 @@ HeapMemoryPool::HeapMemoryPool()
 //==============================================================================
 HeapMemoryPool::~HeapMemoryPool()
 {
-	if(m_allocationsCount != 0)
+	if(m_allocationsCount.load() != 0)
 	{
 		ANKI_LOGW("Memory pool destroyed before all memory being released");
 	}
@@ -242,7 +242,7 @@ void* HeapMemoryPool::allocate(PtrSize size, PtrSize alignment)
 
 	if(mem != nullptr)
 	{
-		++m_allocationsCount;
+		m_allocationsCount.fetchAdd(1);
 
 #if ANKI_MEM_SIGNATURES
 		memset(mem, 0, m_headerSize);
@@ -275,7 +275,7 @@ void HeapMemoryPool::free(void* ptr)
 
 	ptr = static_cast<void*>(memU8);
 #endif
-	--m_allocationsCount;
+	m_allocationsCount.fetchSub(1);
 	m_allocCb(m_allocCbUserData, ptr, 0, 0);
 }
 
@@ -319,7 +319,7 @@ Error StackMemoryPool::create(
 	m_memsize = getAlignedRoundUp(alignmentBytes, size);
 	m_ignoreDeallocationErrors = ignoreDeallocationErrors;
 
-	m_memory = reinterpret_cast<U8*>(m_allocCb(
+	m_memory = static_cast<U8*>(m_allocCb(
 		m_allocCbUserData, nullptr, m_memsize, m_alignmentBytes));
 
 	if(m_memory != nullptr)
@@ -330,7 +330,7 @@ Error StackMemoryPool::create(
 #endif
 
 		// Align allocated memory
-		m_top = m_memory;
+		m_top.store(m_memory);
 
 		// Calc header size
 		m_headerSize = 
@@ -356,7 +356,7 @@ void* StackMemoryPool::allocate(PtrSize size, PtrSize alignment)
 
 	ANKI_ASSERT(size < MAX_U32 && "Too big allocation");
 
-	U8* out = m_top.fetch_add(size);
+	U8* out = m_top.fetchAdd(size);
 
 	if(out + size <= m_memory + m_memsize)
 	{
@@ -378,7 +378,7 @@ void* StackMemoryPool::allocate(PtrSize size, PtrSize alignment)
 		ANKI_ASSERT(isAligned(m_alignmentBytes, out));
 
 		// Increase count
-		++m_allocationsCount;
+		m_allocationsCount.fetchAdd(1);
 	}
 	else
 	{
@@ -426,10 +426,10 @@ void StackMemoryPool::free(void* ptr)
 	//     expected = top;
 	//     exchange = false;
 	// }
-	Bool exchange = m_top.compare_exchange_strong(expected, desired);
+	Bool exchange = m_top.compareExchange(expected, desired);
 
 	// Decrease count
-	--m_allocationsCount;
+	m_allocationsCount.fetchSub(1);
 
 	// Error if needed
 	if(!m_ignoreDeallocationErrors && !exchange)
@@ -451,8 +451,8 @@ void StackMemoryPool::reset()
 	memset(m_memory, 0xCC, m_memsize);
 #endif
 
-	m_top = m_memory;
-	m_allocationsCount = 0;
+	m_top.store(m_memory);
+	m_allocationsCount.store(0);
 }
 
 //==============================================================================
@@ -484,7 +484,7 @@ ChainMemoryPool::ChainMemoryPool()
 //==============================================================================
 ChainMemoryPool::~ChainMemoryPool()
 {
-	if(m_allocationsCount != 0)
+	if(m_allocationsCount.load() != 0)
 	{
 		ANKI_LOGW("Memory pool destroyed before all memory being released");
 	}
@@ -596,7 +596,7 @@ void* ChainMemoryPool::allocate(PtrSize size, PtrSize alignment)
 		ANKI_ASSERT(mem != nullptr && "The chunk should have space");
 	}
 
-	++m_allocationsCount;
+	m_allocationsCount.fetchAdd(1);
 
 	return mem;
 }
@@ -659,7 +659,7 @@ void ChainMemoryPool::free(void* ptr)
 		destroyChunk(chunk);
 	}
 
-	--m_allocationsCount;
+	m_allocationsCount.fetchSub(1);
 }
 
 //==============================================================================

testapp/Main.cpp

@@ -250,7 +250,7 @@ Error init()
 		move->setLocalOrigin(Vec4(0.0, 1.4, 0.6, 0.0));
 	}
 
-#if 0
+#if 1
 	{
 		ScriptResourcePointer script;
 
@@ -263,7 +263,9 @@ Error init()
 #endif
 
 	PhysicsPlayerController::Initializer init;
-	scene._getPhysicsWorld().newPlayerController(init);
+	auto player = scene._getPhysicsWorld().newPlayerController(init);
+	player->moveToPosition(Vec4(5.0, 2.0, 0.0, 0.0));
+	player->setVelocity(0.1, 0.0, 0.0, Vec4(1.0, 0.0, 0.0, 0.0));
 
 	/*AnimationResourcePointer anim;
 	anim.load("maps/sponza/unnamed_0.ankianim");