Remove cancellation from inertia calculation to improve accuracy (#955)

I was computing the shape inertia about the body origin and then
shifting to the center of mass. But this involves quadratic math with
bad cancellation effects.

I'm now buffering the shape inertias about their centroids and then
shifting them to the final body center of mass. This removes the
quadratic cancellation.

Fixes #949

Author: erincatto

include/box2d/collision.h

@@ -97,7 +97,7 @@ typedef struct b2MassData
 	/// The position of the shape's centroid relative to the shape's origin.
 	b2Vec2 center;
 
-	/// The rotational inertia of the shape about the local origin.
+	/// The rotational inertia of the shape about the shape center.
 	float rotationalInertia;
 } b2MassData;
 

samples/sample_issues.cpp

@@ -1,6 +1,7 @@
-#include "box2d/box2d.h"
 #include "sample.h"
 
+#include "box2d/box2d.h"
+
 #include <GLFW/glfw3.h>
 
 struct PhysicsHitQueryResult2D
@@ -22,11 +23,11 @@ struct CastContext_Single
 	bool hit = false;
 };
 
-static float b2CastResult_Closest(b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* c)
+static float b2CastResult_Closest( b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* c )
 {
-	CastContext_Single* context = static_cast<CastContext_Single*>(c);
+	CastContext_Single* context = static_cast<CastContext_Single*>( c );
 
-	if (b2Dot(context->translation, normal) >= 0.0f)
+	if ( b2Dot( context->translation, normal ) >= 0.0f )
 		return -1.0f;
 
 	context->result.shapeId = shapeId;
@@ -40,15 +41,15 @@ static float b2CastResult_Closest(b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal
 	return fraction;
 }
 
-static b2ShapeProxy TransformShapeProxy(const b2Transform& t, const b2ShapeProxy& proxy)
+static b2ShapeProxy TransformShapeProxy( const b2Transform& t, const b2ShapeProxy& proxy )
 {
 	b2ShapeProxy ret;
 	ret.count = proxy.count;
 	ret.radius = proxy.radius;
 
-	for (int i = 0; i < proxy.count; ++i)
+	for ( int i = 0; i < proxy.count; ++i )
 	{
-		ret.points[i] = b2TransformPoint(t, proxy.points[i]);
+		ret.points[i] = b2TransformPoint( t, proxy.points[i] );
 	}
 
 	return ret;
@@ -58,20 +59,14 @@ static b2ShapeProxy TransformShapeProxy(const b2Transform& t, const b2ShapeProxy
 class ShapeCastChain : public Sample
 {
 public:
-	explicit ShapeCastChain(SampleContext* context)
-		: Sample(context)
+	explicit ShapeCastChain( SampleContext* context )
+		: Sample( context )
 	{
 		// World Body & Shape
 		b2BodyDef worldBodyDef = b2DefaultBodyDef();
-		const b2BodyId groundId = b2CreateBody(m_worldId, &worldBodyDef);
+		const b2BodyId groundId = b2CreateBody( m_worldId, &worldBodyDef );
 
-		b2Vec2 points[4] =
-		{
-			{ 1.0f, 0.0f },
-			{ -1.0f, 0.0f },
-			{ -1.0f, -1.0f },
-			{ 1.0f, -1.0f }
-		};
+		b2Vec2 points[4] = { { 1.0f, 0.0f }, { -1.0f, 0.0f }, { -1.0f, -1.0f }, { 1.0f, -1.0f } };
 
 		b2ChainDef worldChainDef = b2DefaultChainDef();
 		worldChainDef.userData = nullptr;
@@ -82,12 +77,12 @@ class ShapeCastChain : public Sample
 		worldChainDef.filter.groupIndex = 0;
 		worldChainDef.isLoop = true;
 		worldChainDef.enableSensorEvents = false;
-		b2CreateChain(groundId, &worldChainDef);
+		b2CreateChain( groundId, &worldChainDef );
 
 		// "Character" Body & Shape
 		b2BodyDef characterBodyDef = b2DefaultBodyDef();
 		characterBodyDef.position = { 0.0f, 0.103f };
-		characterBodyDef.rotation = b2MakeRot(0.0f);
+		characterBodyDef.rotation = b2MakeRot( 0.0f );
 		characterBodyDef.linearDamping = 0.0f;
 		characterBodyDef.angularDamping = 0.0f;
 		characterBodyDef.gravityScale = 1.0f;
@@ -98,7 +93,7 @@ class ShapeCastChain : public Sample
 		characterBodyDef.userData = nullptr;
 		characterBodyDef.type = b2_kinematicBody;
 
-		characterBodyId_ = b2CreateBody(m_worldId, &characterBodyDef);
+		characterBodyId_ = b2CreateBody( m_worldId, &characterBodyDef );
 
 		b2ShapeDef characterShapeDef = b2DefaultShapeDef();
 
@@ -114,8 +109,8 @@ class ShapeCastChain : public Sample
 		characterShapeDef.invokeContactCreation = false;
 		characterShapeDef.updateBodyMass = false;
 
-		characterBox_ = b2MakeBox(0.1f, 0.1f);
-		b2CreatePolygonShape(characterBodyId_, &characterShapeDef, &characterBox_);
+		characterBox_ = b2MakeBox( 0.1f, 0.1f );
+		b2CreatePolygonShape( characterBodyId_, &characterShapeDef, &characterBox_ );
 
 		context->camera.m_center = b2Vec2_zero;
 	}
@@ -125,32 +120,32 @@ class ShapeCastChain : public Sample
 		const float timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f;
 
 		bool noXInput = true;
-		if (glfwGetKey(m_context->window, GLFW_KEY_A))
+		if ( glfwGetKey( m_context->window, GLFW_KEY_A ) )
 		{
 			characterVelocity_.x -= timeStep * 5.0f;
 			noXInput = false;
 		}
-		if (glfwGetKey(m_context->window, GLFW_KEY_D))
+		if ( glfwGetKey( m_context->window, GLFW_KEY_D ) )
 		{
 			characterVelocity_.x += timeStep * 5.0f;
 			noXInput = false;
 		}
 
 		bool noYInput = true;
-		if (glfwGetKey(m_context->window, GLFW_KEY_S))
+		if ( glfwGetKey( m_context->window, GLFW_KEY_S ) )
 		{
 			characterVelocity_.y -= timeStep * 5.0f;
 			noYInput = false;
 		}
-		if (glfwGetKey(m_context->window, GLFW_KEY_W))
+		if ( glfwGetKey( m_context->window, GLFW_KEY_W ) )
 		{
 			characterVelocity_.y += timeStep * 5.0f;
 			noYInput = false;
 		}
 
-		if (noXInput)
+		if ( noXInput )
 		{
-			if (b2AbsFloat(characterVelocity_.x) > 0.01f)
+			if ( b2AbsFloat( characterVelocity_.x ) > 0.01f )
 			{
 				const float decel = characterVelocity_.x > 0.0f ? 5.0f : -5.0f;
 				if ( b2AbsFloat( decel ) < characterVelocity_.x )
@@ -168,7 +163,7 @@ class ShapeCastChain : public Sample
 			}
 		}
 
-		if (noYInput)
+		if ( noYInput )
 		{
 			if ( b2AbsFloat( characterVelocity_.y ) > 0.01f )
 			{
@@ -188,42 +183,39 @@ class ShapeCastChain : public Sample
 			}
 		}
 
-		b2Vec2 characterPos = b2Body_GetPosition(characterBodyId_);
+		b2Vec2 characterPos = b2Body_GetPosition( characterBodyId_ );
 		b2Vec2 newCharacterPos = characterPos;
 		newCharacterPos.x += characterVelocity_.x * timeStep;
 		newCharacterPos.y += characterVelocity_.y * timeStep;
-		b2Body_SetTransform(characterBodyId_, newCharacterPos, b2Rot_identity);
+		b2Body_SetTransform( characterBodyId_, newCharacterPos, b2Rot_identity );
 
 		PhysicsHitQueryResult2D hitResult;
-		const b2ShapeProxy shapeProxy = b2MakeProxy(characterBox_.vertices, characterBox_.count, characterBox_.radius);
-		if (ShapeCastSingle(hitResult, characterPos, newCharacterPos, 0.0f, shapeProxy))
+		const b2ShapeProxy shapeProxy = b2MakeProxy( characterBox_.vertices, characterBox_.count, characterBox_.radius );
+		if ( ShapeCastSingle( hitResult, characterPos, newCharacterPos, 0.0f, shapeProxy ) )
 		{
 			hitPos = hitResult.point;
 			hitNormal = hitResult.normal;
 		}
 
-		m_draw->DrawLine(hitPos, { hitPos.x + hitNormal.x, hitPos.y + hitNormal.y }, b2_colorRed);
+		m_draw->DrawLine( hitPos, { hitPos.x + hitNormal.x, hitPos.y + hitNormal.y }, b2_colorRed );
 
 		Sample::Step();
 	}
 
-	bool ShapeCastSingle(PhysicsHitQueryResult2D& outResult, b2Vec2 start, b2Vec2 end, float rotation, const b2ShapeProxy& shape) const
+	bool ShapeCastSingle( PhysicsHitQueryResult2D& outResult, b2Vec2 start, b2Vec2 end, float rotation,
+						  const b2ShapeProxy& shape ) const
 	{
-		const b2Transform transform =
-		{
-			start,
-			b2MakeRot(rotation)
-		};
-		const b2ShapeProxy transformedShape = TransformShapeProxy(transform, shape);
+		const b2Transform transform = { start, b2MakeRot( rotation ) };
+		const b2ShapeProxy transformedShape = TransformShapeProxy( transform, shape );
 
 		const b2Vec2 translation = { end.x - start.x, end.y - start.y };
 		const b2QueryFilter filter = { 0x1, 0x1 };
 		CastContext_Single context;
 		context.startPos = start;
 		context.translation = { translation.x, translation.y };
-		b2World_CastShape(m_worldId, &transformedShape, translation, filter, &b2CastResult_Closest, &context);
+		b2World_CastShape( m_worldId, &transformedShape, translation, filter, &b2CastResult_Closest, &context );
 
-		if (context.hit)
+		if ( context.hit )
 		{
 			outResult = context.result;
 			return true;
@@ -232,9 +224,9 @@ class ShapeCastChain : public Sample
 		return false;
 	}
 
-	static Sample* Create(SampleContext* context)
+	static Sample* Create( SampleContext* context )
 	{
-		return new ShapeCastChain(context);
+		return new ShapeCastChain( context );
 	}
 
 private:
@@ -245,4 +237,53 @@ class ShapeCastChain : public Sample
 	b2Vec2 hitNormal = b2Vec2_zero;
 };
 
-static int sampleShapeCastChain = RegisterSample("Issues", "Shape Cast Chain", ShapeCastChain::Create );
+static int sampleShapeCastChain = RegisterSample( "Issues", "Shape Cast Chain", ShapeCastChain::Create );
+
+class BadSteiner : public Sample
+{
+public:
+	explicit BadSteiner( SampleContext* context )
+		: Sample( context )
+	{
+		if ( m_context->restart == false )
+		{
+			m_context->camera.m_center = { 0.0f, 1.75f };
+			m_context->camera.m_zoom = 2.5f;
+		}
+
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef );
+
+			b2ShapeDef shapeDef = b2DefaultShapeDef();
+			b2Segment segment = { { -100.0f, 0.0f }, { 100.0f, 0.0f } };
+			b2CreateSegmentShape( groundId, &shapeDef, &segment );
+		}
+
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.type = b2_dynamicBody;
+			bodyDef.position = { -48.0f, 62.0f };
+			b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
+
+			b2ShapeDef shapeDef = b2DefaultShapeDef();
+
+			b2Vec2 points[3] = {
+				{ 48.7599983f, -60.5699997f },
+				{ 48.7400017f, -60.5400009f },
+				{ 48.6800003f, -60.5600014f },
+			};
+
+			b2Hull hull = b2ComputeHull( points, 3 );
+			b2Polygon poly = b2MakePolygon( &hull, 0.0f );
+			b2CreatePolygonShape( bodyId, &shapeDef, &poly );
+		}
+	}
+
+	static Sample* Create( SampleContext* context )
+	{
+		return new BadSteiner( context );
+	}
+};
+
+static int sampleBadSteiner = RegisterSample( "Issues", "Bad Steiner", BadSteiner::Create );

