Physics2D/RigidBody: Add position offset

SDK/include/NDK/Components/CollisionComponent2D.hpp

@@ -28,10 +28,14 @@ namespace Ndk
 			CollisionComponent2D(const CollisionComponent2D& collision);
 			~CollisionComponent2D() = default;
 
+			void Align(const Nz::Rectf& aabb);
+
 			Nz::Rectf GetAABB() const;
 			const Nz::Collider2DRef& GetGeom() const;
+			const Nz::Vector2f& GetGeomOffset() const;
 
 			void SetGeom(Nz::Collider2DRef geom);
+			void SetGeomOffset(const Nz::Vector2f& geomOffset);
 
 			CollisionComponent2D& operator=(Nz::Collider2DRef geom);
 			CollisionComponent2D& operator=(CollisionComponent2D&& collision) = default;
@@ -40,6 +44,8 @@ namespace Ndk
 
 		private:
 			void InitializeStaticBody();
+			Nz::RigidBody2D* GetRigidBody();
+			const Nz::RigidBody2D* GetRigidBody() const;
 			Nz::RigidBody2D* GetStaticBody();
 
 			void OnAttached() override;

SDK/include/NDK/Components/CollisionComponent2D.inl

@@ -28,16 +28,6 @@ namespace Ndk
 	{
 	}
 
-	/*!
-	* \brief Gets the collision box representing the entity
-	* \return The physics collision box
-	*/
-
-	inline Nz::Rectf CollisionComponent2D::GetAABB() const
-	{
-		return m_staticBody->GetAABB();
-	}
-
 	/*!
 	* \brief Gets the geometry representing the entity
 	* \return A constant reference to the physics geometry

SDK/src/NDK/Components/CollisionComponent2D.cpp

@@ -17,28 +17,41 @@ namespace Ndk
 	* \brief NDK class that represents a two-dimensional collision geometry
 	*/
 
+	void CollisionComponent2D::Align(const Nz::Rectf& aabb)
+	{
+		const Nz::RigidBody2D* rigidBody = GetRigidBody();
+		SetGeomOffset(aabb.GetCenter() - rigidBody->GetAABB().GetCenter() + rigidBody->GetPositionOffset());
+	}
+
+	/*!
+	* \brief Gets the collision box representing the entity
+	* \return The physics collision box
+	*/
+	Nz::Rectf CollisionComponent2D::GetAABB() const
+	{
+		return GetRigidBody()->GetAABB();
+	}
+
+	const Nz::Vector2f& CollisionComponent2D::GetGeomOffset() const
+	{
+		return GetRigidBody()->GetPositionOffset();
+	}
+
 	/*!
 	* \brief Sets geometry for the entity
 	*
 	* \param geom Geometry used for collisions
-	*
-	* \remark Produces a NazaraAssert if the entity has no physics component and has no static body
 	*/
 	void CollisionComponent2D::SetGeom(Nz::Collider2DRef geom)
 	{
 		m_geom = std::move(geom);
 
-		if (m_entity->HasComponent<PhysicsComponent2D>())
+		GetRigidBody()->SetGeom(m_geom);
-		{
-			// We update the geometry of the PhysiscsObject linked to the PhysicsComponent2D
-			PhysicsComponent2D& physComponent = m_entity->GetComponent<PhysicsComponent2D>();
-			physComponent.GetRigidBody()->SetGeom(m_geom);
 	}
-		else
+
+	void CollisionComponent2D::SetGeomOffset(const Nz::Vector2f& geomOffset)
 	{
-			NazaraAssert(m_staticBody, "An entity without physics component should have a static body");
+		GetRigidBody()->SetPositionOffset(geomOffset);
-			m_staticBody->SetGeom(m_geom);
-		}
 	}
 
 	/*!
@@ -47,7 +60,6 @@ namespace Ndk
 	* \remark Produces a NazaraAssert if entity is invalid
 	* \remark Produces a NazaraAssert if entity is not linked to a world, or the world has no physics system
 	*/
