Box2D-0.1.0.0: upstream/src/shape.c

// SPDX-FileCopyrightText: 2023 Erin Catto
// SPDX-License-Identifier: MIT

#include "shape.h"

#include "body.h"
#include "broad_phase.h"
#include "contact.h"
#include "physics_world.h"
#include "recording.h"
#include "sensor.h"

// needed for dll export
#include "solver_set.h"

#include "box2d/box2d.h"

#include <stddef.h>

static b2Shape* b2GetShape( b2World* world, b2ShapeId shapeId )
{
	int id = shapeId.index1 - 1;
	b2Shape* shape = b2Array_Get( world->shapes, id );
	B2_ASSERT( shape->id == id && shape->generation == shapeId.generation );
	return shape;
}

static b2ChainShape* b2GetChainShape( b2World* world, b2ChainId chainId )
{
	int id = chainId.index1 - 1;
	b2ChainShape* chain = b2Array_Get( world->chainShapes, id );
	B2_ASSERT( chain->id == id && chain->generation == chainId.generation );
	return chain;
}

static float b2ComputeShapeMargin( b2Shape* shape )
{
	float margin = 0.0f;

	switch ( shape->type )
	{
		case b2_capsuleShape:
		{
			margin = 0.5f * b2Distance( shape->capsule.center2, shape->capsule.center1 ) + shape->capsule.radius;
		}
		break;

		case b2_circleShape:
		{
			margin = shape->circle.radius;
		}
		break;

		case b2_polygonShape:
		{
			const b2Polygon* poly = &shape->polygon;
			float maxExtentSqr = 0.0f;
			int count = poly->count;
			for ( int i = 0; i < count; ++i )
			{
				float distanceSqr = b2DistanceSquared( poly->vertices[i], poly->centroid );
				maxExtentSqr = b2MaxFloat( maxExtentSqr, distanceSqr );
			}

			margin = sqrtf( maxExtentSqr );
		}
		break;

		case b2_segmentShape:
		{
			margin = 0.5f * b2Distance( shape->segment.point1, shape->segment.point2 );
		}
		break;

		case b2_chainSegmentShape:
		{
			margin = 0.5f * b2Distance( shape->chainSegment.segment.point1, shape->chainSegment.segment.point2 );
		}
		break;

		default:
			B2_VALIDATE( false );
			return B2_MAX_AABB_MARGIN;
	}

	return b2MinFloat( B2_MAX_AABB_MARGIN, B2_AABB_MARGIN_FRACTION * margin );
}

static void b2UpdateShapeAABBs( b2Shape* shape, b2AABB* fatAABB, b2WorldTransform transform, b2BodyType proxyType )
{
	// Compute a bounding box with a speculative margin
	const float speculativeDistance = B2_SPECULATIVE_DISTANCE;
	const float aabbMargin = shape->aabbMargin;

	b2AABB aabb = b2ComputeFatShapeAABB( shape, transform, speculativeDistance );
	shape->aabb = aabb;

	// Smaller margin for static bodies. Cannot be zero due to TOI tolerance.
	float margin = proxyType == b2_staticBody ? speculativeDistance : aabbMargin;
	fatAABB->lowerBound.x = aabb.lowerBound.x - margin;
	fatAABB->lowerBound.y = aabb.lowerBound.y - margin;
	fatAABB->upperBound.x = aabb.upperBound.x + margin;
	fatAABB->upperBound.y = aabb.upperBound.y + margin;
}

static b2Shape* b2CreateShapeInternal( b2World* world, b2Body* body, b2WorldTransform transform, const b2ShapeDef* def,
									   const void* geometry, b2ShapeType shapeType )
{
	int shapeId = b2AllocId( &world->shapeIdPool );

	if ( shapeId == world->shapes.count )
	{
		b2Array_Push( world->shapes, (b2Shape){ 0 } );
		b2Array_Push( world->fatAABBs, (b2AABB){ 0 } );
	}
	else
	{
		B2_ASSERT( world->shapes.data[shapeId].id == B2_NULL_INDEX );
	}

	B2_ASSERT( world->fatAABBs.count == world->shapes.count );

	b2Shape* shape = b2Array_Get( world->shapes, shapeId );

	switch ( shapeType )
	{
		case b2_capsuleShape:
			shape->capsule = *(const b2Capsule*)geometry;
			break;

		case b2_circleShape:
			shape->circle = *(const b2Circle*)geometry;
			break;

		case b2_polygonShape:
			shape->polygon = *(const b2Polygon*)geometry;
			break;

		case b2_segmentShape:
			shape->segment = *(const b2Segment*)geometry;
			break;

		case b2_chainSegmentShape:
			shape->chainSegment = *(const b2ChainSegment*)geometry;
			break;

		default:
			B2_ASSERT( false );
			break;
	}

	shape->id = shapeId;
	shape->bodyId = body->id;
	shape->type = shapeType;
	shape->density = def->density;
	shape->material = def->material;
	shape->filter = def->filter;
	shape->userData = def->userData;
	shape->enableSensorEvents = def->enableSensorEvents;
	shape->enableContactEvents = def->enableContactEvents;
	shape->enableCustomFiltering = def->enableCustomFiltering;
	shape->enableHitEvents = def->enableHitEvents;
	shape->enablePreSolveEvents = def->enablePreSolveEvents;
	shape->proxyKey = B2_NULL_INDEX;
	shape->localCentroid = b2GetShapeCentroid( shape );
	shape->aabbMargin = b2ComputeShapeMargin( shape );
	shape->aabb = (b2AABB){ b2Vec2_zero, b2Vec2_zero };
	world->fatAABBs.data[shapeId] = (b2AABB){ b2Vec2_zero, b2Vec2_zero };
	shape->generation += 1;

	if ( body->setIndex != b2_disabledSet )
	{
		b2BodyType proxyType = body->type;
		b2CreateShapeProxy( world, shape, proxyType, transform, def->invokeContactCreation || def->isSensor );
	}

	// Add to shape doubly linked list
	if ( body->headShapeId != B2_NULL_INDEX )
	{
		b2Shape* headShape = b2Array_Get( world->shapes, body->headShapeId );
		headShape->prevShapeId = shapeId;
	}

	shape->prevShapeId = B2_NULL_INDEX;
	shape->nextShapeId = body->headShapeId;
	body->headShapeId = shapeId;
	body->shapeCount += 1;

	if ( def->isSensor )
	{
		shape->sensorIndex = world->sensors.count;
		b2Sensor sensor = { 0 };
		sensor.shapeId = shapeId;
		b2Array_CreateN( sensor.hits, 4 );
		b2Array_CreateN( sensor.overlaps1, 16 );
		b2Array_CreateN( sensor.overlaps2, 16 );
		b2Array_Push( world->sensors, sensor );
	}
	else
	{
		shape->sensorIndex = B2_NULL_INDEX;
	}

	b2ValidateSolverSets( world );

	return shape;
}

static bool b2IsValidMaterial( const b2SurfaceMaterial* material )
{
	return b2IsValidFloat( material->friction ) && material->friction >= 0.0f && b2IsValidFloat( material->restitution ) &&
		   material->restitution >= 0.0f && b2IsValidFloat( material->rollingResistance ) &&
		   material->rollingResistance >= 0.0f && b2IsValidFloat( material->tangentSpeed );
}

static bool b2IsValidCircle( const b2Circle* circle )
{
	return b2IsValidVec2( circle->center ) && b2IsValidFloat( circle->radius ) && circle->radius >= 0.0f;
}

static bool b2IsValidCapsule( const b2Capsule* capsule )
{
	return b2IsValidVec2( capsule->center1 ) && b2IsValidVec2( capsule->center2 ) && b2IsValidFloat( capsule->radius ) &&
		   capsule->radius >= 0.0f;
}

static bool b2IsValidSegment( const b2Segment* segment )
{
	return b2IsValidVec2( segment->point1 ) && b2IsValidVec2( segment->point2 );
}

