diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index d39993e0..b4d1c559 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -47,6 +47,9 @@ B2_API void b2World_Step(b2WorldId worldId, float timeStep, int32_t subStepCount /// Call this to draw shapes and other debug draw data. This is intentionally non-const. B2_API void b2World_Draw(b2WorldId worldId, b2DebugDraw* debugDraw); +/// Get the body events for the current time step. The event data is transient. Do not store a reference to this data. +B2_API b2BodyEvents b2World_GetBodyEvents(b2WorldId worldId); + /// Get sensor events for the current time step. The event data is transient. Do not store a reference to this data. B2_API b2SensorEvents b2World_GetSensorEvents(b2WorldId worldId); diff --git a/include/box2d/event_types.h b/include/box2d/event_types.h index f8f38fcd..cf4095c8 100644 --- a/include/box2d/event_types.h +++ b/include/box2d/event_types.h @@ -70,3 +70,25 @@ typedef struct b2ContactData b2ShapeId shapeIdB; b2Manifold manifold; } b2ContactData; + +/// Triggered when a body moves from simulation. Not reported for bodies moved by the user. +/// This also has a flag to indicate that the body went to sleep so the application can also +/// sleep that actor/entity/object associated with the body. +/// On the other hand if the flag does not indicate the body went to sleep then the application +/// can treat the actor/entity/object associated with the body as awake. +typedef struct b2BodyMoveEvent +{ + b2Transform transform; + b2BodyId bodyId; + void* userData; + bool fellAsleep; +} b2BodyMoveEvent; + +/// Body events are buffered in the Box2D world and are available +/// as event arrays after the time step is complete. +/// Note: this date becomes invalid if bodies are destroyed +typedef struct b2BodyEvents +{ + b2BodyMoveEvent* moveEvents; + int moveCount; +} b2BodyEvents; diff --git a/src/array.c b/src/array.c index e8b97637..3ca5476c 100644 --- a/src/array.c +++ b/src/array.c @@ -8,7 +8,7 @@ #include -void* b2CreateArray(int32_t elementSize, int32_t capacity) +void* b2CreateArray(int elementSize, int capacity) { void* result = (b2ArrayHeader*)b2Alloc(sizeof(b2ArrayHeader) + elementSize * capacity) + 1; b2Array(result).count = 0; @@ -16,20 +16,20 @@ void* b2CreateArray(int32_t elementSize, int32_t capacity) return result; } -void b2DestroyArray(void* a, int32_t elementSize) +void b2DestroyArray(void* a, int elementSize) { - int32_t capacity = b2Array(a).capacity; - int32_t size = sizeof(b2ArrayHeader) + elementSize * capacity; + int capacity = b2Array(a).capacity; + int size = sizeof(b2ArrayHeader) + elementSize * capacity; b2Free(((b2ArrayHeader*)a) - 1, size); } -void b2Array_Grow(void** a, int32_t elementSize) +void b2Array_Grow(void** a, int elementSize) { - int32_t capacity = b2Array(*a).capacity; + int capacity = b2Array(*a).capacity; B2_ASSERT(capacity == b2Array(*a).count); // grow by 50% - int32_t newCapacity = capacity + (capacity >> 1); + int newCapacity = capacity + (capacity >> 1); newCapacity = newCapacity >= 2 ? newCapacity : 2; void* tmp = *a; *a = (b2ArrayHeader*)b2Alloc(sizeof(b2ArrayHeader) + elementSize * newCapacity) + 1; @@ -38,3 +38,27 @@ void b2Array_Grow(void** a, int32_t elementSize) memcpy(*a, tmp, capacity * elementSize); b2DestroyArray(tmp, elementSize); } + +void b2Array_Resize(void** a, int elementSize, int count) +{ + int capacity = b2Array(*a).capacity; + if (capacity >= count) + { + b2Array(*a).count = count; + return; + } + + int originalCount = b2Array(*a).count; + + // grow by 50% + int newCapacity = count + (count >> 1); + newCapacity = newCapacity >= 2 ? newCapacity : 2; + void* tmp = *a; + *a = (b2ArrayHeader*)b2Alloc(sizeof(b2ArrayHeader) + elementSize * newCapacity) + 1; + b2Array(*a).capacity = newCapacity; + b2Array(*a).count = count; + + // copy existing elements + memcpy(*a, tmp, originalCount * elementSize); + b2DestroyArray(tmp, elementSize); +} diff --git a/src/array.h b/src/array.h index 750ae331..3a80fbd7 100644 --- a/src/array.h +++ b/src/array.h @@ -5,19 +5,18 @@ #include "core.h" -#include - typedef struct b2ArrayHeader { - int32_t count; - int32_t capacity; + int count; + int capacity; } b2ArrayHeader; #define b2Array(a) ((b2ArrayHeader*)(a))[-1] -void* b2CreateArray(int32_t elementSize, int32_t capacity); -void b2DestroyArray(void* a, int32_t elementSize); -void b2Array_Grow(void** a, int32_t elementSize); +void* b2CreateArray(int elementSize, int capacity); +void b2DestroyArray(void* a, int elementSize); +void b2Array_Grow(void** a, int elementSize); +void b2Array_Resize(void** a, int elementSize, int count); #define b2Array_Check(a, index) B2_ASSERT(0 <= index && index < b2Array(a).count) diff --git a/src/body.c b/src/body.c index b35f72d8..52ccabf6 100644 --- a/src/body.c +++ b/src/body.c @@ -260,6 +260,7 @@ b2BodyId b2CreateBody(b2WorldId worldId, const b2BodyDef* def) body->islandIndex = B2_NULL_INDEX; body->islandPrev = B2_NULL_INDEX; body->islandNext = B2_NULL_INDEX; + body->solverIndex = B2_NULL_INDEX; if (body->isEnabled) { diff --git a/src/body.h b/src/body.h index 69e7e5af..63296664 100644 --- a/src/body.h +++ b/src/body.h @@ -96,6 +96,8 @@ typedef struct b2Body int32_t islandPrev; int32_t islandNext; + int32_t solverIndex; + float mass, invMass; // Rotational inertia about the center of mass. diff --git a/src/contact_solver.c b/src/contact_solver.c index da09250c..49d68d88 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -25,18 +25,13 @@ void b2PrepareOverflowContacts(b2StepContext* context) b2World* world = context->world; b2ConstraintGraph* graph = context->graph; b2Contact* contacts = world->contacts; - const int32_t* bodyMap = context->bodyToSolverMap; - const b2BodyParam* params = context->bodyParams; - b2BodyState* states = context->bodyStates; + b2Body* bodies = world->bodies; + int32_t bodyCapacity = world->bodyPool.capacity; b2ContactConstraint* constraints = graph->overflow.contactConstraints; int32_t* contactIndices = graph->overflow.contactArray; int32_t contactCount = b2Array(graph->overflow.contactArray).count; - // This is a dummy body to represent a static body because static bodies don't have a solver body. - b2BodyState dummyState = b2_identityBodyState; - b2BodyParam dummyParam = {0}; - b2Softness contactSoftness = context->contactSoftness; b2Softness staticSoftness = context->staticSoftness; @@ -52,34 +47,34 @@ void b2PrepareOverflowContacts(b2StepContext* context) B2_ASSERT(0 < pointCount && pointCount <= 2); - // resolve solver body indices - int32_t indexA = bodyMap[contact->edges[0].bodyIndex]; - int32_t indexB = bodyMap[contact->edges[1].bodyIndex]; + int32_t indexA = contact->edges[0].bodyIndex; + int32_t indexB = contact->edges[1].bodyIndex; + B2_ASSERT(0 <= indexA && indexA < bodyCapacity); + B2_ASSERT(0 <= indexB && indexB < bodyCapacity); + + b2Body* bodyA = context->bodies + indexA; + b2Body* bodyB = context->bodies + indexB; + B2_ASSERT(bodyA->object.index == bodyA->object.next); + B2_ASSERT(bodyB->object.index == bodyB->object.next); b2ContactConstraint* constraint = constraints + i; constraint->contact = contact; - constraint->indexA = indexA; - constraint->indexB = indexB; + constraint->indexA = bodyA->solverIndex; + constraint->indexB = bodyB->solverIndex; constraint->normal = manifold->normal; constraint->friction = contact->friction; constraint->restitution = contact->restitution; constraint->pointCount = pointCount; - b2BodyState* stateA = indexA == B2_NULL_INDEX ? &dummyState : states + indexA; - const b2BodyParam* paramA = indexA == B2_NULL_INDEX ? &dummyParam : params + indexA; - - b2BodyState* stateB = indexB == B2_NULL_INDEX ? &dummyState : states + indexB; - const b2BodyParam* paramB = indexB == B2_NULL_INDEX ? &dummyParam : params + indexB; - - b2Vec2 vA = stateA->linearVelocity; - float wA = stateA->angularVelocity; - float mA = paramA->invMass; - float iA = paramA->invI; + b2Vec2 vA = bodyA->linearVelocity; + float wA = bodyA->angularVelocity; + float mA = bodyA->invMass; + float iA = bodyA->invI; - b2Vec2 vB = stateB->linearVelocity; - float wB = stateB->angularVelocity; - float mB = paramB->invMass; - float iB = paramB->invI; + b2Vec2 vB = bodyB->linearVelocity; + float wB = bodyB->angularVelocity; + float mB = bodyB->invMass; + float iB = bodyB->invI; // Stiffer for static contacts to avoid bodies getting pushed through the ground if (indexA == B2_NULL_INDEX || indexB == B2_NULL_INDEX) @@ -568,19 +563,13 @@ static void b2ScatterBodies(b2BodyState* restrict bodies, int32_t* restrict indi void b2PrepareContactsTask(int32_t startIndex, int32_t endIndex, b2StepContext* context) { b2TracyCZoneNC(prepare_contact, "Prepare Contact", b2_colorYellow, true); - b2World* world = context->world; b2Contact* contacts = world->contacts; - const int32_t* bodyMap = context->bodyToSolverMap; - b2BodyState* states = context->bodyStates; - const b2BodyParam* params = context->bodyParams; + b2Body* bodies = world->bodies; + int32_t bodyCapacity = world->bodyPool.capacity; b2ContactConstraintSIMD* constraints = context->contactConstraints; const int32_t* contactIndices = context->contactIndices; - // dummy body to represent a static body - b2BodyState dummyState = b2_identityBodyState; - b2BodyParam dummyParam = {0}; - b2Softness contactSoftness = context->contactSoftness; b2Softness staticSoftness = context->staticSoftness; @@ -600,28 +589,29 @@ void b2PrepareContactsTask(int32_t startIndex, int32_t endIndex, b2StepContext* b2Contact* contact = contacts + contactIndex; const b2Manifold* manifold = &contact->manifold; - int32_t indexA = bodyMap[contact->edges[0].bodyIndex]; - int32_t indexB = bodyMap[contact->edges[1].bodyIndex]; - - constraint->indexA[j] = indexA; - constraint->indexB[j] = indexB; + int32_t indexA = contact->edges[0].bodyIndex; + int32_t indexB = contact->edges[1].bodyIndex; + B2_ASSERT(0 <= indexA && indexA < bodyCapacity); + B2_ASSERT(0 <= indexB && indexB < bodyCapacity); - b2BodyState* stateA = indexA == B2_NULL_INDEX ? &dummyState : states + indexA; - const b2BodyParam* paramA = indexA == B2_NULL_INDEX ? &dummyParam : params + indexA; + b2Body* bodyA = context->bodies + indexA; + b2Body* bodyB = context->bodies + indexB; + B2_ASSERT(bodyA->object.index == bodyA->object.next); + B2_ASSERT(bodyB->object.index == bodyB->object.next); - b2BodyState* stateB = indexB == B2_NULL_INDEX ? &dummyState : states + indexB; - const b2BodyParam* paramB = indexB == B2_NULL_INDEX ? &dummyParam : params + indexB; + constraint->indexA[j] = bodyA->solverIndex; + constraint->indexB[j] = bodyB->solverIndex; - b2Vec2 vA = stateA->linearVelocity; - float wA = stateA->angularVelocity; + b2Vec2 vA = bodyA->linearVelocity; + float wA = bodyA->angularVelocity; - b2Vec2 vB = stateB->linearVelocity; - float wB = stateB->angularVelocity; + b2Vec2 vB = bodyB->linearVelocity; + float wB = bodyB->angularVelocity; - float mA = paramA->invMass; - float iA = paramA->invI; - float mB = paramB->invMass; - float iB = paramB->invI; + float mA = bodyA->invMass; + float iA = bodyA->invI; + float mB = bodyB->invMass; + float iB = bodyB->invI; ((float*)&constraint->invMassA)[j] = mA; ((float*)&constraint->invMassB)[j] = mB; diff --git a/src/distance_joint.c b/src/distance_joint.c index bde253e1..4edecd89 100644 --- a/src/distance_joint.c +++ b/src/distance_joint.c @@ -55,8 +55,8 @@ void b2PrepareDistanceJoint(b2Joint* base, b2StepContext* context) b2DistanceJoint* joint = &base->distanceJoint; - joint->indexA = context->bodyToSolverMap[indexA]; - joint->indexB = context->bodyToSolverMap[indexB]; + joint->indexA = bodyA->solverIndex; + joint->indexB = bodyB->solverIndex; // initial anchors in world space joint->anchorA = b2RotateVector(bodyA->rotation, b2Sub(base->localOriginAnchorA, bodyA->localCenter)); diff --git a/src/motor_joint.c b/src/motor_joint.c index 76a2bc09..b0c5a1a3 100644 --- a/src/motor_joint.c +++ b/src/motor_joint.c @@ -50,8 +50,8 @@ void b2PrepareMotorJoint(b2Joint* base, b2StepContext* context) base->invIB = iB; b2MotorJoint* joint = &base->motorJoint; - joint->indexA = context->bodyToSolverMap[indexA]; - joint->indexB = context->bodyToSolverMap[indexB]; + joint->indexA = bodyA->solverIndex; + joint->indexB = bodyB->solverIndex; joint->anchorA = b2RotateVector(bodyA->rotation, b2Sub(base->localOriginAnchorA, bodyA->localCenter)); joint->anchorB = b2RotateVector(bodyB->rotation, b2Sub(base->localOriginAnchorB, bodyB->localCenter)); diff --git a/src/mouse_joint.c b/src/mouse_joint.c index 4d701054..158ae9f9 100644 --- a/src/mouse_joint.c +++ b/src/mouse_joint.c @@ -55,7 +55,7 @@ void b2PrepareMouseJoint(b2Joint* base, b2StepContext* context) base->invIB = bodyB->invI; b2MouseJoint* joint = &base->mouseJoint; - joint->indexB = context->bodyToSolverMap[indexB]; + joint->indexB = bodyB->solverIndex; joint->anchorB = b2RotateVector(bodyB->rotation, b2Sub(base->localOriginAnchorB, bodyB->localCenter)); joint->linearSoftness = b2MakeSoft(joint->hertz, joint->dampingRatio, context->h); diff --git a/src/prismatic_joint.c b/src/prismatic_joint.c index 461bb4aa..3ea83c77 100644 --- a/src/prismatic_joint.c +++ b/src/prismatic_joint.c @@ -169,8 +169,8 @@ void b2PreparePrismaticJoint(b2Joint* base, b2StepContext* context) b2PrismaticJoint* joint = &base->prismaticJoint; - joint->indexA = context->bodyToSolverMap[indexA]; - joint->indexB = context->bodyToSolverMap[indexB]; + joint->indexA = bodyA->solverIndex; + joint->indexB = bodyB->solverIndex; joint->anchorA = b2RotateVector(bodyA->rotation, b2Sub(base->localOriginAnchorA, bodyA->localCenter)); joint->anchorB = b2RotateVector(bodyB->rotation, b2Sub(base->localOriginAnchorB, bodyB->localCenter)); diff --git a/src/revolute_joint.c b/src/revolute_joint.c index 75b02d00..f44be482 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -145,8 +145,8 @@ void b2PrepareRevoluteJoint(b2Joint* base, b2StepContext* context) b2RevoluteJoint* joint = &base->revoluteJoint; - joint->indexA = context->bodyToSolverMap[indexA]; - joint->indexB = context->bodyToSolverMap[indexB]; + joint->indexA = bodyA->solverIndex; + joint->indexB = bodyB->solverIndex; // initial anchors in world space joint->anchorA = b2RotateVector(bodyA->rotation, b2Sub(base->localOriginAnchorA, bodyA->localCenter)); diff --git a/src/solver.c b/src/solver.c index edeed7ce..06c1a289 100644 --- a/src/solver.c +++ b/src/solver.c @@ -5,7 +5,6 @@ #include "aabb.h" #include "allocate.h" -#include "stack_allocator.h" #include "array.h" #include "bitset.inl" #include "body.h" @@ -15,8 +14,11 @@ #include "joint.h" #include "shape.h" #include "solver.h" +#include "stack_allocator.h" #include "world.h" +#include "box2d/event_types.h" + // for mm_pause #include "x86/sse2.h" @@ -181,6 +183,9 @@ static void b2FinalizeBodiesTask(int32_t startIndex, int32_t endIndex, uint32_t const b2Vec2 aabbMargin = {b2_aabbMargin, b2_aabbMargin}; float timeStep = context->dt; + uint16_t worldIndex = world->poolIndex; + b2BodyMoveEvent* moveEvents = world->bodyMoveEventArray; + b2BitSet* awakeContactBitSet = &world->taskContextArray[threadIndex].awakeContactBitSet; b2BitSet* shapeBitSet = &world->taskContextArray[threadIndex].shapeBitSet; b2BitSet* awakeIslandBitSet = &world->taskContextArray[threadIndex].awakeIslandBitSet; @@ -216,6 +221,11 @@ static void b2FinalizeBodiesTask(int32_t startIndex, int32_t endIndex, uint32_t body->rotation = b2MulRot(state->deltaRotation, body->rotation); body->origin = b2Sub(body->position, b2RotateVector(body->rotation, body->localCenter)); + moveEvents[i].transform = b2MakeTransform(body); + moveEvents[i].bodyId = (b2BodyId){bodyIndex + 1, worldIndex, body->object.revision}; + moveEvents[i].userData = body->userData; + moveEvents[i].fellAsleep = false; + // reset applied force and torque body->force = b2Vec2_zero; body->torque = 0.0f; @@ -740,24 +750,16 @@ static bool b2SolveConstraintGraph(b2World* world, b2StepContext* context) return false; } - // Reserve space for awake bodies + // Reserve space for solver body arrays b2Body* bodies = world->bodies; - b2Body** awakeBodies = b2AllocateStackItem(world->stackAllocator, awakeBodyCount * sizeof(b2Body*), "awake bodies"); - b2BodyState* bodyStates = - b2AllocateStackItem(world->stackAllocator, awakeBodyCount * sizeof(b2BodyState), "body states"); - b2BodyParam* bodyParams = - b2AllocateStackItem(world->stackAllocator, awakeBodyCount * sizeof(b2BodyParam), "body params"); + b2BodyState* bodyStates = b2AllocateStackItem(world->stackAllocator, awakeBodyCount * sizeof(b2BodyState), "body states"); + b2BodyParam* bodyParams = b2AllocateStackItem(world->stackAllocator, awakeBodyCount * sizeof(b2BodyParam), "body params"); + + b2Array_Resize(&world->bodyMoveEventArray, sizeof(b2BodyMoveEvent), awakeBodyCount); // Map from solver body to body - // todo have body directly reference solver body for user access? int32_t* solverToBodyMap = b2AllocateStackItem(world->stackAllocator, awakeBodyCount * sizeof(int32_t), "solver body map"); - // Map from world body to solver body - // todo eliminate this? - int32_t bodyCapacity = world->bodyPool.capacity; - int32_t* bodyToSolverMap = b2AllocateStackItem(world->stackAllocator, bodyCapacity * sizeof(int32_t), "body map"); - memset(bodyToSolverMap, 0xFF, bodyCapacity * sizeof(int32_t)); - // Build array of awake bodies and also search for an awake island to split b2Vec2 gravity = world->gravity; float h = context->h; @@ -784,14 +786,9 @@ static bool b2SolveConstraintGraph(b2World* world, b2StepContext* context) B2_ASSERT(b2IsValidObject(&body->object)); B2_ASSERT(body->object.index == bodyIndex); - awakeBodies[index] = body; - - B2_ASSERT(0 <= bodyIndex && bodyIndex < bodyCapacity); - bodyToSolverMap[bodyIndex] = index; + body->solverIndex = index; solverToBodyMap[index] = bodyIndex; - // todo cache misses - b2BodyState* state = bodyStates + index; state->linearVelocity = body->linearVelocity; state->angularVelocity = body->angularVelocity; @@ -813,7 +810,8 @@ static bool b2SolveConstraintGraph(b2World* world, b2StepContext* context) param->angularDamping = 1.0f / (1.0f + h * body->angularDamping); param->linearDamping = 1.0f / (1.0f + h * body->linearDamping); - param->linearVelocityDelta = b2MulSV(body->invMass * h, b2MulAdd(body->force, body->mass * body->gravityScale, gravity)); + param->linearVelocityDelta = + b2MulSV(body->invMass * h, b2MulAdd(body->force, body->mass * body->gravityScale, gravity)); param->angularVelocityDelta = h * body->invI * body->torque; bodyIndex = body->islandNext; @@ -1233,8 +1231,6 @@ static bool b2SolveConstraintGraph(b2World* world, b2StepContext* context) context->world = world; context->graph = graph; context->solverToBodyMap = solverToBodyMap; - context->bodyToSolverMap = bodyToSolverMap; - context->awakeBodies = awakeBodies; context->bodyStates = bodyStates; context->bodyParams = bodyParams; context->contactConstraints = contactConstraints; @@ -1305,11 +1301,9 @@ static bool b2SolveConstraintGraph(b2World* world, b2StepContext* context) b2FreeStackItem(world->stackAllocator, jointIndices); b2FreeStackItem(world->stackAllocator, contactIndices); b2FreeStackItem(world->stackAllocator, contactConstraints); - b2FreeStackItem(world->stackAllocator, bodyToSolverMap); b2FreeStackItem(world->stackAllocator, solverToBodyMap); b2FreeStackItem(world->stackAllocator, bodyParams); b2FreeStackItem(world->stackAllocator, bodyStates); - b2FreeStackItem(world->stackAllocator, awakeBodies); b2TracyCZoneNC(awake_islands, "Awake Islands", b2_colorGainsboro, true); @@ -1341,7 +1335,7 @@ static bool b2SolveConstraintGraph(b2World* world, b2StepContext* context) continue; } - // Put island to sleep + // There are no bodies keeping the island awake, so put the island to sleep b2Island* island = islands + islandIndex; island->awakeIndex = B2_NULL_INDEX; @@ -1366,6 +1360,9 @@ static bool b2SolveConstraintGraph(b2World* world, b2StepContext* context) contactKey = contact->edges[edgeIndex].nextKey; } + // Report to the user that this body fell asleep + world->bodyMoveEventArray[body->solverIndex].fellAsleep = true; + bodyIndex = body->islandNext; } @@ -1375,7 +1372,6 @@ static bool b2SolveConstraintGraph(b2World* world, b2StepContext* context) { b2Contact* contact = contacts + contactIndex; b2RemoveContactFromGraph(world, contact); - contactIndex = contact->islandNext; } @@ -1734,7 +1730,7 @@ void b2Solve(b2World* world, b2StepContext* context) b2TracyCZoneNC(enlarge_proxies, "Enlarge Proxies", b2_colorDarkTurquoise, true); // Enlarge broad-phase proxies and build move array - // todo this is a hack to deal with stale shapeBitSet when no bodies are awake because they were all destroyed + // #todo this is a hack to deal with stale shapeBitSet when no bodies are awake because they were all destroyed if (anyAwake) { b2BroadPhase* broadPhase = &world->broadPhase; @@ -1790,7 +1786,8 @@ void b2Solve(b2World* world, b2StepContext* context) // Parallel continuous collision int32_t minRange = 8; - void* userContinuousTask = world->enqueueTaskFcn(&b2ContinuousParallelForTask, world->fastBodyCount, minRange, world, world->userTaskContext); + void* userContinuousTask = + world->enqueueTaskFcn(&b2ContinuousParallelForTask, world->fastBodyCount, minRange, world, world->userTaskContext); world->taskCount += 1; if (userContinuousTask != NULL) { diff --git a/src/solver.h b/src/solver.h index 975af96e..5c4d9630 100644 --- a/src/solver.h +++ b/src/solver.h @@ -92,9 +92,6 @@ typedef struct b2StepContext struct b2Body* bodies; int32_t bodyCapacity; - // Map from world body pool index to body state and param - const int32_t* bodyToSolverMap; - // Map from body state to world body const int32_t* solverToBodyMap; @@ -104,7 +101,6 @@ typedef struct b2StepContext struct b2World* world; struct b2ConstraintGraph* graph; - struct b2Body** awakeBodies; int32_t* jointIndices; int32_t* contactIndices; diff --git a/src/weld_joint.c b/src/weld_joint.c index 645ab8df..7473f941 100644 --- a/src/weld_joint.c +++ b/src/weld_joint.c @@ -98,8 +98,8 @@ void b2PrepareWeldJoint(b2Joint* base, b2StepContext* context) base->invIB = iB; b2WeldJoint* joint = &base->weldJoint; - joint->indexA = context->bodyToSolverMap[indexA]; - joint->indexB = context->bodyToSolverMap[indexB]; + joint->indexA = bodyA->solverIndex; + joint->indexB = bodyB->solverIndex; joint->anchorA = b2RotateVector(bodyA->rotation, b2Sub(base->localOriginAnchorA, bodyA->localCenter)); joint->anchorB = b2RotateVector(bodyB->rotation, b2Sub(base->localOriginAnchorB, bodyB->localCenter)); diff --git a/src/wheel_joint.c b/src/wheel_joint.c index 9fff7fb3..591d9679 100644 --- a/src/wheel_joint.c +++ b/src/wheel_joint.c @@ -174,8 +174,8 @@ void b2PrepareWheelJoint(b2Joint* base, b2StepContext* context) b2WheelJoint* joint = &base->wheelJoint; - joint->indexA = context->bodyToSolverMap[indexA]; - joint->indexB = context->bodyToSolverMap[indexB]; + joint->indexA = bodyA->solverIndex; + joint->indexB = bodyB->solverIndex; joint->anchorA = b2RotateVector(bodyA->rotation, b2Sub(base->localOriginAnchorA, bodyA->localCenter)); joint->anchorB = b2RotateVector(bodyB->rotation, b2Sub(base->localOriginAnchorB, bodyB->localCenter)); diff --git a/src/world.c b/src/world.c index 20cd0575..d6db222c 100644 --- a/src/world.c +++ b/src/world.c @@ -138,6 +138,8 @@ b2WorldId b2CreateWorld(const b2WorldDef* def) world->awakeContactArray = b2CreateArray(sizeof(int32_t), B2_MAX(def->contactCapacity, 1)); world->contactAwakeIndexArray = b2CreateArray(sizeof(int32_t), world->contactPool.capacity); + world->bodyMoveEventArray = b2CreateArray(sizeof(b2BodyMoveEvent), 4); + world->sensorBeginEventArray = b2CreateArray(sizeof(b2SensorBeginTouchEvent), 4); world->sensorEndEventArray = b2CreateArray(sizeof(b2SensorEndTouchEvent), 4); @@ -206,6 +208,7 @@ void b2DestroyWorld(b2WorldId id) b2DestroyArray(world->awakeIslandArray, sizeof(int32_t)); b2DestroyArray(world->contactAwakeIndexArray, sizeof(int32_t)); + b2DestroyArray(world->bodyMoveEventArray, sizeof(b2BodyMoveEvent)); b2DestroyArray(world->sensorBeginEventArray, sizeof(b2SensorBeginTouchEvent)); b2DestroyArray(world->sensorEndEventArray, sizeof(b2SensorEndTouchEvent)); @@ -497,6 +500,7 @@ void b2World_Step(b2WorldId worldId, float timeStep, int32_t subStepCount) // Prepare to capture events // Ensure user does not access stale data if there is an early return + b2Array_Clear(world->bodyMoveEventArray); b2Array_Clear(world->sensorBeginEventArray); b2Array_Clear(world->sensorEndEventArray); b2Array_Clear(world->contactBeginArray); @@ -898,6 +902,20 @@ void b2World_Draw(b2WorldId worldId, b2DebugDraw* draw) } } +b2BodyEvents b2World_GetBodyEvents(b2WorldId worldId) +{ + b2World* world = b2GetWorldFromId(worldId); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return (b2BodyEvents){0}; + } + + int count = b2Array(world->bodyMoveEventArray).count; + b2BodyEvents events = {world->bodyMoveEventArray, count}; + return events; +} + b2SensorEvents b2World_GetSensorEvents(b2WorldId worldId) { b2World* world = b2GetWorldFromId(worldId); diff --git a/src/world.h b/src/world.h index d4013ac9..552eac91 100644 --- a/src/world.h +++ b/src/world.h @@ -76,6 +76,7 @@ typedef struct b2World // TODO_ERIN use a bit array somehow? int32_t* contactAwakeIndexArray; + struct b2BodyMoveEvent* bodyMoveEventArray; struct b2SensorBeginTouchEvent* sensorBeginEventArray; struct b2SensorEndTouchEvent* sensorEndEventArray; struct b2ContactBeginTouchEvent* contactBeginArray;