src/body.c

@@ -572,23 +572,30 @@ void b2UpdateBodyMassData( b2World* world, b2Body* body )
 		return;
 	}
 
+	int shapeCount = body->shapeCount;
+	b2MassData* masses = b2AllocateArenaItem( &world->arena, shapeCount * sizeof( b2MassData ), "mass data" );
+
 	// Accumulate mass over all shapes.
 	b2Vec2 localCenter = b2Vec2_zero;
 	int shapeId = body->headShapeId;
+	int shapeIndex = 0;
 	while ( shapeId != B2_NULL_INDEX )
 	{
 		const b2Shape* s = b2ShapeArray_Get( &world->shapes, shapeId );
 		shapeId = s->nextShapeId;
 
 		if ( s->density == 0.0f )
 		{
+			masses[shapeIndex] = (b2MassData){ 0 };
 			continue;
 		}
 
 		b2MassData massData = b2ComputeShapeMass( s );
 		body->mass += massData.mass;
 		localCenter = b2MulAdd( localCenter, massData.mass, massData.center );
-		body->inertia += massData.rotationalInertia;
+
+		masses[shapeIndex] = massData;
+		shapeIndex += 1;
 	}
 
 	// Compute center of mass.
@@ -598,11 +605,28 @@ void b2UpdateBodyMassData( b2World* world, b2Body* body )
 		localCenter = b2MulSV( bodySim->invMass, localCenter );
 	}
 