static bool b2IsValidChainSegment( const b2ChainSegment* chainSegment )
{
	return b2IsValidVec2( chainSegment->ghost1 ) && b2IsValidSegment( &chainSegment->segment ) &&
		   b2IsValidVec2( chainSegment->ghost2 );
}

static bool b2IsValidPolygon( const b2Polygon* polygon )
{
	if ( polygon->count < 1 || polygon->count > B2_MAX_POLYGON_VERTICES )
	{
		return false;
	}

	if ( b2IsValidFloat( polygon->radius ) == false || polygon->radius < 0.0f )
	{
		return false;
	}

	if ( b2IsValidVec2( polygon->centroid ) == false )
	{
		return false;
	}

	for ( int i = 0; i < polygon->count; ++i )
	{
		if ( b2IsValidVec2( polygon->vertices[i] ) == false || b2IsValidVec2( polygon->normals[i] ) == false )
		{
			return false;
		}
	}

	return true;
}

static b2ShapeId b2CreateShape( b2BodyId bodyId, const b2ShapeDef* def, const void* geometry, b2ShapeType shapeType )
{
	B2_CHECK_DEF( def );
	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->density ) && def->density >= 0.0f, (b2ShapeId){ 0 } );
	B2_CHECK_INPUT_RETURN( b2IsValidMaterial( &def->material ), (b2ShapeId){ 0 } );

	b2World* world = b2GetWorldLocked( bodyId.world0 );
	if ( world == NULL )
	{
		return (b2ShapeId){ 0 };
	}

	b2Body* body = b2GetBodyFullId( world, bodyId );
	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );

	b2Shape* shape = b2CreateShapeInternal( world, body, transform, def, geometry, shapeType );

	if ( def->updateBodyMass == true )
	{
		b2UpdateBodyMassData( world, body );
	}
	else if ( ( body->flags & b2_dirtyMass ) == 0 )
	{
		body->flags |= b2_dirtyMass;
		b2SyncBodyFlags( world, body );
	}

	b2ValidateSolverSets( world );

	b2ShapeId id = { shape->id + 1, bodyId.world0, shape->generation };
	return id;
}

b2ShapeId b2CreateCircleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Circle* circle )
{
	B2_CHECK_INPUT_RETURN( b2IsValidCircle( circle ), b2_nullShapeId );

	b2ShapeId id = b2CreateShape( bodyId, def, circle, b2_circleShape );

	b2World* world = b2GetWorld( bodyId.world0 );
	B2_REC_CREATE( world, CreateCircleShape, id, bodyId, *def, *circle );

	return id;
}

b2ShapeId b2CreateCapsuleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Capsule* capsule )
{
	B2_CHECK_INPUT_RETURN( b2IsValidCapsule( capsule ), b2_nullShapeId );

	float lengthSqr = b2DistanceSquared( capsule->center1, capsule->center2 );
	if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP )
	{
		return b2_nullShapeId;
	}

	b2ShapeId id = b2CreateShape( bodyId, def, capsule, b2_capsuleShape );

	b2World* world = b2GetWorld( bodyId.world0 );
	B2_REC_CREATE( world, CreateCapsuleShape, id, bodyId, *def, *capsule );

	return id;
}

b2ShapeId b2CreatePolygonShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon )
{
	B2_CHECK_INPUT_RETURN( b2IsValidPolygon( polygon ), b2_nullShapeId );

	b2ShapeId id = b2CreateShape( bodyId, def, polygon, b2_polygonShape );

	b2World* world = b2GetWorld( bodyId.world0 );
	B2_REC_CREATE( world, CreatePolygonShape, id, bodyId, *def, *polygon );

	return id;
}

b2ShapeId b2CreateSegmentShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Segment* segment )
{
	B2_CHECK_INPUT_RETURN( b2IsValidSegment( segment ), b2_nullShapeId );

	float lengthSqr = b2DistanceSquared( segment->point1, segment->point2 );
	if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP )
	{
		B2_ASSERT( false );
		return b2_nullShapeId;
	}

	b2ShapeId id = b2CreateShape( bodyId, def, segment, b2_segmentShape );

	b2World* world = b2GetWorld( bodyId.world0 );
	B2_REC_CREATE( world, CreateSegmentShape, id, bodyId, *def, *segment );

	return id;
}

b2ShapeId b2CreateChainSegmentShape( b2BodyId bodyId, const b2ShapeDef* def, const b2ChainSegment* chainSegment )
{
	B2_CHECK_INPUT_RETURN( b2IsValidChainSegment( chainSegment ), b2_nullShapeId );

	float lengthSqr = b2DistanceSquared( chainSegment->segment.point1, chainSegment->segment.point2 );
	if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP )
	{
		B2_ASSERT( false );
		return b2_nullShapeId;
	}

	// No parent chain shape
	b2ChainSegment local = *chainSegment;
	local.chainId = B2_NULL_INDEX;

	b2ShapeId id = b2CreateShape( bodyId, def, &local, b2_chainSegmentShape );

	b2World* world = b2GetWorld( bodyId.world0 );
	B2_REC_CREATE( world, CreateChainSegmentShape, id, bodyId, *def, local );

	return id;
}

// Destroy a shape on a body. This doesn't need to be called when destroying a body.
static void b2DestroyShapeInternal( b2World* world, b2Shape* shape, b2Body* body )
{
	int shapeId = shape->id;

	// Remove the shape from the body's doubly linked list.
	if ( shape->prevShapeId != B2_NULL_INDEX )
	{
		b2Shape* prevShape = b2Array_Get( world->shapes, shape->prevShapeId );
		prevShape->nextShapeId = shape->nextShapeId;
	}

	if ( shape->nextShapeId != B2_NULL_INDEX )
	{
		b2Shape* nextShape = b2Array_Get( world->shapes, shape->nextShapeId );
		nextShape->prevShapeId = shape->prevShapeId;
	}

	if ( shapeId == body->headShapeId )
	{
		body->headShapeId = shape->nextShapeId;
	}

	body->shapeCount -= 1;

	// Remove from broad-phase.
	b2DestroyShapeProxy( shape, &world->broadPhase );

	// Destroy any contacts associated with the shape.
	int contactKey = body->headContactKey;
	while ( contactKey != B2_NULL_INDEX )
	{
		int contactId = contactKey >> 1;
		int edgeIndex = contactKey & 1;

		b2Contact* contact = b2Array_Get( world->contacts, contactId );
		contactKey = contact->edges[edgeIndex].nextKey;

		if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId )
		{
			b2DestroyContact( world, contact );
		}
	}

	if ( shape->sensorIndex != B2_NULL_INDEX )
	{
		b2Sensor* sensor = b2Array_Get( world->sensors, shape->sensorIndex );
		for ( int i = 0; i < sensor->overlaps2.count; ++i )
		{
			b2Visitor* ref = sensor->overlaps2.data + i;
			b2SensorEndTouchEvent event = {
				.sensorShapeId =
					{
						.index1 = shapeId + 1,
						.world0 = world->worldId,
						.generation = shape->generation,
					},
				.visitorShapeId =
					{
						.index1 = ref->shapeId + 1,
						.world0 = world->worldId,
						.generation = ref->generation,
					},
			};

			b2Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );
		}

		// Destroy sensor
		b2Array_Destroy( sensor->hits );
		b2Array_Destroy( sensor->overlaps1 );
		b2Array_Destroy( sensor->overlaps2 );

		int movedIndex = b2Array_RemoveSwap( world->sensors, shape->sensorIndex );
		if ( movedIndex != B2_NULL_INDEX )
		{
			// Fixup moved sensor
			b2Sensor* movedSensor = b2Array_Get( world->sensors, shape->sensorIndex );
			b2Shape* otherSensorShape = b2Array_Get( world->shapes, movedSensor->shapeId );
			otherSensorShape->sensorIndex = shape->sensorIndex;
		}
	}

	// Return shape to free list.
	b2FreeId( &world->shapeIdPool, shapeId );
	shape->id = B2_NULL_INDEX;

	b2ValidateSolverSets( world );
}

