Physics2D: Automatically compute center of mass

src/Nazara/Physics2D/Collider2D.cpp

@@ -20,7 +20,7 @@ namespace Nz
 		{
 			cpShape* shape = (*shapes)[i];
 
-			cpShapeSetCollisionType(shape, cpFloat(m_collisionId));
+			cpShapeSetCollisionType(shape, m_collisionId);
 			cpShapeSetElasticity(shape, cpFloat(m_elasticity));
 			cpShapeSetFilter(shape, filter);
 			cpShapeSetFriction(shape, cpFloat(m_friction));
@@ -44,6 +44,11 @@ namespace Nz
 	{
 	}
 
+	Nz::Vector2f BoxCollider2D::ComputeCenterOfMass() const
+	{
+		return m_rect.GetCenter();
+	}
+
 	float BoxCollider2D::ComputeMomentOfInertia(float mass) const
 	{
 		return static_cast<float>(cpMomentForBox2(mass, cpBBNew(m_rect.x, m_rect.y, m_rect.x + m_rect.width, m_rect.y + m_rect.height)));
@@ -68,6 +73,11 @@ namespace Nz
 	{
 	}
 
+	Nz::Vector2f CircleCollider2D::ComputeCenterOfMass() const
+	{
+		return m_offset + Nz::Vector2f(m_radius, m_radius);
+	}
+
 	float CircleCollider2D::ComputeMomentOfInertia(float mass) const
 	{
 		return static_cast<float>(cpMomentForCircle(mass, 0.f, m_radius, cpv(m_offset.x, m_offset.y)));
@@ -92,6 +102,15 @@ namespace Nz
 	{
 	}
 
+	Nz::Vector2f CompoundCollider2D::ComputeCenterOfMass() const
+	{
+		Nz::Vector2f centerOfMass = Nz::Vector2f::Zero();
+		for (const auto& geom : m_geoms)
+			centerOfMass += geom->ComputeCenterOfMass();
+
+		return centerOfMass / float(m_geoms.size());
+	}
+
 	float CompoundCollider2D::ComputeMomentOfInertia(float mass) const
 	{
 		///TODO: Correctly compute moment using parallel axis theorem:
@@ -144,6 +163,15 @@ namespace Nz
 			m_vertices[i].Set(*vertices++);
 	}
 
+	Nz::Vector2f ConvexCollider2D::ComputeCenterOfMass() const
+	{
+		static_assert(sizeof(cpVect) == sizeof(Vector2d), "Chipmunk vector is not equivalent to Vector2d");
+
+		cpVect center = cpCentroidForPoly(int(m_vertices.size()), reinterpret_cast<const cpVect*>(m_vertices.data()));
+
+		return Nz::Vector2f(float(center.x), float(center.y));
+	}
+
 	float ConvexCollider2D::ComputeMomentOfInertia(float mass) const
 	{
 		static_assert(sizeof(cpVect) == sizeof(Vector2d), "Chipmunk vector is not equivalent to Vector2d");
@@ -169,6 +197,11 @@ namespace Nz
 		return ColliderType2D_Null;
 	}
 
+	Nz::Vector2f NullCollider2D::ComputeCenterOfMass() const
+	{
+		return Nz::Vector2f::Zero();
+	}
+
 	float NullCollider2D::ComputeMomentOfInertia(float mass) const
 	{
 		return (mass > 0.f) ? 1.f : 0.f; //< Null inertia is only possible for static/kinematic objects
@@ -181,6 +214,11 @@ namespace Nz
 
 	/******************************** SegmentCollider2D *********************************/
 
+	Nz::Vector2f SegmentCollider2D::ComputeCenterOfMass() const
+	{
+		return (m_first + m_second) / 2.f;
+	}
+
 	float SegmentCollider2D::ComputeMomentOfInertia(float mass) const
 	{
 		return static_cast<float>(cpMomentForSegment(mass, cpv(m_first.x, m_first.y), cpv(m_second.x, m_second.y), m_thickness));

src/Nazara/Physics2D/RigidBody2D.cpp

@@ -48,7 +48,7 @@ namespace Nz
 		NazaraAssert(m_geom, "Invalid geometry");
 
 		m_handle = Create(m_mass, object.GetMomentOfInertia());
-		SetGeom(object.GetGeom(), false);
+		SetGeom(object.GetGeom(), false, false);
 
 		CopyBodyData(object.GetHandle(), m_handle);
 
@@ -362,7 +362,7 @@ namespace Nz
 		cpShapeSetFriction(m_shapes[shapeIndex], cpFloat(friction));
 	}
 
-	void RigidBody2D::SetGeom(Collider2DRef geom, bool recomputeMoment)
+	void RigidBody2D::SetGeom(Collider2DRef geom, bool recomputeMoment, bool recomputeMassCenter)
 	{
 		// We have no public way of getting rid of an existing geom without removing the whole body
 		// So let's save some attributes of the body, destroy it and rebuild it
@@ -399,6 +399,9 @@ namespace Nz
 			if (!IsStatic() && !IsKinematic())
 				cpBodySetMoment(m_handle, m_geom->ComputeMomentOfInertia(m_mass));
 		}
+
+		if (recomputeMassCenter)
+			SetMassCenter(m_geom->ComputeCenterOfMass());
 	}
 
 	void RigidBody2D::SetMass(float mass, bool recomputeMoment)