-
 	void CollisionComponent2D::InitializeStaticBody()
 	{
 		NazaraAssert(m_entity, "Invalid entity");
@@ -67,7 +79,34 @@ namespace Ndk
 			matrix.MakeIdentity();
 
 		m_staticBody->SetPosition(Nz::Vector2f(matrix.GetTranslation()));
+	}
 
+	Nz::RigidBody2D* CollisionComponent2D::GetRigidBody()
+	{
+		if (m_entity->HasComponent<PhysicsComponent2D>())
+		{
+			PhysicsComponent2D& physComponent = m_entity->GetComponent<PhysicsComponent2D>();
+			return physComponent.GetRigidBody();
+		}
+		else
+		{
+			NazaraAssert(m_staticBody, "An entity without physics component should have a static body");
+			return m_staticBody.get();
+		}
+	}
+
+	const Nz::RigidBody2D* CollisionComponent2D::GetRigidBody() const
+	{
+		if (m_entity->HasComponent<PhysicsComponent2D>())
+		{
+			PhysicsComponent2D& physComponent = m_entity->GetComponent<PhysicsComponent2D>();
+			return physComponent.GetRigidBody();
+		}
+		else
+		{
+			NazaraAssert(m_staticBody, "An entity without physics component should have a static body");
+			return m_staticBody.get();
+		}
 	}
 
 	/*!

include/Nazara/Physics2D/RigidBody2D.hpp

@@ -32,7 +32,7 @@ namespace Nz
 			RigidBody2D(PhysWorld2D* world, float mass);
 			RigidBody2D(PhysWorld2D* world, float mass, Collider2DRef geom);
 			RigidBody2D(const RigidBody2D& object);
-			RigidBody2D(RigidBody2D&& object);
+			RigidBody2D(RigidBody2D&& object) noexcept;
 			~RigidBody2D();
 
 			void AddForce(const Vector2f& force, CoordSys coordSys = CoordSys_Global);
@@ -60,6 +60,7 @@ namespace Nz
 			Vector2f GetMassCenter(CoordSys coordSys = CoordSys_Local) const;
 			float GetMomentOfInertia() const;
 			Vector2f GetPosition() const;
+			inline const Vector2f& GetPositionOffset() const;
 			RadianAnglef GetRotation() const;
 			inline std::size_t GetShapeCount() const;
 			std::size_t GetShapeIndex(cpShape* shape) const;
@@ -87,6 +88,7 @@ namespace Nz
 			void SetMassCenter(const Vector2f& center, CoordSys coordSys = CoordSys_Local);
 			void SetMomentOfInertia(float moment);
 			void SetPosition(const Vector2f& position);
+			void SetPositionOffset(const Vector2f& offset);
 			void SetRotation(const RadianAnglef& rotation);
 			void SetSurfaceVelocity(const Vector2f& surfaceVelocity);
 			void SetSurfaceVelocity(std::size_t shapeIndex, const Vector2f& surfaceVelocity);
@@ -114,6 +116,7 @@ namespace Nz
 			static void CopyBodyData(cpBody* from, cpBody* to);
 			static void CopyShapeData(cpShape* from, cpShape* to);
 
+			Vector2f m_positionOffset;
 			VelocityFunc m_velocityFunc;
 			std::vector<cpShape*> m_shapes;
 			Collider2DRef m_geom;

include/Nazara/Physics2D/RigidBody2D.inl

@@ -17,6 +17,11 @@ namespace Nz
 		return GetMassCenter(coordSys);
 	}
 
+	inline const Vector2f& RigidBody2D::GetPositionOffset() const
+	{
+		return m_positionOffset;
+	}
+
 	inline std::size_t RigidBody2D::GetShapeCount() const
 	{
 		return m_shapes.size();

src/Nazara/Physics2D/RigidBody2D.cpp

@@ -19,6 +19,7 @@ namespace Nz
 	}
 
 	RigidBody2D::RigidBody2D(PhysWorld2D* world, float mass, Collider2DRef geom) :
+	m_positionOffset(Vector2f::Zero()),
 	m_geom(),
 	m_userData(nullptr),
 	m_world(world),
@@ -35,6 +36,7 @@ namespace Nz
 	}
 
 	RigidBody2D::RigidBody2D(const RigidBody2D& object) :
+	m_positionOffset(object.m_positionOffset),
 	m_geom(object.m_geom),
 	m_userData(object.m_userData),
 	m_world(object.m_world),
@@ -59,9 +61,10 @@ namespace Nz
 		}
 	}
 
-	RigidBody2D::RigidBody2D(RigidBody2D&& object) :
+	RigidBody2D::RigidBody2D(RigidBody2D&& object) noexcept :
 	OnRigidBody2DMove(std::move(object.OnRigidBody2DMove)),
 	OnRigidBody2DRelease(std::move(object.OnRigidBody2DRelease)),
+	m_positionOffset(std::move(object.m_positionOffset)),
 	m_shapes(std::move(object.m_shapes)),
 	m_geom(std::move(object.m_geom)),
 	m_handle(object.m_handle),
@@ -260,7 +263,7 @@ namespace Nz
 
 	Vector2f RigidBody2D::GetPosition() const
 	{
-		cpVect pos = cpBodyGetPosition(m_handle);
+		cpVect pos = cpBodyLocalToWorld(m_handle, cpv(-m_positionOffset.x, -m_positionOffset.y));
 		return Vector2f(static_cast<float>(pos.x), static_cast<float>(pos.y));
 	}
 
@@ -467,7 +470,9 @@ namespace Nz
 
 	void RigidBody2D::SetPosition(const Vector2f& position)
 	{
-		cpBodySetPosition(m_handle, cpv(position.x, position.y));
+		cpVect oldPosition = cpBodyGetPosition(m_handle);
+
+		cpBodySetPosition(m_handle, cpBodyLocalToWorld(m_handle, cpv(position.x - oldPosition.x + m_positionOffset.x, position.y - oldPosition.y + m_positionOffset.y)));
 		if (m_isStatic)
 		{
 			m_world->RegisterPostStep(this, [](Nz::RigidBody2D* body)
@@ -477,6 +482,13 @@ namespace Nz
 		}
 	}
 
+	void RigidBody2D::SetPositionOffset(const Vector2f& offset)
+	{
+		Nz::Vector2f position = GetPosition();
+		m_positionOffset = offset;
+		SetPosition(position);
+	}
+
 	void RigidBody2D::SetRotation(const RadianAnglef& rotation)
 	{
 		cpBodySetAngle(m_handle, rotation.value);
@@ -573,6 +585,7 @@ namespace Nz
 		m_geom                = std::move(object.m_geom);
 		m_gravityFactor       = object.m_gravityFactor;
 		m_mass                = object.m_mass;
+		m_positionOffset      = object.m_positionOffset;
 		m_shapes              = std::move(object.m_shapes);
 		m_userData            = object.m_userData;
 		m_velocityFunc        = std::move(object.m_velocityFunc);
@@ -653,9 +666,10 @@ namespace Nz
 
 	void RigidBody2D::CopyBodyData(cpBody* from, cpBody* to)
 	{
+		cpBodySetCenterOfGravity(to, cpBodyGetCenterOfGravity(from));
+
 		cpBodySetAngle(to, cpBodyGetAngle(from));
 		cpBodySetAngularVelocity(to, cpBodyGetAngularVelocity(from));
-		cpBodySetCenterOfGravity(to, cpBodyGetCenterOfGravity(from));
 		cpBodySetForce(to, cpBodyGetForce(from));
 		cpBodySetPosition(to, cpBodyGetPosition(from));
 		cpBodySetTorque(to, cpBodyGetTorque(from));