void b2DestroyShape( b2ShapeId shapeId, bool updateBodyMass )
{
	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	b2Shape* shape = b2GetShape( world, shapeId );

	// Cannot destroy a chain segment that has a parent chain shape
	if ( shape->type == b2_chainSegmentShape && shape->chainSegment.chainId != B2_NULL_INDEX )
	{
		B2_ASSERT( false );
		return;
	}

	B2_REC( world, DestroyShape, shapeId, updateBodyMass );

	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );
	b2DestroyShapeInternal( world, shape, body );

	if ( updateBodyMass == true )
	{
		b2UpdateBodyMassData( world, body );
	}
}

b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def )
{
	B2_CHECK_DEF( def );
	B2_CHECK_INPUT_RETURN( def->isLoop ? def->pointCount >= 3 : def->pointCount >= 2, (b2ChainId){ 0 } );

	int segmentCount = def->isLoop ? def->pointCount : def->pointCount - 1;

	B2_CHECK_INPUT_RETURN( def->materialCount == 1 || def->materialCount == segmentCount, (b2ChainId){ 0 } );

	for ( int i = 0; i < def->pointCount; ++i )
	{
		B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->points[i] ), (b2ChainId){ 0 } );
	}

	for ( int i = 0; i < def->materialCount; ++i )
	{
		B2_CHECK_INPUT_RETURN( b2IsValidMaterial( def->materials + i ), (b2ChainId){ 0 } );
	}

	if ( def->isLoop == false )
	{
		// These are set to INFINITY in the default def as a sentinel to ensure they are set.
		B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->ghost1 ), (b2ChainId){ 0 } );
		B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->ghost2 ), (b2ChainId){ 0 } );
	}

	b2World* world = b2GetWorldLocked( bodyId.world0 );
	if ( world == NULL )
	{
		return (b2ChainId){ 0 };
	}

	b2Body* body = b2GetBodyFullId( world, bodyId );
	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );

	int chainId = b2AllocId( &world->chainIdPool );

	if ( chainId == world->chainShapes.count )
	{
		b2Array_Push( world->chainShapes, (b2ChainShape){ 0 } );
	}
	else
	{
		B2_ASSERT( world->chainShapes.data[chainId].id == B2_NULL_INDEX );
	}

	b2ChainShape* chainShape = b2Array_Get( world->chainShapes, chainId );

	chainShape->id = chainId;
	chainShape->bodyId = body->id;
	chainShape->nextChainId = body->headChainId;
	chainShape->segmentCount = segmentCount;
	chainShape->generation += 1;

	int materialCount = def->materialCount;

	body->headChainId = chainId;

	b2ShapeDef shapeDef = b2DefaultShapeDef();
	shapeDef.userData = def->userData;
	shapeDef.filter = def->filter;
	shapeDef.enableSensorEvents = def->enableSensorEvents;
	shapeDef.enableContactEvents = false;
	shapeDef.enableHitEvents = false;

	const b2Vec2* points = def->points;
	int n = segmentCount;
	chainShape->shapeIndices = b2Alloc( n * sizeof( int ) );

	float tolSqr = B2_LINEAR_SLOP * B2_LINEAR_SLOP;
	B2_UNUSED( tolSqr );

	if ( def->isLoop )
	{
		b2ChainSegment chainSegment;

		int prevIndex = n - 1;
		for ( int i = 0; i < n; ++i )
		{
			chainSegment.ghost1 = points[prevIndex];
			chainSegment.segment.point1 = points[i];
			chainSegment.segment.point2 = points[( i + 1 ) % n];
			chainSegment.ghost2 = points[( i + 2 ) % n];

			B2_ASSERT( b2DistanceSquared( chainSegment.ghost1, chainSegment.segment.point1 ) > tolSqr );
			B2_ASSERT( b2DistanceSquared( chainSegment.segment.point1, chainSegment.segment.point2 ) > tolSqr );

			chainSegment.chainId = chainId;
			prevIndex = i;

			int materialIndex = materialCount == 1 ? 0 : i;
			shapeDef.material = def->materials[materialIndex];

			b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape );
			chainShape->shapeIndices[i] = shape->id;
		}
	}
	else
	{
		b2ChainSegment chainSegment;

		for ( int i = 0; i < n; ++i )
		{
			B2_ASSERT( i + 1 < def->pointCount );
			chainSegment.ghost1 = i == 0 ? def->ghost1 : points[i - 1];
			chainSegment.segment.point1 = points[i + 0];
			chainSegment.segment.point2 = points[i + 1];
			chainSegment.ghost2 = i == n - 1 ? def->ghost2 : points[i + 2];

			B2_ASSERT( b2DistanceSquared( chainSegment.ghost1, chainSegment.segment.point1 ) > tolSqr );
			B2_ASSERT( b2DistanceSquared( chainSegment.segment.point1, chainSegment.segment.point2 ) > tolSqr );
			B2_ASSERT( b2DistanceSquared( chainSegment.segment.point2, chainSegment.ghost2 ) > tolSqr );

			chainSegment.chainId = chainId;

			int materialIndex = materialCount == 1 ? 0 : i;
			shapeDef.material = def->materials[materialIndex];

			b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape );
			chainShape->shapeIndices[i] = shape->id;
		}
	}

	b2ChainId id = { chainId + 1, world->worldId, chainShape->generation };

	B2_REC_CREATE( world, CreateChain, id, bodyId, *def );

	return id;
}

void b2FreeChainData( b2ChainShape* chain )
{
	b2Free( chain->shapeIndices, chain->segmentCount * sizeof( int ) );
	chain->shapeIndices = NULL;
}

void b2DestroyChain( b2ChainId chainId )
{
	b2World* world = b2GetWorldLocked( chainId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, DestroyChain, chainId );

	b2ChainShape* chain = b2GetChainShape( world, chainId );

	b2Body* body = b2Array_Get( world->bodies, chain->bodyId );

	// Remove the chain from the body's singly linked list.
	int* chainIdPtr = &body->headChainId;
	bool found = false;
	while ( *chainIdPtr != B2_NULL_INDEX )
	{
		if ( *chainIdPtr == chain->id )
		{
			*chainIdPtr = chain->nextChainId;
			found = true;
			break;
		}

		chainIdPtr = &( world->chainShapes.data[*chainIdPtr].nextChainId );
	}

	B2_ASSERT( found == true );
	if ( found == false )
	{
		return;
	}

	int count = chain->segmentCount;
	for ( int i = 0; i < count; ++i )
	{
		int shapeId = chain->shapeIndices[i];
		b2Shape* shape = b2Array_Get( world->shapes, shapeId );
		b2DestroyShapeInternal( world, shape, body );
	}

	b2FreeChainData( chain );

	// Return chain to free list.
	b2FreeId( &world->chainIdPool, chain->id );
	chain->id = B2_NULL_INDEX;

	b2ValidateSolverSets( world );
}

b2WorldId b2Chain_GetWorld( b2ChainId chainId )
{
	b2World* world = b2GetWorld( chainId.world0 );
	return (b2WorldId){ chainId.world0 + 1, world->generation };
}

int b2Chain_GetSegmentCount( b2ChainId chainId )
{
	b2World* world = b2GetWorldLocked( chainId.world0 );
	if ( world == NULL )
	{
		return 0;
	}

	b2ChainShape* chain = b2GetChainShape( world, chainId );
	return chain->segmentCount;
}

int b2Chain_GetSegments( b2ChainId chainId, b2ShapeId* segmentArray, int capacity )
{
	b2World* world = b2GetWorldLocked( chainId.world0 );
	if ( world == NULL )
	{
		return 0;
	}

	b2ChainShape* chain = b2GetChainShape( world, chainId );

	int count = b2MinInt( chain->segmentCount, capacity );
	for ( int i = 0; i < count; ++i )
	{
		int shapeId = chain->shapeIndices[i];
		b2Shape* shape = b2Array_Get( world->shapes, shapeId );
		segmentArray[i] = (b2ShapeId){ shapeId + 1, chainId.world0, shape->generation };
	}

	return count;
}