+	// Second loop to accumulate the rotational inertia about the center of mass
+	for ( shapeIndex = 0; shapeIndex < shapeCount; ++shapeIndex )
+	{
+		b2MassData massData = masses[shapeIndex];
+		if (massData.mass == 0.0f)
+		{
+			continue;
+		}
+
+		// Shift to center of mass. This is safe because it can only increase.
+		b2Vec2 offset = b2Sub( localCenter, massData.center );
+		float inertia = massData.rotationalInertia + massData.mass * b2Dot( offset, offset );
+		body->inertia += inertia;
+	}
+
+	b2FreeArenaItem( &world->arena, masses );
+	masses = NULL;
+
+	B2_ASSERT( body->inertia >= 0.0f );
+
 	if ( body->inertia > 0.0f )
 	{
-		// Center the inertia about the center of mass.
-		body->inertia -= body->mass * b2Dot( localCenter, localCenter );
-		B2_ASSERT( body->inertia > 0.0f );
 		bodySim->invInertia = 1.0f / body->inertia;
 	}
 	else

src/geometry.c

@@ -225,8 +225,8 @@ b2MassData b2ComputeCircleMass( const b2Circle* shape, float density )
 	massData.mass = density * B2_PI * rr;
 	massData.center = shape->center;
 
-	// inertia about the local origin
-	massData.rotationalInertia = massData.mass * ( 0.5f * rr + b2Dot( shape->center, shape->center ) );
+	// inertia about the center of mass
+	massData.rotationalInertia = massData.mass * 0.5f * rr ;
 
 	return massData;
 }