void b2Chain_SetSurfaceMaterial( b2ChainId chainId, const b2SurfaceMaterial* material, int segmentIndex )
{
	B2_CHECK_INPUT( b2IsValidMaterial( material ) );

	b2World* world = b2GetWorldLocked( chainId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ChainSetSurfaceMaterial, chainId, *material, segmentIndex );

	b2ChainShape* chainShape = b2GetChainShape( world, chainId );
	B2_ASSERT( 0 <= segmentIndex && segmentIndex < chainShape->segmentCount );
	int shapeId = chainShape->shapeIndices[segmentIndex];
	b2Shape* shape = b2Array_Get( world->shapes, shapeId );
	shape->material = *material;
}

void b2Chain_SetAllSurfaceMaterials( b2ChainId chainId, const b2SurfaceMaterial* material )
{
	B2_CHECK_INPUT( b2IsValidMaterial( material ) );

	b2World* world = b2GetWorldLocked( chainId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ChainSetAllSurfaceMaterials, chainId, *material );

	b2ChainShape* chainShape = b2GetChainShape( world, chainId );

	for ( int i = 0; i < chainShape->segmentCount; ++i )
	{
		int shapeId = chainShape->shapeIndices[i];
		b2Shape* shape = b2Array_Get( world->shapes, shapeId );
		shape->material = *material;
	}
}

b2SurfaceMaterial b2Chain_GetSurfaceMaterial( b2ChainId chainId, int segmentIndex )
{
	b2World* world = b2GetWorld( chainId.world0 );
	b2ChainShape* chainShape = b2GetChainShape( world, chainId );
	B2_ASSERT( 0 <= segmentIndex && segmentIndex < chainShape->segmentCount );
	int shapeId = chainShape->shapeIndices[segmentIndex];
	b2Shape* shape = b2Array_Get( world->shapes, shapeId );
	return shape->material;
}

b2AABB b2ComputeShapeAABB( const b2Shape* shape, b2WorldTransform xf )
{
	switch ( shape->type )
	{
		case b2_capsuleShape:
			return b2ComputeCapsuleBounds( &shape->capsule, xf );
		case b2_circleShape:
			return b2ComputeCircleBounds( &shape->circle, xf );
		case b2_polygonShape:
			return b2ComputePolygonBounds( &shape->polygon, xf );
		case b2_segmentShape:
			return b2ComputeSegmentBounds( &shape->segment, xf );
		case b2_chainSegmentShape:
			return b2ComputeSegmentBounds( &shape->chainSegment.segment, xf );
		default:
		{
			B2_ASSERT( false );
			b2AABB empty = { { 0.0f, 0.0f }, { 0.0f, 0.0f } };
			return empty;
		}
	}
}

b2Vec2 b2GetShapeCentroid( const b2Shape* shape )
{
	switch ( shape->type )
	{
		case b2_capsuleShape:
			return b2Lerp( shape->capsule.center1, shape->capsule.center2, 0.5f );
		case b2_circleShape:
			return shape->circle.center;
		case b2_polygonShape:
			return shape->polygon.centroid;
		case b2_segmentShape:
			return b2Lerp( shape->segment.point1, shape->segment.point2, 0.5f );
		case b2_chainSegmentShape:
			return b2Lerp( shape->chainSegment.segment.point1, shape->chainSegment.segment.point2, 0.5f );
		default:
			return b2Vec2_zero;
	}
}

// todo_erin maybe compute this on shape creation
float b2GetShapePerimeter( const b2Shape* shape )
{
	switch ( shape->type )
	{
		case b2_capsuleShape:
			return 2.0f * b2Length( b2Sub( shape->capsule.center1, shape->capsule.center2 ) ) +
				   2.0f * B2_PI * shape->capsule.radius;
		case b2_circleShape:
			return 2.0f * B2_PI * shape->circle.radius;
		case b2_polygonShape:
		{
			const b2Vec2* points = shape->polygon.vertices;
			int count = shape->polygon.count;
			float perimeter = 2.0f * B2_PI * shape->polygon.radius;
			B2_ASSERT( count > 0 );
			b2Vec2 prev = points[count - 1];
			for ( int i = 0; i < count; ++i )
			{
				b2Vec2 next = points[i];
				perimeter += b2Length( b2Sub( next, prev ) );
				prev = next;
			}

			return perimeter;
		}
		case b2_segmentShape:
			return 2.0f * b2Length( b2Sub( shape->segment.point1, shape->segment.point2 ) );
		case b2_chainSegmentShape:
			return 2.0f * b2Length( b2Sub( shape->chainSegment.segment.point1, shape->chainSegment.segment.point2 ) );
		default:
			return 0.0f;
	}
}

// This projects the shape perimeter onto an infinite line
float b2GetShapeProjectedPerimeter( const b2Shape* shape, b2Vec2 line )
{
	switch ( shape->type )
	{
		case b2_capsuleShape:
		{
			b2Vec2 axis = b2Sub( shape->capsule.center2, shape->capsule.center1 );
			float projectedLength = b2AbsFloat( b2Dot( axis, line ) );
			return projectedLength + 2.0f * shape->capsule.radius;
		}

		case b2_circleShape:
			return 2.0f * shape->circle.radius;

		case b2_polygonShape:
		{
			const b2Vec2* points = shape->polygon.vertices;
			int count = shape->polygon.count;
			B2_ASSERT( count > 0 );
			float value = b2Dot( points[0], line );
			float lower = value;
			float upper = value;
			for ( int i = 1; i < count; ++i )
			{
				value = b2Dot( points[i], line );
				lower = b2MinFloat( lower, value );
				upper = b2MaxFloat( upper, value );
			}

			return ( upper - lower ) + 2.0f * shape->polygon.radius;
		}

		case b2_segmentShape:
		{
			float value1 = b2Dot( shape->segment.point1, line );
			float value2 = b2Dot( shape->segment.point2, line );
			return b2AbsFloat( value2 - value1 );
		}

		case b2_chainSegmentShape:
		{
			float value1 = b2Dot( shape->chainSegment.segment.point1, line );
			float value2 = b2Dot( shape->chainSegment.segment.point2, line );
			return b2AbsFloat( value2 - value1 );
		}

		default:
			return 0.0f;
	}
}

b2MassData b2ComputeShapeMass( const b2Shape* shape )
{
	switch ( shape->type )
	{
		case b2_capsuleShape:
			return b2ComputeCapsuleMass( &shape->capsule, shape->density );
		case b2_circleShape:
			return b2ComputeCircleMass( &shape->circle, shape->density );
		case b2_polygonShape:
			return b2ComputePolygonMass( &shape->polygon, shape->density );
		default:
			return (b2MassData){ 0 };
	}
}

b2ShapeExtent b2ComputeShapeExtent( const b2Shape* shape, b2Vec2 localCenter )
{
	b2ShapeExtent extent = { 0 };

	switch ( shape->type )
	{
		case b2_capsuleShape:
		{
			float radius = shape->capsule.radius;
			extent.minExtent = radius;
			b2Vec2 c1 = b2Sub( shape->capsule.center1, localCenter );
			b2Vec2 c2 = b2Sub( shape->capsule.center2, localCenter );
			extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) ) + radius;
		}
		break;

		case b2_circleShape:
		{
			float radius = shape->circle.radius;
			extent.minExtent = radius;
			extent.maxExtent = b2Length( b2Sub( shape->circle.center, localCenter ) ) + radius;
		}
		break;

		case b2_polygonShape:
		{
			const b2Polygon* poly = &shape->polygon;
			float minExtent = B2_HUGE;
			float maxExtentSqr = 0.0f;
			int count = poly->count;
			for ( int i = 0; i < count; ++i )
			{
				b2Vec2 v = poly->vertices[i];
				float planeOffset = b2Dot( poly->normals[i], b2Sub( v, poly->centroid ) );
				minExtent = b2MinFloat( minExtent, planeOffset );

				float distanceSqr = b2LengthSquared( b2Sub( v, localCenter ) );
				maxExtentSqr = b2MaxFloat( maxExtentSqr, distanceSqr );
			}

			extent.minExtent = minExtent + poly->radius;
			extent.maxExtent = sqrtf( maxExtentSqr ) + poly->radius;
		}
		break;

		case b2_segmentShape:
		{
			extent.minExtent = 0.0f;
			b2Vec2 c1 = b2Sub( shape->segment.point1, localCenter );
			b2Vec2 c2 = b2Sub( shape->segment.point2, localCenter );
			extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) );
		}
		break;

		case b2_chainSegmentShape:
		{
			extent.minExtent = 0.0f;
			b2Vec2 c1 = b2Sub( shape->chainSegment.segment.point1, localCenter );
			b2Vec2 c2 = b2Sub( shape->chainSegment.segment.point2, localCenter );
			extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) );
		}
		break;

		default:
			break;
	}

	return extent;
}