@@ -267,9 +267,6 @@ b2MassData b2ComputeCapsuleMass( const b2Capsule* shape, float density )
 	float boxInertia = boxMass * ( 4.0f * rr + ll ) / 12.0f;
 	massData.rotationalInertia = circleInertia + boxInertia;
 
-	// inertia about the local origin
-	massData.rotationalInertia += massData.mass * b2Dot( massData.center, massData.center );
-
 	return massData;
 }
 
@@ -392,8 +389,11 @@ b2MassData b2ComputePolygonMass( const b2Polygon* shape, float density )
 	// Inertia tensor relative to the local origin (point s).
 	massData.rotationalInertia = density * rotationalInertia;
 
-	// Shift to center of mass then to original body origin.
-	massData.rotationalInertia += massData.mass * ( b2Dot( massData.center, massData.center ) - b2Dot( center, center ) );
+	// Shift inertia to center of mass
+	massData.rotationalInertia -= massData.mass * b2Dot( center, center );
+
+	// If this goes negative we are hosed
+	B2_ASSERT( massData.rotationalInertia >= 0.0f );
 
 	return massData;
 }

test/test_shape.c

@@ -21,7 +21,7 @@ static int ShapeMassTest( void )
 		b2MassData md = b2ComputeCircleMass( &circle, 1.0f );
 		ENSURE_SMALL( md.mass - B2_PI, FLT_EPSILON );
 		ENSURE( md.center.x == 1.0f && md.center.y == 0.0f );
-		ENSURE_SMALL( md.rotationalInertia - 1.5f * B2_PI, FLT_EPSILON );
+		ENSURE_SMALL( md.rotationalInertia - 0.5f * B2_PI, FLT_EPSILON );
 	}
 
 	{