b2CastOutput b2RayCastShape( const b2RayCastInput* input, const b2Shape* shape, b2Transform transform )
{
	b2RayCastInput localInput = *input;
	localInput.origin = b2InvTransformPoint( transform, input->origin );
	localInput.translation = b2InvRotateVector( transform.q, input->translation );

	b2CastOutput output = { 0 };
	switch ( shape->type )
	{
		case b2_capsuleShape:
			output = b2CastRayCapsule( &shape->capsule, &localInput );
			break;
		case b2_circleShape:
			output = b2CastRayCircle( &shape->circle, &localInput );
			break;
		case b2_polygonShape:
			output = b2CastRayPolygon( &shape->polygon, &localInput );
			break;
		case b2_segmentShape:
			output = b2CastRaySegment( &shape->segment, &localInput, false );
			break;
		case b2_chainSegmentShape:
			output = b2CastRaySegment( &shape->chainSegment.segment, &localInput, true );
			break;
		default:
			return output;
	}

	// The output point stays in the frame of the input transform, a caller chosen frame that is
	// typically re-centered near the origin.
	output.point = b2TransformPoint( transform, output.point );
	output.normal = b2RotateVector( transform.q, output.normal );
	return output;
}

b2CastOutput b2ShapeCastShape( const b2ShapeCastInput* input, const b2Shape* shape, b2Transform transform )
{
	b2CastOutput output = { 0 };

	if ( input->proxy.count == 0 )
	{
		return output;
	}

	b2ShapeCastInput localInput = *input;

	for ( int i = 0; i < localInput.proxy.count; ++i )
	{
		localInput.proxy.points[i] = b2InvTransformPoint( transform, input->proxy.points[i] );
	}

	localInput.translation = b2InvRotateVector( transform.q, input->translation );

	switch ( shape->type )
	{
		case b2_capsuleShape:
			output = b2CastShapeCapsule( &shape->capsule, &localInput );
			break;
		case b2_circleShape:
			output = b2CastShapeCircle( &shape->circle, &localInput );
			break;
		case b2_polygonShape:
			output = b2CastShapePolygon( &shape->polygon, &localInput );
			break;
		case b2_segmentShape:
			output = b2CastShapeSegment( &shape->segment, &localInput );
			break;
		case b2_chainSegmentShape:
		{
			// Check for back side collision
			b2Vec2 approximateCentroid = localInput.proxy.points[0];
			for ( int i = 1; i < localInput.proxy.count; ++i )
			{
				approximateCentroid = b2Add( approximateCentroid, localInput.proxy.points[i] );
			}

			approximateCentroid = b2MulSV( 1.0f / localInput.proxy.count, approximateCentroid );

			b2Vec2 edge = b2Sub( shape->chainSegment.segment.point2, shape->chainSegment.segment.point1 );
			b2Vec2 r = b2Sub( approximateCentroid, shape->chainSegment.segment.point1 );

			if ( b2Cross( r, edge ) < 0.0f )
			{
				// Shape cast starts behind
				return output;
			}

			output = b2CastShapeSegment( &shape->chainSegment.segment, &localInput );
		}
		break;
		default:
			return output;
	}

	// Same frame contract as b2RayCastShape, the point stays in the input transform frame
	output.point = b2TransformPoint( transform, output.point );
	output.normal = b2RotateVector( transform.q, output.normal );
	return output;
}

b2PlaneResult b2CollideMover( const b2Capsule* mover, const b2Shape* shape, b2Transform transform )
{
	b2Capsule localMover;
	localMover.center1 = b2InvTransformPoint( transform, mover->center1 );
	localMover.center2 = b2InvTransformPoint( transform, mover->center2 );
	localMover.radius = mover->radius;

	b2PlaneResult result = { 0 };
	switch ( shape->type )
	{
		case b2_capsuleShape:
			result = b2CollideMoverAndCapsule( &localMover, &shape->capsule );
			break;
		case b2_circleShape:
			result = b2CollideMoverAndCircle( &localMover, &shape->circle );
			break;
		case b2_polygonShape:
			result = b2CollideMoverAndPolygon( &localMover, &shape->polygon );
			break;
		case b2_segmentShape:
			result = b2CollideMoverAndSegment( &localMover, &shape->segment );
			break;
		case b2_chainSegmentShape:
			result = b2CollideMoverAndSegment( &localMover, &shape->chainSegment.segment );
			break;
		default:
			return result;
	}

	if ( result.hit == false )
	{
		return result;
	}

	result.plane.normal = b2RotateVector( transform.q, result.plane.normal );
	return result;
}

void b2CreateShapeProxy( b2World* world, b2Shape* shape, b2BodyType type, b2WorldTransform transform, bool forcePairCreation )
{
	B2_ASSERT( shape->proxyKey == B2_NULL_INDEX );

	b2AABB* fatAABB = world->fatAABBs.data + shape->id;
	b2UpdateShapeAABBs( shape, fatAABB, transform, type );

	// Create proxies in the broad-phase.
	shape->proxyKey = b2BroadPhase_CreateProxy( &world->broadPhase, type, *fatAABB, shape->filter.categoryBits, shape->id,
												forcePairCreation );
	B2_ASSERT( B2_PROXY_TYPE( shape->proxyKey ) < b2_bodyTypeCount );
}

void b2DestroyShapeProxy( b2Shape* shape, b2BroadPhase* bp )
{
	if ( shape->proxyKey != B2_NULL_INDEX )
	{
		b2BroadPhase_DestroyProxy( bp, shape->proxyKey );
		shape->proxyKey = B2_NULL_INDEX;
	}
}

b2ShapeProxy b2MakeShapeDistanceProxy( const b2Shape* shape )
{
	switch ( shape->type )
	{
		case b2_capsuleShape:
			return b2MakeProxy( &shape->capsule.center1, 2, shape->capsule.radius );
		case b2_circleShape:
			return b2MakeProxy( &shape->circle.center, 1, shape->circle.radius );
		case b2_polygonShape:
			return b2MakeProxy( shape->polygon.vertices, shape->polygon.count, shape->polygon.radius );
		case b2_segmentShape:
			return b2MakeProxy( &shape->segment.point1, 2, 0.0f );
		case b2_chainSegmentShape:
			return b2MakeProxy( &shape->chainSegment.segment.point1, 2, 0.0f );
		default:
		{
			B2_ASSERT( false );
			b2ShapeProxy empty = { 0 };
			return empty;
		}
	}
}

b2BodyId b2Shape_GetBody( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return b2MakeBodyId( world, shape->bodyId );
}

b2WorldId b2Shape_GetWorld( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	return (b2WorldId){ shapeId.world0 + 1, world->generation };
}

void b2Shape_SetUserData( b2ShapeId shapeId, void* userData )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	shape->userData = userData;
}

void* b2Shape_GetUserData( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->userData;
}

bool b2Shape_IsSensor( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->sensorIndex != B2_NULL_INDEX;
}

bool b2Shape_TestPoint( b2ShapeId shapeId, b2Pos point )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );

	b2WorldTransform transform = b2GetBodyTransform( world, shape->bodyId );
	b2Vec2 localPoint = b2InvTransformWorldPoint( transform, point );

	bool result;
	switch ( shape->type )
	{
		case b2_capsuleShape:
			result = b2PointInCapsule( &shape->capsule, localPoint );
			break;

		case b2_circleShape:
			result = b2PointInCircle( &shape->circle, localPoint );
			break;

		case b2_polygonShape:
			result = b2PointInPolygon( &shape->polygon, localPoint );
			break;

		default:
			result = false;
			break;
	}

	return result;
}

b2WorldCastOutput b2Shape_CastRay( b2ShapeId shapeId, b2Pos origin, b2Vec2 translation )
{
	B2_ASSERT( b2IsValidPos( origin ) );
	B2_ASSERT( b2IsValidVec2( translation ) );

	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );

	// Re-center on the origin so the cast runs in float precision
	b2Transform transform = b2ToRelativeTransform( b2GetBodyTransform( world, shape->bodyId ), origin );

	// The ray starts at the origin, so its origin in the re-centered frame is zero
	b2RayCastInput input = { b2Vec2_zero, translation, 1.0f };

	// Lift the re-centered float result back to a world position
	b2CastOutput local = b2RayCastShape( &input, shape, transform );
	b2WorldCastOutput output;
	output.normal = local.normal;
	output.point = b2OffsetPos( origin, local.point );
	output.fraction = local.fraction;
	output.iterations = local.iterations;
	output.hit = local.hit;

	return output;
}

void b2Shape_SetDensity( b2ShapeId shapeId, float density, bool updateBodyMass )
{
	B2_CHECK_INPUT( b2IsValidFloat( density ) && density >= 0.0f );

	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ShapeSetDensity, shapeId, density, updateBodyMass );

	b2Shape* shape = b2GetShape( world, shapeId );
	if ( density == shape->density )
	{
		// early return to avoid expensive function
		return;
	}

	shape->density = density;

	if ( updateBodyMass == true )
	{
		b2Body* body = b2Array_Get( world->bodies, shape->bodyId );
		b2UpdateBodyMassData( world, body );
	}
}

float b2Shape_GetDensity( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->density;
}

void b2Shape_SetFriction( b2ShapeId shapeId, float friction )
{
	B2_CHECK_INPUT( b2IsValidFloat( friction ) && friction >= 0.0f );

	b2World* world = b2GetWorld( shapeId.world0 );
	B2_ASSERT( world->locked == false );
	if ( world->locked )
	{
		return;
	}

	B2_REC( world, ShapeSetFriction, shapeId, friction );

	b2Shape* shape = b2GetShape( world, shapeId );
	shape->material.friction = friction;
}

float b2Shape_GetFriction( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->material.friction;
}

void b2Shape_SetRestitution( b2ShapeId shapeId, float restitution )
{
	B2_CHECK_INPUT( b2IsValidFloat( restitution ) && restitution >= 0.0f );

	b2World* world = b2GetWorld( shapeId.world0 );
	B2_ASSERT( world->locked == false );
	if ( world->locked )
	{
		return;
	}

	B2_REC( world, ShapeSetRestitution, shapeId, restitution );

	b2Shape* shape = b2GetShape( world, shapeId );
	shape->material.restitution = restitution;
}

float b2Shape_GetRestitution( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->material.restitution;
}

void b2Shape_SetUserMaterial( b2ShapeId shapeId, uint64_t material )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	B2_ASSERT( world->locked == false );
	if ( world->locked )
	{
		return;
	}

	B2_REC( world, ShapeSetUserMaterial, shapeId, material );

	b2Shape* shape = b2GetShape( world, shapeId );
	shape->material.userMaterialId = material;
}

uint64_t b2Shape_GetUserMaterial( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->material.userMaterialId;
}

b2SurfaceMaterial b2Shape_GetSurfaceMaterial( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->material;
}

void b2Shape_SetSurfaceMaterial( b2ShapeId shapeId, const b2SurfaceMaterial* surfaceMaterial )
{
	B2_CHECK_INPUT( b2IsValidMaterial( surfaceMaterial ) );

	b2World* world = b2GetWorld( shapeId.world0 );
	B2_REC( world, ShapeSetSurfaceMaterial, shapeId, *surfaceMaterial );
	b2Shape* shape = b2GetShape( world, shapeId );
	shape->material = *surfaceMaterial;
}

b2Filter b2Shape_GetFilter( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->filter;
}

static void b2ResetProxy( b2World* world, b2Shape* shape, bool destroyProxy )
{
	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );

	int shapeId = shape->id;

	// destroy all contacts associated with this shape
	int contactKey = body->headContactKey;
	while ( contactKey != B2_NULL_INDEX )
	{
		int contactId = contactKey >> 1;
		int edgeIndex = contactKey & 1;

		b2Contact* contact = b2Array_Get( world->contacts, contactId );
		contactKey = contact->edges[edgeIndex].nextKey;

		if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId )
		{
			b2DestroyContact( world, contact );
		}
	}

	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );
	if ( shape->proxyKey != B2_NULL_INDEX )
	{
		b2BodyType proxyType = B2_PROXY_TYPE( shape->proxyKey );
		b2AABB* fatAABB = world->fatAABBs.data + shapeId;
		b2UpdateShapeAABBs( shape, fatAABB, transform, proxyType );

		if ( destroyProxy )
		{
			b2BroadPhase_DestroyProxy( &world->broadPhase, shape->proxyKey );

			bool forcePairCreation = true;
			shape->proxyKey = b2BroadPhase_CreateProxy( &world->broadPhase, proxyType, *fatAABB, shape->filter.categoryBits,
														shapeId, forcePairCreation );
		}
		else
		{
			b2BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, *fatAABB );
		}
	}
	else
	{
		b2BodyType proxyType = body->type;
		b2UpdateShapeAABBs( shape, world->fatAABBs.data + shapeId, transform, proxyType );
	}

	b2ValidateSolverSets( world );
}

void b2Shape_SetFilter( b2ShapeId shapeId, b2Filter filter )
{
	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ShapeSetFilter, shapeId, filter );

	b2Shape* shape = b2GetShape( world, shapeId );
	if ( filter.maskBits == shape->filter.maskBits && filter.categoryBits == shape->filter.categoryBits &&
		 filter.groupIndex == shape->filter.groupIndex )
	{
		return;
	}

	// If the category bits change, I need to destroy the proxy because it affects the tree sorting.
	bool destroyProxy = filter.categoryBits != shape->filter.categoryBits;

	shape->filter = filter;

	b2ResetProxy( world, shape, destroyProxy );

	// note: this does not immediately update sensor overlaps. Instead sensor
	// overlaps are updated the next time step
}

void b2Shape_EnableSensorEvents( b2ShapeId shapeId, bool flag )
{
	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ShapeEnableSensorEvents, shapeId, flag );

	b2Shape* shape = b2GetShape( world, shapeId );
	shape->enableSensorEvents = flag;
}

bool b2Shape_AreSensorEventsEnabled( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->enableSensorEvents;
}

void b2Shape_EnableContactEvents( b2ShapeId shapeId, bool flag )
{
	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ShapeEnableContactEvents, shapeId, flag );

	b2Shape* shape = b2GetShape( world, shapeId );
	shape->enableContactEvents = flag;
}

bool b2Shape_AreContactEventsEnabled( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->enableContactEvents;
}

void b2Shape_EnablePreSolveEvents( b2ShapeId shapeId, bool flag )
{
	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ShapeEnablePreSolveEvents, shapeId, flag );

	b2Shape* shape = b2GetShape( world, shapeId );
	shape->enablePreSolveEvents = flag;
}

bool b2Shape_ArePreSolveEventsEnabled( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->enablePreSolveEvents;
}

void b2Shape_EnableHitEvents( b2ShapeId shapeId, bool flag )
{
	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ShapeEnableHitEvents, shapeId, flag );

	b2Shape* shape = b2GetShape( world, shapeId );
	shape->enableHitEvents = flag;
}

bool b2Shape_AreHitEventsEnabled( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->enableHitEvents;
}

b2ShapeType b2Shape_GetType( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->type;
}

b2Circle b2Shape_GetCircle( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	B2_ASSERT( shape->type == b2_circleShape );
	return shape->circle;
}

b2Segment b2Shape_GetSegment( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	B2_ASSERT( shape->type == b2_segmentShape );
	return shape->segment;
}

b2ChainSegment b2Shape_GetChainSegment( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	B2_ASSERT( shape->type == b2_chainSegmentShape );
	return shape->chainSegment;
}

b2Capsule b2Shape_GetCapsule( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	B2_ASSERT( shape->type == b2_capsuleShape );
	return shape->capsule;
}

b2Polygon b2Shape_GetPolygon( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	B2_ASSERT( shape->type == b2_polygonShape );
	return shape->polygon;
}

void b2Shape_SetCircle( b2ShapeId shapeId, const b2Circle* circle )
{
	B2_CHECK_INPUT( b2IsValidCircle( circle ) );

	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ShapeSetCircle, shapeId, *circle );

	b2Shape* shape = b2GetShape( world, shapeId );
	shape->circle = *circle;
	shape->type = b2_circleShape;
	shape->aabbMargin = b2ComputeShapeMargin( shape );

	bool destroyProxy = true;
	b2ResetProxy( world, shape, destroyProxy );
}

void b2Shape_SetCapsule( b2ShapeId shapeId, const b2Capsule* capsule )
{
	B2_CHECK_INPUT( b2IsValidCapsule( capsule ) );

	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	float lengthSqr = b2DistanceSquared( capsule->center1, capsule->center2 );
	if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP )
	{
		return;
	}

	B2_REC( world, ShapeSetCapsule, shapeId, *capsule );

	b2Shape* shape = b2GetShape( world, shapeId );
	shape->capsule = *capsule;
	shape->type = b2_capsuleShape;
	shape->aabbMargin = b2ComputeShapeMargin( shape );

	bool destroyProxy = true;
	b2ResetProxy( world, shape, destroyProxy );
}

void b2Shape_SetSegment( b2ShapeId shapeId, const b2Segment* segment )
{
	B2_CHECK_INPUT( b2IsValidSegment( segment ) );

	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ShapeSetSegment, shapeId, *segment );

	b2Shape* shape = b2GetShape( world, shapeId );
	shape->segment = *segment;
	shape->type = b2_segmentShape;
	shape->aabbMargin = b2ComputeShapeMargin( shape );

	bool destroyProxy = true;
	b2ResetProxy( world, shape, destroyProxy );
}

void b2Shape_SetPolygon( b2ShapeId shapeId, const b2Polygon* polygon )
{
	B2_CHECK_INPUT( b2IsValidPolygon( polygon ) );

	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ShapeSetPolygon, shapeId, *polygon );

	b2Shape* shape = b2GetShape( world, shapeId );
	shape->polygon = *polygon;
	shape->type = b2_polygonShape;
	shape->aabbMargin = b2ComputeShapeMargin( shape );

	bool destroyProxy = true;
	b2ResetProxy( world, shape, destroyProxy );
}

void b2Shape_SetChainSegment( b2ShapeId shapeId, const b2ChainSegment* chainSegment )
{
	B2_CHECK_INPUT( b2IsValidChainSegment( chainSegment ) );

	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	b2Shape* shape = b2GetShape( world, shapeId );

	// Cannot modify a chain segment that has a parent chain shape
	if ( shape->type == b2_chainSegmentShape && shape->chainSegment.chainId != B2_NULL_INDEX )
	{
		B2_ASSERT( false );
		return;
	}

	float lengthSqr = b2DistanceSquared( chainSegment->segment.point1, chainSegment->segment.point2 );
	if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP )
	{
		return;
	}

	B2_REC( world, ShapeSetChainSegment, shapeId, *chainSegment );

	shape->chainSegment = *chainSegment;
	shape->chainSegment.chainId = B2_NULL_INDEX;
	shape->type = b2_chainSegmentShape;
	shape->aabbMargin = b2ComputeShapeMargin( shape );

	bool destroyProxy = true;
	b2ResetProxy( world, shape, destroyProxy );
}

b2ChainId b2Shape_GetParentChain( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	b2Shape* shape = b2GetShape( world, shapeId );
	if ( shape->type == b2_chainSegmentShape )
	{
		int chainId = shape->chainSegment.chainId;
		if ( chainId != B2_NULL_INDEX )
		{
			b2ChainShape* chain = b2Array_Get( world->chainShapes, chainId );
			b2ChainId id = { chainId + 1, shapeId.world0, chain->generation };
			return id;
		}
	}

	return (b2ChainId){ 0 };
}

int b2Shape_GetContactCapacity( b2ShapeId shapeId )
{
	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return 0;
	}

	b2Shape* shape = b2GetShape( world, shapeId );
	if ( shape->sensorIndex != B2_NULL_INDEX )
	{
		return 0;
	}

	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );

	// Conservative and fast
	return body->contactCount;
}

int b2Shape_GetContactData( b2ShapeId shapeId, b2ContactData* contactData, int capacity )
{
	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return 0;
	}

	b2Shape* shape = b2GetShape( world, shapeId );
	if ( shape->sensorIndex != B2_NULL_INDEX )
	{
		return 0;
	}

	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );
	int contactKey = body->headContactKey;
	int index = 0;
	while ( contactKey != B2_NULL_INDEX && index < capacity )
	{
		int contactId = contactKey >> 1;
		int edgeIndex = contactKey & 1;

		b2Contact* contact = b2Array_Get( world->contacts, contactId );

		// Does contact involve this shape and is it touching?
		if ( ( contact->shapeIdA == shapeId.index1 - 1 || contact->shapeIdB == shapeId.index1 - 1 ) &&
			 ( contact->flags & b2_contactTouchingFlag ) != 0 )
		{
			b2Shape* shapeA = world->shapes.data + contact->shapeIdA;
			b2Shape* shapeB = world->shapes.data + contact->shapeIdB;

			contactData[index].contactId = (b2ContactId){ contact->contactId + 1, shapeId.world0, 0, contact->generation };
			contactData[index].shapeIdA = (b2ShapeId){ shapeA->id + 1, shapeId.world0, shapeA->generation };
			contactData[index].shapeIdB = (b2ShapeId){ shapeB->id + 1, shapeId.world0, shapeB->generation };

			b2ContactSim* contactSim = b2GetContactSim( world, contact );
			contactData[index].manifold = contactSim->manifold;
			index += 1;
		}

		contactKey = contact->edges[edgeIndex].nextKey;
	}

	B2_ASSERT( index <= capacity );

	return index;
}

int b2Shape_GetSensorCapacity( b2ShapeId shapeId )
{
	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return 0;
	}

	b2Shape* shape = b2GetShape( world, shapeId );
	if ( shape->sensorIndex == B2_NULL_INDEX )
	{
		return 0;
	}

	b2Sensor* sensor = b2Array_Get( world->sensors, shape->sensorIndex );
	return sensor->overlaps2.count;
}

int b2Shape_GetSensorData( b2ShapeId shapeId, b2ShapeId* visitorIds, int capacity )
{
	b2World* world = b2GetWorldLocked( shapeId.world0 );
	if ( world == NULL )
	{
		return 0;
	}

	b2Shape* shape = b2GetShape( world, shapeId );
	if ( shape->sensorIndex == B2_NULL_INDEX )
	{
		return 0;
	}

	b2Sensor* sensor = b2Array_Get( world->sensors, shape->sensorIndex );

	int count = b2MinInt( sensor->overlaps2.count, capacity );
	b2Visitor* refs = sensor->overlaps2.data;
	for ( int i = 0; i < count; ++i )
	{
		b2ShapeId visitorId = {
			.index1 = refs[i].shapeId + 1,
			.world0 = shapeId.world0,
			.generation = refs[i].generation,
		};

		visitorIds[i] = visitorId;
	}

	return count;
}

b2AABB b2Shape_GetBounds( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	if ( world == NULL )
	{
		return (b2AABB){ 0 };
	}

	b2Shape* shape = b2GetShape( world, shapeId );
	return shape->aabb;
}

b2MassData b2Shape_ComputeMassData( b2ShapeId shapeId )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	if ( world == NULL )
	{
		return (b2MassData){ 0 };
	}

	b2Shape* shape = b2GetShape( world, shapeId );
	return b2ComputeShapeMass( shape );
}

b2Pos b2Shape_GetClosestPoint( b2ShapeId shapeId, b2Pos target )
{
	b2World* world = b2GetWorld( shapeId.world0 );
	if ( world == NULL )
	{
		return b2Pos_zero;
	}

	b2Shape* shape = b2GetShape( world, shapeId );
	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );
	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );

	// The target rides in as the frame of proxy B, so the relative pose is differenced in double
	// and the result stays exact far from the origin
	b2Vec2 zero = b2Vec2_zero;
	b2WorldTransform targetTransform = { target, b2Rot_identity };

	b2DistanceInput input;
	input.proxyA = b2MakeShapeDistanceProxy( shape );
	input.proxyB = b2MakeProxy( &zero, 1, 0.0f );
	input.transform = b2InvMulWorldTransforms( transform, targetTransform );
	input.useRadii = true;

	b2SimplexCache cache = { 0 };
	b2DistanceOutput output = b2ComputeShapeDistance( &input, &cache, NULL, 0 );

	return b2TransformWorldPoint( transform, output.pointA );
}

// https://en.wikipedia.org/wiki/Density_of_air
// https://www.engineeringtoolbox.com/wind-load-d_1775.html
// force = 0.5 * air_density * velocity^2 * area
// https://en.wikipedia.org/wiki/Lift_(force)
void b2Shape_ApplyWind( b2ShapeId shapeId, b2Vec2 wind, float drag, float lift, bool wake )
{
	B2_CHECK_INPUT( b2IsValidVec2( wind ) );
	B2_CHECK_INPUT( b2IsValidFloat( drag ) );
	B2_CHECK_INPUT( b2IsValidFloat( lift ) );

	b2World* world = b2GetWorld( shapeId.world0 );
	if ( world == NULL )
	{
		return;
	}

	B2_REC( world, ShapeApplyWind, shapeId, wind, drag, lift, wake );

	b2Shape* shape = b2GetShape( world, shapeId );

	b2ShapeType shapeType = shape->type;
	if ( shapeType != b2_circleShape && shapeType != b2_capsuleShape && shapeType != b2_polygonShape )
	{
		return;
	}

	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );

	if ( body->type != b2_dynamicBody )
	{
		return;
	}

	if ( body->setIndex >= b2_firstSleepingSet && wake == false )
	{
		return;
	}

	b2BodySim* sim = b2GetBodySim( world, body );

	if ( body->setIndex != b2_awakeSet )
	{
		// Must wake for state to exist
		b2WakeBody( world, body );
	}

	B2_ASSERT( body->setIndex == b2_awakeSet );

	b2BodyState* state = b2GetBodyState( world, body );
	b2WorldTransform transform = sim->transform;

	float lengthUnits = b2GetLengthUnitsPerMeter();
	float volumeUnits = lengthUnits * lengthUnits * lengthUnits;

	// In 2D I'm assuming unit depth
	float airDensity = 1.2250f / ( volumeUnits );

	b2Vec2 force = { 0 };
	float torque = 0.0f;

	switch ( shape->type )
	{
		case b2_circleShape:
		{
			float radius = shape->circle.radius;
			b2Vec2 centroid = shape->localCentroid;
			b2Vec2 lever = b2RotateVector( transform.q, b2Sub( centroid, sim->localCenter ) );
			b2Vec2 shapeVelocity = b2Add( state->linearVelocity, b2CrossSV( state->angularVelocity, lever ) );
			b2Vec2 relativeVelocity = b2MulSub( wind, drag, shapeVelocity );
			float speed;
			b2Vec2 direction = b2GetLengthAndNormalize( &speed, relativeVelocity );
			float projectedArea = 2.0f * radius;
			force = b2MulSV( 0.5f * airDensity * projectedArea * speed * speed, direction );
			torque = b2Cross( lever, force );
		}
		break;

		case b2_capsuleShape:
		{
			b2Vec2 centroid = shape->localCentroid;
			b2Vec2 lever = b2RotateVector( transform.q, b2Sub( centroid, sim->localCenter ) );
			b2Vec2 shapeVelocity = b2Add( state->linearVelocity, b2CrossSV( state->angularVelocity, lever ) );
			b2Vec2 relativeVelocity = b2MulSub( wind, drag, shapeVelocity );
			float speed;
			b2Vec2 direction = b2GetLengthAndNormalize( &speed, relativeVelocity );

			b2Vec2 d = b2Sub( shape->capsule.center2, shape->capsule.center1 );
			d = b2RotateVector( transform.q, d );

			float radius = shape->capsule.radius;
			float projectedArea = 2.0f * radius + b2AbsFloat( b2Cross( d, direction ) );

			// Normal that opposes the wind
			b2Vec2 normal = b2LeftPerp( b2Normalize( d ) );
			normal = b2Dot( normal, direction ) > 0.0f ? b2Neg( normal ) : normal;

			// portion of wind that is perpendicular to surface
			b2Vec2 liftDirection = b2CrossSV( b2Cross( normal, direction ), direction );

			float forceMagnitude = 0.5f * airDensity * projectedArea * speed * speed;
			force = b2MulSV( forceMagnitude, b2MulAdd( direction, lift, liftDirection ) );

			b2Vec2 edgeLever = b2MulAdd( lever, radius, normal );
			torque = b2Cross( edgeLever, force );
		}
		break;

		case b2_polygonShape:
		{
			b2Vec2 centroid = shape->localCentroid;
			b2Vec2 lever = b2RotateVector( transform.q, b2Sub( centroid, sim->localCenter ) );
			b2Vec2 shapeVelocity = b2Add( state->linearVelocity, b2CrossSV( state->angularVelocity, lever ) );
			b2Vec2 relativeVelocity = b2MulSub( wind, drag, shapeVelocity );
			float speed;
			b2Vec2 direction = b2GetLengthAndNormalize( &speed, relativeVelocity );

			// polygon radius is ignored for simplicity
			int count = shape->polygon.count;
			b2Vec2* vertices = shape->polygon.vertices;

			b2Vec2 v1 = vertices[count - 1];
			for ( int i = 0; i < count; ++i )
			{
				b2Vec2 v2 = vertices[i];
				b2Vec2 d = b2Sub( v2, v1 );
				b2Vec2 edgeCenter = b2Lerp( v1, v2, 0.5f );
				v1 = v2;

				d = b2RotateVector( transform.q, d );

				float projectedArea = b2Cross( d, direction );
				if ( projectedArea <= 0.0f )
				{
					// back facing
					continue;
				}

				b2Vec2 normal = b2RightPerp( b2Normalize( d ) );

				// portion of wind that is perpendicular to surface
				b2Vec2 liftDirection = b2CrossSV( b2Cross( normal, direction ), direction );

				float forceMagnitude = 0.5f * airDensity * projectedArea * speed * speed;
				b2Vec2 f = b2MulSV( forceMagnitude, b2MulAdd( direction, lift, liftDirection ) );

				b2Vec2 edgeLever = b2RotateVector( transform.q, b2Sub( edgeCenter, sim->localCenter ) );

				force = b2Add( force, f );
				torque += b2Cross( edgeLever, f );
			}
		}
		break;

		default:
			break;
	}

	sim->force = b2Add( sim->force, force );
	sim->torque += torque;
}