From 468d2c473be701335676e02c1ef0ac42c0e181d6 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Wed, 13 Dec 2023 22:50:21 -0800 Subject: [PATCH 01/11] weeble sample --- include/box2d/box2d.h | 2 +- include/box2d/types.h | 2 +- samples/collection/benchmark.cpp | 10 +++- samples/collection/sample_bodies.cpp | 83 ++++++++++++++++++++++------ src/body.c | 18 +++++- src/world.c | 5 +- 6 files changed, 96 insertions(+), 24 deletions(-) diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index 9046a169..034eccb2 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -78,7 +78,7 @@ BOX2D_API b2Vec2 b2Body_GetWorldCenterOfMass(b2BodyId bodyId); /// Override the body's mass properties. Normally this is computed automatically using the /// shape geometry and density. This information is lost if a shape is added or removed or if the /// body type changes. -BOX2D_API void b2Body_SetMassData(b2MassData massData); +BOX2D_API void b2Body_SetMassData(b2BodyId bodyId, b2MassData massData); /// Is this body awake? BOX2D_API void b2Body_IsAwake(b2BodyId bodyId); diff --git a/include/box2d/types.h b/include/box2d/types.h index 3fcf82a8..584813bf 100644 --- a/include/box2d/types.h +++ b/include/box2d/types.h @@ -286,7 +286,7 @@ typedef struct b2ShapeDef } b2ShapeDef; static const b2ShapeDef b2_defaultShapeDef = { - NULL, 0.6f, 0.0f, 0.0f, {0x00000001, 0xFFFFFFFF, 0}, false, + NULL, 0.6f, 0.0f, 1.0f, {0x00000001, 0xFFFFFFFF, 0}, false, }; /// Used to create a chain of edges. This is designed to eliminate ghost collisions with some limitations. diff --git a/samples/collection/benchmark.cpp b/samples/collection/benchmark.cpp index 66b5a4b2..aa01132d 100644 --- a/samples/collection/benchmark.cpp +++ b/samples/collection/benchmark.cpp @@ -46,7 +46,13 @@ class BenchmarkBarrel : public Sample box = b2MakeOffsetBox(1.2f, 2.0f * groundSize, {groundSize, 2.0f * groundSize}, 0.0f); b2Body_CreatePolygon(groundId, &sd, &box); - b2Segment segment = {{-200.0f, -40.0f}, {200.0f, -40.0f}}; + b2Segment segment = {{-400.0f, -60.0f}, {400.0f, -60.0f}}; + b2Body_CreateSegment(groundId, &sd, &segment); + + segment = {{-800.0f, 100.0f}, {-400.0f, -60.0f}}; + b2Body_CreateSegment(groundId, &sd, &segment); + + segment = {{400.0f, -60.0f}, {800.0f, 100.0f}}; b2Body_CreateSegment(groundId, &sd, &segment); } @@ -55,7 +61,7 @@ class BenchmarkBarrel : public Sample m_bodies[i] = b2_nullBodyId; } - m_shapeType = e_boxShape; + m_shapeType = e_circleShape; CreateScene(); } diff --git a/samples/collection/sample_bodies.cpp b/samples/collection/sample_bodies.cpp index f681a236..256c0821 100644 --- a/samples/collection/sample_bodies.cpp +++ b/samples/collection/sample_bodies.cpp @@ -17,37 +17,37 @@ class BodyType : public Sample { b2BodyId groundId = b2_nullBodyId; { - b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyDef bodyDef = b2_defaultBodyDef; groundId = b2World_CreateBody(m_worldId, &bodyDef); b2Segment segment = {{-20.0f, 0.0f}, {20.0f, 0.0f}}; - b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2ShapeDef shapeDef = b2_defaultShapeDef; b2Body_CreateSegment(groundId, &shapeDef, &segment); } // Define attachment { - b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyDef bodyDef = b2_defaultBodyDef; bodyDef.type = b2_dynamicBody; bodyDef.position = {0.0f, 3.0f}; m_attachmentId = b2World_CreateBody(m_worldId, &bodyDef); b2Polygon box = b2MakeBox(0.5f, 2.0f); - b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2ShapeDef shapeDef = b2_defaultShapeDef; shapeDef.density = 1.0f; b2Body_CreatePolygon(m_attachmentId, &shapeDef, &box); } // Define platform { - b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyDef bodyDef = b2_defaultBodyDef; bodyDef.type = b2_dynamicBody; bodyDef.position = {-4.0f, 5.0f}; m_platformId = b2World_CreateBody(m_worldId, &bodyDef); b2Polygon box = b2MakeOffsetBox(0.5f, 4.0f, {4.0f, 0.0f}, 0.5f * b2_pi); - b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2ShapeDef shapeDef = b2_defaultShapeDef; shapeDef.friction = 0.6f; shapeDef.density = 2.0f; b2Body_CreatePolygon(m_platformId, &shapeDef, &box); @@ -83,14 +83,14 @@ class BodyType : public Sample // Create a payload { - b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyDef bodyDef = b2_defaultBodyDef; bodyDef.type = b2_dynamicBody; bodyDef.position = {0.0f, 8.0f}; b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); b2Polygon box = b2MakeBox(0.75f, 0.75f); - b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2ShapeDef shapeDef = b2_defaultShapeDef; shapeDef.friction = 0.6f; shapeDef.density = 2.0f; @@ -340,13 +340,62 @@ class Character : public Sample static int sampleCharacter = RegisterSample("Bodies", "Character", Character::Create); +class Weeble : public Sample +{ +public: + Weeble(const Settings& settings) + : Sample(settings) + { + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bodyDef = b2_defaultBodyDef; + groundId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Segment segment = {{-20.0f, 0.0f}, {20.0f, 0.0f}}; + b2ShapeDef shapeDef = b2_defaultShapeDef; + b2Body_CreateSegment(groundId, &shapeDef, &segment); + } + + // Build weeble + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = {0.0f, 3.0f}; + m_weebleId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Capsule capsule = {{0.0f, -1.0f}, {0.0f, 1.0f}, 1.0f}; + b2ShapeDef shapeDef = b2_defaultShapeDef; + shapeDef.density = 1.0f; + b2Body_CreateCapsule(m_weebleId, &shapeDef, &capsule); + + float mass = b2Body_GetMass(m_weebleId); + float I = b2Body_GetInertiaTensor(m_weebleId); + + b2MassData massData = {mass, {0.0f, -1.5f}, I, 2.0f, 4.0f}; + b2Body_SetMassData(m_weebleId, massData); + } + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 400.0f)); + ImGui::SetNextWindowSize(ImVec2(200.0f, 60.0f)); + ImGui::Begin("Sample Controls", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize); + + if (ImGui::Button("Teleport")) + { + b2Body_SetTransform(m_weebleId, {0.0f, 5.0f}, 0.95 * b2_pi); + } + + ImGui::End(); + } + + static Sample* Create(const Settings& settings) + { + return new Weeble(settings); + } + + b2BodyId m_weebleId; +}; -// Test all these APIs: -#if 0 -void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle); -float b2Body_GetMass(b2BodyId bodyId); -float b2Body_GetInertiaTensor(b2BodyId bodyId); -float b2Body_GetCenterOfMass(b2BodyId bodyId); -void b2Body_SetMassData(b2MassData massData); -void b2Body_Wake(b2BodyId bodyId); -#endif \ No newline at end of file +static int sampleWeeble = RegisterSample("Bodies", "Weeble", Weeble::Create); diff --git a/src/body.c b/src/body.c index 4e0ec836..cdeb5571 100644 --- a/src/body.c +++ b/src/body.c @@ -875,7 +875,7 @@ b2Vec2 b2Body_GetLocalCenterOfMass(b2BodyId bodyId) b2Body* body = b2GetBody(world, bodyId); return body->localCenter; } - + b2Vec2 b2Body_GetWorldCenterOfMass(b2BodyId bodyId) { b2World* world = b2GetWorldFromIndex(bodyId.world); @@ -883,6 +883,22 @@ b2Vec2 b2Body_GetWorldCenterOfMass(b2BodyId bodyId) return body->position; } +void b2Body_SetMassData(b2BodyId bodyId, b2MassData massData) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + body->mass = massData.mass; + body->I = massData.I; + body->localCenter = massData.center; + + b2Vec2 p = b2TransformPoint(body->transform, massData.center); + body->position = p; + body->position0 = p; + + body->invMass = body->mass > 0.0f ? 1.0f / body->mass : 0.0f; + body->invI = body->I > 0.0f ? 1.0f / body->I : 0.0f; +} + void b2Body_Wake(b2BodyId bodyId) { b2World* world = b2GetWorldFromIndex(bodyId.world); diff --git a/src/world.c b/src/world.c index 99c7ee42..a90ca654 100644 --- a/src/world.c +++ b/src/world.c @@ -761,9 +761,10 @@ void b2World_Draw(b2WorldId worldId, b2DebugDraw* draw) continue; } - draw->DrawTransform(body->transform, draw->context); + b2Transform transform = {body->position, body->transform.q}; + draw->DrawTransform(transform, draw->context); - b2Vec2 p = b2TransformPoint(body->transform, offset); + b2Vec2 p = b2TransformPoint(transform, offset); char buffer[32]; sprintf(buffer, "%.1f", body->mass); From 3708319599041879a88246462b6a945dad2e1663 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Thu, 14 Dec 2023 22:57:45 -0800 Subject: [PATCH 02/11] wip --- include/box2d/box2d.h | 14 +++++++++-- include/box2d/event_types.h | 36 ++++++++++++++++++++++++++++ include/box2d/id.h | 14 +++++++++-- include/box2d/manifold.h | 1 + samples/collection/sample_bodies.cpp | 7 +++++- src/body.c | 30 +++++++++++++++++++++++ src/shape.c | 13 ++++++---- src/shape.h | 2 ++ src/world.c | 31 ++++++++++++++++++++---- src/world.h | 2 ++ 10 files changed, 136 insertions(+), 14 deletions(-) diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index 034eccb2..9ade8f4c 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -103,17 +103,27 @@ BOX2D_API b2ShapeId b2Body_CreateCapsule(b2BodyId bodyId, const b2ShapeDef* def, BOX2D_API b2ShapeId b2Body_CreatePolygon(b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon); BOX2D_API void b2Body_DestroyShape(b2ShapeId shapeId); +BOX2D_API b2ChainId b2Body_CreateChain(b2BodyId bodyId, const b2ChainDef* def); +BOX2D_API void b2Body_DestroyChain(b2ChainId chainId); + +/// Iterate over shapes on a body +BOX2D_API b2ShapeId b2Body_GetFirstShape(b2BodyId bodyId); +BOX2D_API b2ShapeId b2Body_GetNextShape(b2ShapeId shapeId); + BOX2D_API b2BodyId b2Shape_GetBody(b2ShapeId shapeId); BOX2D_API void* b2Shape_GetUserData(b2ShapeId shapeId); BOX2D_API bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point); BOX2D_API void b2Shape_SetFriction(b2ShapeId shapeId, float friction); BOX2D_API void b2Shape_SetRestitution(b2ShapeId shapeId, float restitution); -BOX2D_API b2ChainId b2Body_CreateChain(b2BodyId bodyId, const b2ChainDef* def); -BOX2D_API void b2Body_DestroyChain(b2ChainId chainId); BOX2D_API void b2Chain_SetFriction(b2ChainId chainId, float friction); BOX2D_API void b2Chain_SetRestitution(b2ChainId chainId, float restitution); +/// Contacts +BOX2D_API b2ContactId b2Body_GetFirstContact(b2BodyId bodyId); +BOX2D_API b2ContactId b2Body_GetNextContact(b2BodyId bodyId, b2ContactId contactId); +BOX2D_API b2ContactData b2Contact_GetData(b2ContactId contactId); + /// Create a joint BOX2D_API b2JointId b2World_CreateDistanceJoint(b2WorldId worldId, const b2DistanceJointDef* def); BOX2D_API b2JointId b2World_CreateMouseJoint(b2WorldId worldId, const b2MouseJointDef* def); diff --git a/include/box2d/event_types.h b/include/box2d/event_types.h index 2a82ea9b..0173c1e7 100644 --- a/include/box2d/event_types.h +++ b/include/box2d/event_types.h @@ -4,6 +4,7 @@ #pragma once #include "box2d/id.h" +#include "box2d/manifold.h" #include #include @@ -25,6 +26,7 @@ typedef struct b2SensorEndTouchEvent /// Sensor events are buffered in the Box2D world and are available /// as begin/end overlap event arrays after the time step is complete. +/// Note: these may become invalid if bodies and/or shapes are destroyed typedef struct b2SensorEvents { b2SensorBeginTouchEvent* beginEvents; @@ -32,3 +34,37 @@ typedef struct b2SensorEvents int beginCount; int endCount; } b2SensorEvents; + +/// A begin touch event is generated when two shapes begin touching. +typedef struct b2ContactBeginTouchEvent +{ + b2ShapeId shapeIdA; + b2ShapeId shapeIdB; + b2Manifold manifold; +} b2ContactBeginTouchEvent; + +/// An end touch event is generated when two shapes stop touching. +typedef struct b2ContactEndTouchEvent +{ + b2ShapeId shapeIdA; + b2ShapeId shapeIdB; +} b2ContactEndTouchEvent; + +/// Contact events are buffered in the Box2D world and are available +/// as event arrays after the time step is complete. +/// Note: these may become invalid if bodies and/or shapes are destroyed +typedef struct b2ContactEvents +{ + b2ContactBeginTouchEvent* beginEvents; + b2ContactEndTouchEvent* endEvents; + int beginCount; + int endCount; +} b2ContactEvents; + +/// This is the data you can access using a b2ContactId +typedef struct b2ContactData +{ + b2ShapeId shapeIdA; + b2ShapeId shapeIdB; + b2Manifold manifold; +} b2ContactData; diff --git a/include/box2d/id.h b/include/box2d/id.h index 7d610e07..c482d154 100644 --- a/include/box2d/id.h +++ b/include/box2d/id.h @@ -31,6 +31,14 @@ typedef struct b2ShapeId uint16_t revision; } b2ShapeId; +/// References a contact instance +typedef struct b2ContactId +{ + int32_t index; + int16_t world; + uint16_t revision; +} b2ContactId; + /// References a joint instance typedef struct b2JointId { @@ -39,6 +47,7 @@ typedef struct b2JointId uint16_t revision; } b2JointId; +/// References a chain instances typedef struct b2ChainId { int32_t index; @@ -49,8 +58,9 @@ typedef struct b2ChainId static const b2WorldId b2_nullWorldId = {-1, 0}; static const b2BodyId b2_nullBodyId = {-1, -1, 0}; static const b2ShapeId b2_nullShapeId = {-1, -1, 0}; +static const b2JointId b2_nullContactId = {-1, -1, 0}; static const b2JointId b2_nullJointId = {-1, -1, 0}; static const b2ChainId b2_nullChainId = {-1, -1, 0}; -#define B2_IS_NULL(ID) (ID.index == -1) -#define B2_NON_NULL(ID) (ID.index != -1) +#define B2_IS_NULL(id) (id.index == -1) +#define B2_NON_NULL(id) (id.index != -1) diff --git a/include/box2d/manifold.h b/include/box2d/manifold.h index b3201e8d..fe58933c 100644 --- a/include/box2d/manifold.h +++ b/include/box2d/manifold.h @@ -14,6 +14,7 @@ typedef struct b2Polygon b2Polygon; typedef struct b2Segment b2Segment; typedef struct b2SmoothSegment b2SmoothSegment; +// todo internal #define B2_MAKE_ID(A, B) ((uint8_t)(A) << 8 | (uint8_t)(B)) /// A manifold point is a contact point belonging to a contact diff --git a/samples/collection/sample_bodies.cpp b/samples/collection/sample_bodies.cpp index 256c0821..fad64208 100644 --- a/samples/collection/sample_bodies.cpp +++ b/samples/collection/sample_bodies.cpp @@ -370,8 +370,13 @@ class Weeble : public Sample float mass = b2Body_GetMass(m_weebleId); float I = b2Body_GetInertiaTensor(m_weebleId); + + float offset = 1.5f; + + // See: https://en.wikipedia.org/wiki/Parallel_axis_theorem + I += mass * offset * offset; - b2MassData massData = {mass, {0.0f, -1.5f}, I, 2.0f, 4.0f}; + b2MassData massData = {mass, {0.0f, -offset}, I, 2.0f, 4.0f}; b2Body_SetMassData(m_weebleId, massData); } } diff --git a/src/body.c b/src/body.c index cdeb5571..e9b5a30f 100644 --- a/src/body.c +++ b/src/body.c @@ -943,6 +943,36 @@ void b2Body_Enable(b2BodyId bodyId) } } +b2ShapeId b2Body_GetFirstShape(b2BodyId bodyId) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + + if (body->shapeList == B2_NULL_INDEX) + { + return b2_nullShapeId; + } + + b2Shape* shape = world->shapes + body->shapeList; + b2ShapeId id = {shape->object.index, bodyId.world, shape->object.revision}; + return id; +} + +b2ShapeId b2Body_GetNextShape(b2ShapeId shapeId) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); + + if (shape->nextShapeIndex == B2_NULL_INDEX) + { + return b2_nullShapeId; + } + + shape = world->shapes + shape->nextShapeIndex; + b2ShapeId id = {shape->object.index, shapeId.world, shape->object.revision}; + return id; +} + bool b2ShouldBodiesCollide(b2World* world, b2Body* bodyA, b2Body* bodyB) { int32_t jointKey; diff --git a/src/shape.c b/src/shape.c index d73a476d..5f4784a8 100644 --- a/src/shape.c +++ b/src/shape.c @@ -189,12 +189,12 @@ b2DistanceProxy b2MakeShapeDistanceProxy(const b2Shape* shape) } } -static b2Shape* b2GetShape(b2ShapeId shapeId) +b2Shape* b2GetShape(b2World* world, b2ShapeId shapeId) { - b2World* world = b2GetWorldFromIndex(shapeId.world); B2_ASSERT(0 <= shapeId.index && shapeId.index < world->shapePool.capacity); b2Shape* shape = world->shapes + shapeId.index; B2_ASSERT(b2ObjectValid(&shape->object)); + B2_ASSERT(shape->object.revision == shapeId.revision); return shape; } @@ -215,7 +215,8 @@ b2BodyId b2Shape_GetBody(b2ShapeId shapeId) void* b2Shape_GetUserData(b2ShapeId shapeId) { - b2Shape* shape = b2GetShape(shapeId); + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); return shape->userData; } @@ -250,13 +251,15 @@ bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point) void b2Shape_SetFriction(b2ShapeId shapeId, float friction) { - b2Shape* shape = b2GetShape(shapeId); + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); shape->friction = friction; } void b2Shape_SetRestitution(b2ShapeId shapeId, float restitution) { - b2Shape* shape = b2GetShape(shapeId); + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); shape->restitution = restitution; } diff --git a/src/shape.h b/src/shape.h index 618139b9..40ffddca 100644 --- a/src/shape.h +++ b/src/shape.h @@ -75,3 +75,5 @@ b2DistanceProxy b2MakeShapeDistanceProxy(const b2Shape* shape); b2RayCastOutput b2RayCastShape(const b2RayCastInput* input, const b2Shape* shape, b2Transform xf); b2RayCastOutput b2ShapeCastShape(const b2ShapeCastInput* input, const b2Shape* shape, b2Transform xf); + +b2Shape* b2GetShape(b2World* world, b2ShapeId shapeId); diff --git a/src/world.c b/src/world.c index a90ca654..8e4ee2c7 100644 --- a/src/world.c +++ b/src/world.c @@ -124,6 +124,10 @@ b2WorldId b2CreateWorld(const b2WorldDef* def) world->sensorBeginEventArray = b2CreateArray(sizeof(b2SensorBeginTouchEvent), 4); world->sensorEndEventArray = b2CreateArray(sizeof(b2SensorEndTouchEvent), 4); + world->contactBeginArray = b2CreateArray(sizeof(b2ContactBeginTouchEvent), 4); + world->contactPersistArray = b2CreateArray(sizeof(b2ContactPersistTouchEvent), 4); + world->contactEndArray = b2CreateArray(sizeof(b2ContactEndTouchEvent), 4); + world->stepId = 0; world->activeTaskCount = 0; world->taskCount = 0; @@ -194,6 +198,10 @@ void b2DestroyWorld(b2WorldId id) b2DestroyArray(world->sensorBeginEventArray, sizeof(b2SensorBeginTouchEvent)); b2DestroyArray(world->sensorEndEventArray, sizeof(b2SensorEndTouchEvent)); + b2DestroyArray(world->contactBeginArray, sizeof(b2ContactBeginTouchEvent)); + b2DestroyArray(world->contactPersistArray, sizeof(b2ContactPersistTouchEvent)); + b2DestroyArray(world->contactEndArray, sizeof(b2ContactEndTouchEvent)); + b2DestroyPool(&world->islandPool); b2DestroyPool(&world->jointPool); b2DestroyPool(&world->contactPool); @@ -371,6 +379,9 @@ static void b2Collide(b2World* world) // Prepare to capture events b2Array_Clear(world->sensorBeginEventArray); b2Array_Clear(world->sensorEndEventArray); + b2Array_Clear(world->contactBeginArray); + b2Array_Clear(world->contactEndArray); + const b2Shape* shapes = world->shapes; int16_t worldIndex = world->index; @@ -389,9 +400,19 @@ static void b2Collide(b2World* world) B2_ASSERT(contactIndex != B2_NULL_INDEX); b2Contact* contact = world->contacts + contactIndex; + const b2Shape* shapeA = shapes + contact->shapeIndexA; + const b2Shape* shapeB = shapes + contact->shapeIndexB; + b2ShapeId shapeIdA = {shapeA->object.index, worldIndex, shapeA->object.revision}; + b2ShapeId shapeIdB = {shapeB->object.index, worldIndex, shapeB->object.revision}; if (contact->flags & b2_contactDisjoint) { + if (contact->flags & b2_contactTouchingFlag) + { + b2ContactEndTouchEvent event = {shapeIdA, shapeIdB}; + b2Array_Push(world->contactEndArray, event); + } + // Bounding boxes no longer overlap b2DestroyContact(world, contact); } @@ -400,10 +421,6 @@ static void b2Collide(b2World* world) B2_ASSERT(contact->islandIndex == B2_NULL_INDEX); if (contact->flags & b2_contactSensorFlag) { - const b2Shape* shapeA = shapes + contact->shapeIndexA; - const b2Shape* shapeB = shapes + contact->shapeIndexB; - b2ShapeId shapeIdA = {shapeA->object.index, worldIndex, shapeA->object.revision}; - b2ShapeId shapeIdB = {shapeB->object.index, worldIndex, shapeB->object.revision}; if (shapeA->isSensor) { b2SensorBeginTouchEvent event = {shapeIdA, shapeIdB}; @@ -418,6 +435,9 @@ static void b2Collide(b2World* world) } else { + b2ContactBeginTouchEvent event = {shapeIdA, shapeIdB, contact->manifold}; + b2Array_Push(world->contactBeginArray, event); + b2LinkContact(world, contact); b2AddContactToGraph(world, contact); } @@ -446,6 +466,9 @@ static void b2Collide(b2World* world) } else { + b2ContactEndTouchEvent event = {shapeIdA, shapeIdB}; + b2Array_Push(world->contactEndArray, event); + b2UnlinkContact(world, contact); b2RemoveContactFromGraph(world, contact); } diff --git a/src/world.h b/src/world.h index 7b3f6cae..4d898118 100644 --- a/src/world.h +++ b/src/world.h @@ -80,6 +80,8 @@ typedef struct b2World struct b2SensorBeginTouchEvent* sensorBeginEventArray; struct b2SensorEndTouchEvent* sensorEndEventArray; + struct b2ContactBeginTouchEvent* contactBeginArray; + struct b2ContactEndTouchEvent* contactEndArray; // Array of fast bodies that need continuous collision handling int32_t* fastBodies; From cfae4fccdcece5388c6c7731a9f64cf8539dbe3f Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Fri, 15 Dec 2023 22:31:46 -0800 Subject: [PATCH 03/11] wip --- include/box2d/box2d.h | 6 + include/box2d/debug_draw.h | 4 + include/box2d/id.h | 2 +- include/box2d/types.h | 28 +++- samples/collection/benchmark.cpp | 6 +- samples/collection/sample_continuous.cpp | 2 +- samples/collection/sample_events.cpp | 4 +- samples/collection/sample_joints.cpp | 12 +- samples/collection/sample_robustness.cpp | 2 +- samples/collection/sample_shapes.cpp | 2 +- samples/draw.cpp | 14 +- samples/main.cpp | 80 +++++------ samples/sample.cpp | 171 ++++------------------- samples/sample.h | 4 +- samples/settings.cpp | 34 ++--- samples/settings.h | 48 +++---- src/array.h | 3 + src/body.c | 6 +- src/body.h | 5 +- src/contact.c | 85 ++++++++++- src/contact.h | 10 +- src/distance_joint.c | 8 +- src/joint.c | 57 +++++++- src/joint.h | 2 +- src/revolute_joint.c | 4 +- src/shape.c | 5 +- src/shape.h | 6 +- src/world.c | 120 ++++++++++++++-- test/test_determinism.c | 2 +- 29 files changed, 445 insertions(+), 287 deletions(-) diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index 9ade8f4c..fb5fd738 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -42,6 +42,9 @@ BOX2D_API b2BodyId b2World_CreateBody(b2WorldId worldId, const b2BodyDef* def); /// @warning This function is locked during callbacks. BOX2D_API void b2World_DestroyBody(b2BodyId bodyId); +/// Destroy a rigid body and get an array of all the shapes it was touching (on other bodies). +BOX2D_API int32_t b2World_DestroyBodyWithResults(b2BodyId bodyId, b2ShapeId* touchingShapes, int32_t maxShapes); + BOX2D_API b2Vec2 b2Body_GetPosition(b2BodyId bodyId); BOX2D_API float b2Body_GetAngle(b2BodyId bodyId); BOX2D_API void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle); @@ -103,6 +106,9 @@ BOX2D_API b2ShapeId b2Body_CreateCapsule(b2BodyId bodyId, const b2ShapeDef* def, BOX2D_API b2ShapeId b2Body_CreatePolygon(b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon); BOX2D_API void b2Body_DestroyShape(b2ShapeId shapeId); +/// Destroy a shape from a body and get an array of all the other shapes this shape was touching. +BOX2D_API int32_t b2Body_DestroyShapeWithResults(b2ShapeId shapeId, b2ShapeId* touchingShapes, int32_t maxShapes); + BOX2D_API b2ChainId b2Body_CreateChain(b2BodyId bodyId, const b2ChainDef* def); BOX2D_API void b2Body_DestroyChain(b2ChainId chainId); diff --git a/include/box2d/debug_draw.h b/include/box2d/debug_draw.h index b3385567..8a28efa6 100644 --- a/include/box2d/debug_draw.h +++ b/include/box2d/debug_draw.h @@ -46,6 +46,10 @@ typedef struct b2DebugDraw bool drawJoints; bool drawAABBs; bool drawMass; + bool drawContacts; bool drawGraphColors; + bool drawContactNormals; + bool drawContactImpulses; + bool drawFrictionImpulses; void* context; } b2DebugDraw; diff --git a/include/box2d/id.h b/include/box2d/id.h index c482d154..c1f56793 100644 --- a/include/box2d/id.h +++ b/include/box2d/id.h @@ -58,7 +58,7 @@ typedef struct b2ChainId static const b2WorldId b2_nullWorldId = {-1, 0}; static const b2BodyId b2_nullBodyId = {-1, -1, 0}; static const b2ShapeId b2_nullShapeId = {-1, -1, 0}; -static const b2JointId b2_nullContactId = {-1, -1, 0}; +static const b2ContactId b2_nullContactId = {-1, -1, 0}; static const b2JointId b2_nullJointId = {-1, -1, 0}; static const b2ChainId b2_nullChainId = {-1, -1, 0}; diff --git a/include/box2d/types.h b/include/box2d/types.h index 584813bf..c4d12e3c 100644 --- a/include/box2d/types.h +++ b/include/box2d/types.h @@ -13,10 +13,15 @@ #ifdef __cplusplus #define B2_LITERAL(T) T -#define B2_ZERO_INIT {} +#define B2_ZERO_INIT \ + { \ + } #else #define B2_LITERAL(T) (T) -#define B2_ZERO_INIT {0} +#define B2_ZERO_INIT \ + { \ + 0 \ + } #endif #ifdef NDEBUG @@ -279,14 +284,23 @@ typedef struct b2ShapeDef /// Contact filtering data. b2Filter filter; - /// A sensor shape collects contact information but never generates a collision - /// response. + /// A sensor shape collects contact information but never generates a collision response. bool isSensor; + /// Enable sensor events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. + bool enableSensorEvents; + + /// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. + bool enableContactEvents; + + /// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive + /// and must be carefully handled due to multi-threading. Ignored for sensors. + bool enablePreSolveEvents; + } b2ShapeDef; static const b2ShapeDef b2_defaultShapeDef = { - NULL, 0.6f, 0.0f, 1.0f, {0x00000001, 0xFFFFFFFF, 0}, false, + NULL, 0.6f, 0.0f, 1.0f, {0x00000001, 0xFFFFFFFF, 0}, false, true, true, false, }; /// Used to create a chain of edges. This is designed to eliminate ghost collisions with some limitations. @@ -371,6 +385,10 @@ static inline struct b2ShapeDef b2DefaultShapeDef(void) def.density = 0.0f; def.filter = b2_defaultFilter; def.isSensor = false; + def.enableSensorEvents = true; + def.enableContactEvents = true; + def.enablePreSolveEvents = false; + return def; } diff --git a/samples/collection/benchmark.cpp b/samples/collection/benchmark.cpp index aa01132d..07f10394 100644 --- a/samples/collection/benchmark.cpp +++ b/samples/collection/benchmark.cpp @@ -239,7 +239,7 @@ class BenchmarkTumbler : public Sample void Step(Settings& settings) override { - if (settings.m_pause == false || settings.m_singleStep == true) + if (settings.pause == false || settings.singleStep == true) { float a = 0.125f; for (int32_t i = 0; i < 5 && m_count < m_maxCount; ++i) @@ -786,12 +786,12 @@ class BenchmarkCreateDestroy : public Sample void Step(Settings& settings) override { - float timeStep = settings.m_hertz > 0.0f ? 1.0f / settings.m_hertz : float(0.0f); + float timeStep = settings.hertz > 0.0f ? 1.0f / settings.hertz : float(0.0f); for (int32_t i = 0; i < m_iterations; ++i) { CreateScene(); - b2World_Step(m_worldId, timeStep, settings.m_velocityIterations, settings.m_relaxIterations); + b2World_Step(m_worldId, timeStep, settings.velocityIterations, settings.relaxIterations); } Sample::Step(settings); diff --git a/samples/collection/sample_continuous.cpp b/samples/collection/sample_continuous.cpp index df6e8b66..c693fbe5 100644 --- a/samples/collection/sample_continuous.cpp +++ b/samples/collection/sample_continuous.cpp @@ -27,7 +27,7 @@ class BounceHouse : public Sample BounceHouse(const Settings& settings) : Sample(settings) { - if (settings.m_restart == false) + if (settings.restart == false) { g_camera.m_center = {0.0f, 0.0f}; } diff --git a/samples/collection/sample_events.cpp b/samples/collection/sample_events.cpp index b6b15b43..a015e087 100644 --- a/samples/collection/sample_events.cpp +++ b/samples/collection/sample_events.cpp @@ -286,9 +286,9 @@ class Sensor : public Sample } } - if (settings.m_hertz > 0.0f && settings.m_pause == false) + if (settings.hertz > 0.0f && settings.pause == false) { - m_wait -= 1.0f / settings.m_hertz; + m_wait -= 1.0f / settings.hertz; if (m_wait < 0.0f) { CreateRing(); diff --git a/samples/collection/sample_joints.cpp b/samples/collection/sample_joints.cpp index 12756131..f0f3b405 100644 --- a/samples/collection/sample_joints.cpp +++ b/samples/collection/sample_joints.cpp @@ -135,11 +135,11 @@ class RevoluteJoint : public Sample { Sample::Step(settings); - float torque1 = b2RevoluteJoint_GetMotorTorque(m_joint1, settings.m_hertz); + float torque1 = b2RevoluteJoint_GetMotorTorque(m_joint1, settings.hertz); g_draw.DrawString(5, m_textLine, "Motor Torque 1= %4.0f", torque1); m_textLine += m_textIncrement; - float torque2 = b2RevoluteJoint_GetMotorTorque(m_joint2, settings.m_hertz); + float torque2 = b2RevoluteJoint_GetMotorTorque(m_joint2, settings.hertz); g_draw.DrawString(5, m_textLine, "Motor Torque 2= %4.0f", torque2); m_textLine += m_textIncrement; } @@ -469,7 +469,7 @@ class DistanceJoint : public Sample DistanceJoint(const Settings& settings) : Sample(settings) { - if (settings.m_restart == false) + if (settings.restart == false) { g_camera.m_zoom = 0.25f; } @@ -825,18 +825,18 @@ class UserConstraint : public Sample b2Transform axes = b2Transform_identity; g_draw.DrawTransform(axes); - if (settings.m_pause) + if (settings.pause) { return; } - float timeStep = settings.m_hertz > 0.0f ? 1.0f / settings.m_hertz : 0.0f; + float timeStep = settings.hertz > 0.0f ? 1.0f / settings.hertz : 0.0f; if (timeStep == 0.0f) { return; } - float invTimeStep = settings.m_hertz; + float invTimeStep = settings.hertz; static float hertz = 3.0f; static float zeta = 0.7f; diff --git a/samples/collection/sample_robustness.cpp b/samples/collection/sample_robustness.cpp index 57a85c16..ac4e7c16 100644 --- a/samples/collection/sample_robustness.cpp +++ b/samples/collection/sample_robustness.cpp @@ -204,7 +204,7 @@ class OverlapRecovery : public Sample OverlapRecovery(const Settings& settings) : Sample(settings) { - if (settings.m_restart == false) + if (settings.restart == false) { g_camera.m_zoom = 0.25f; g_camera.m_center = {0.0f, 5.0f}; diff --git a/samples/collection/sample_shapes.cpp b/samples/collection/sample_shapes.cpp index 6fc684d0..423cabab 100644 --- a/samples/collection/sample_shapes.cpp +++ b/samples/collection/sample_shapes.cpp @@ -41,7 +41,7 @@ class ChainShape : public Sample ChainShape(const Settings& settings) : Sample(settings) { - if (settings.m_restart == false) + if (settings.restart == false) { g_camera.m_center = {0.0f, 0.0f}; g_camera.m_zoom = 1.75f; diff --git a/samples/draw.cpp b/samples/draw.cpp index 2ac3bb1e..54a30960 100644 --- a/samples/draw.cpp +++ b/samples/draw.cpp @@ -905,11 +905,15 @@ void Draw::Create() DrawTransformFcn, DrawPointFcn, DrawStringFcn, - true, - true, - false, - false, - false, + true, // shapes + true, // joints + false, // aabbs + false, // mass + false, // contacts + false, // colors + false, // normals + false, // impulse + false, // friction this}; } diff --git a/samples/main.cpp b/samples/main.cpp index d4716fd2..bc49c7c1 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -108,9 +108,9 @@ static void SortTests() static void RestartSample() { delete s_sample; - s_settings.m_restart = true; - s_sample = g_sampleEntries[s_settings.m_sampleIndex].createFcn(s_settings); - s_settings.m_restart = false; + s_settings.restart = true; + s_sample = g_sampleEntries[s_settings.sampleIndex].createFcn(s_settings); + s_settings.restart = false; } static void CreateUI(GLFWwindow* window, const char* glslVersion) @@ -170,8 +170,8 @@ static void ResizeWindowCallback(GLFWwindow*, int width, int height) { g_camera.m_width = int(width / s_windowScale); g_camera.m_height = int(height / s_windowScale); - s_settings.m_windowWidth = int(width / s_windowScale); - s_settings.m_windowHeight = int(height / s_windowScale); + s_settings.windowWidth = int(width / s_windowScale); + s_settings.windowHeight = int(height / s_windowScale); } static void KeyCallback(GLFWwindow* window, int key, int scancode, int action, int mods) @@ -252,11 +252,11 @@ static void KeyCallback(GLFWwindow* window, int key, int scancode, int action, i break; case GLFW_KEY_O: - s_settings.m_singleStep = true; + s_settings.singleStep = true; break; case GLFW_KEY_P: - s_settings.m_pause = !s_settings.m_pause; + s_settings.pause = !s_settings.pause; break; case GLFW_KEY_LEFT_BRACKET: @@ -395,46 +395,46 @@ static void UpdateUI() { if (ImGui::BeginTabItem("Controls")) { - ImGui::SliderInt("Vel Iters", &s_settings.m_velocityIterations, 0, 50); - ImGui::SliderInt("Relax Iters", &s_settings.m_relaxIterations, 0, 50); - ImGui::SliderFloat("Hertz", &s_settings.m_hertz, 5.0f, 120.0f, "%.0f hz"); + ImGui::SliderInt("Vel Iters", &s_settings.velocityIterations, 0, 50); + ImGui::SliderInt("Relax Iters", &s_settings.relaxIterations, 0, 50); + ImGui::SliderFloat("Hertz", &s_settings.hertz, 5.0f, 120.0f, "%.0f hz"); - if (ImGui::SliderInt("Workers", &s_settings.m_workerCount, 1, maxWorkers)) + if (ImGui::SliderInt("Workers", &s_settings.workerCount, 1, maxWorkers)) { - s_settings.m_workerCount = B2_CLAMP(s_settings.m_workerCount, 1, maxWorkers); + s_settings.workerCount = B2_CLAMP(s_settings.workerCount, 1, maxWorkers); RestartSample(); } ImGui::Separator(); - ImGui::Checkbox("Sleep", &s_settings.m_enableSleep); - ImGui::Checkbox("Warm Starting", &s_settings.m_enableWarmStarting); - ImGui::Checkbox("Continuous", &s_settings.m_enableContinuous); + ImGui::Checkbox("Sleep", &s_settings.enableSleep); + ImGui::Checkbox("Warm Starting", &s_settings.enableWarmStarting); + ImGui::Checkbox("Continuous", &s_settings.enableContinuous); ImGui::Checkbox("Parallel", &b2_parallel); ImGui::Separator(); - ImGui::Checkbox("Shapes", &s_settings.m_drawShapes); - ImGui::Checkbox("Joints", &s_settings.m_drawJoints); - ImGui::Checkbox("AABBs", &s_settings.m_drawAABBs); - ImGui::Checkbox("Contact Points", &s_settings.m_drawContactPoints); - ImGui::Checkbox("Contact Normals", &s_settings.m_drawContactNormals); - ImGui::Checkbox("Contact Impulses", &s_settings.m_drawContactImpulse); - ImGui::Checkbox("Friction Impulses", &s_settings.m_drawFrictionImpulse); - ImGui::Checkbox("Center of Masses", &s_settings.m_drawMass); - ImGui::Checkbox("Graph Colors", &s_settings.m_drawGraphColors); - ImGui::Checkbox("Statistics", &s_settings.m_drawStats); - ImGui::Checkbox("Profile", &s_settings.m_drawProfile); + ImGui::Checkbox("Shapes", &s_settings.drawShapes); + ImGui::Checkbox("Joints", &s_settings.drawJoints); + ImGui::Checkbox("AABBs", &s_settings.drawAABBs); + ImGui::Checkbox("Contact Points", &s_settings.drawContactPoints); + ImGui::Checkbox("Contact Normals", &s_settings.drawContactNormals); + ImGui::Checkbox("Contact Impulses", &s_settings.drawContactImpulses); + ImGui::Checkbox("Friction Impulses", &s_settings.drawFrictionImpulses); + ImGui::Checkbox("Center of Masses", &s_settings.drawMass); + ImGui::Checkbox("Graph Colors", &s_settings.drawGraphColors); + ImGui::Checkbox("Statistics", &s_settings.drawStats); + ImGui::Checkbox("Profile", &s_settings.drawProfile); ImVec2 button_sz = ImVec2(-1, 0); if (ImGui::Button("Pause (P)", button_sz)) { - s_settings.m_pause = !s_settings.m_pause; + s_settings.pause = !s_settings.pause; } if (ImGui::Button("Single Step (O)", button_sz)) { - s_settings.m_singleStep = !s_settings.m_singleStep; + s_settings.singleStep = !s_settings.singleStep; } if (ImGui::Button("Reset Profile", button_sz)) @@ -467,7 +467,7 @@ static void UpdateUI() int i = 0; while (i < g_sampleCount) { - bool categorySelected = strcmp(category, g_sampleEntries[s_settings.m_sampleIndex].category) == 0; + bool categorySelected = strcmp(category, g_sampleEntries[s_settings.sampleIndex].category) == 0; ImGuiTreeNodeFlags nodeSelectionFlags = categorySelected ? ImGuiTreeNodeFlags_Selected : 0; bool nodeOpen = ImGui::TreeNodeEx(category, nodeFlags | nodeSelectionFlags); @@ -476,7 +476,7 @@ static void UpdateUI() while (i < g_sampleCount && strcmp(category, g_sampleEntries[i].category) == 0) { ImGuiTreeNodeFlags selectionFlags = 0; - if (s_settings.m_sampleIndex == i) + if (s_settings.sampleIndex == i) { selectionFlags = ImGuiTreeNodeFlags_Selected; } @@ -531,13 +531,13 @@ int main(int, char**) char buffer[128]; s_settings.Load(); - s_settings.m_workerCount = B2_MIN(8, (int)enki::GetNumHardwareThreads() / 2); + s_settings.workerCount = B2_MIN(8, (int)enki::GetNumHardwareThreads() / 2); SortTests(); glfwSetErrorCallback(glfwErrorCallback); - g_camera.m_width = s_settings.m_windowWidth; - g_camera.m_height = s_settings.m_windowHeight; + g_camera.m_width = s_settings.windowWidth; + g_camera.m_height = s_settings.windowHeight; if (glfwInit() == 0) { @@ -618,9 +618,9 @@ int main(int, char**) g_draw.Create(); CreateUI(g_mainWindow, glslVersion); - s_settings.m_sampleIndex = B2_CLAMP(s_settings.m_sampleIndex, 0, g_sampleCount - 1); - s_selection = s_settings.m_sampleIndex; - s_sample = g_sampleEntries[s_settings.m_sampleIndex].createFcn(s_settings); + s_settings.sampleIndex = B2_CLAMP(s_settings.sampleIndex, 0, g_sampleCount - 1); + s_selection = s_settings.sampleIndex; + s_sample = g_sampleEntries[s_settings.sampleIndex].createFcn(s_settings); glClearColor(0.2f, 0.2f, 0.2f, 1.0f); @@ -678,7 +678,7 @@ int main(int, char**) if (g_draw.m_showUI) { - const SampleEntry& entry = g_sampleEntries[s_settings.m_sampleIndex]; + const SampleEntry& entry = g_sampleEntries[s_settings.sampleIndex]; snprintf(buffer, 128, "%s : %s", entry.category, entry.name); s_sample->DrawTitle(buffer); } @@ -710,12 +710,12 @@ int main(int, char**) FrameMark; - if (s_selection != s_settings.m_sampleIndex) + if (s_selection != s_settings.sampleIndex) { g_camera.ResetView(); - s_settings.m_sampleIndex = s_selection; + s_settings.sampleIndex = s_selection; delete s_sample; - s_sample = g_sampleEntries[s_settings.m_sampleIndex].createFcn(s_settings); + s_sample = g_sampleEntries[s_settings.sampleIndex].createFcn(s_settings); } glfwPollEvents(); diff --git a/samples/sample.cpp b/samples/sample.cpp index 90e1f6bd..27e7e6f4 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -16,17 +16,6 @@ #include #include -bool PreSolveFcn(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, int32_t color, void* context) -{ - Sample* sample = static_cast(context); - if (sample->m_collectContacts) - { - return sample->PreSolve(shapeIdA, shapeIdB, manifold, color); - } - - return true; -} - static void* EnqueueTask(b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext, void* userContext) { Sample* sample = static_cast(userContext); @@ -64,15 +53,15 @@ Sample::Sample(const Settings& settings) { b2Vec2 gravity = {0.0f, -10.0f}; - m_scheduler.Initialize(settings.m_workerCount); + m_scheduler.Initialize(settings.workerCount); m_taskCount = 0; b2WorldDef worldDef = b2DefaultWorldDef(); - worldDef.workerCount = settings.m_workerCount; + worldDef.workerCount = settings.workerCount; worldDef.enqueueTask = &EnqueueTask; worldDef.finishTask = &FinishTask; worldDef.userTaskContext = this; - worldDef.enableSleep = settings.m_enableSleep; + worldDef.enableSleep = settings.enableSleep; // These are not ideal, but useful for testing Box2D worldDef.bodyCapacity = 2; @@ -83,21 +72,10 @@ Sample::Sample(const Settings& settings) m_textLine = 30; m_textIncrement = 18; m_mouseJointId = b2_nullJointId; - m_pointCount = 0; - m_collectContacts = - settings.m_drawContactPoints || settings.m_drawContactNormals || settings.m_drawContactImpulse || settings.m_drawFrictionImpulse; - - // m_destructionListener.test = this; - // m_world->SetDestructionListener(&m_destructionListener); - // m_world->SetContactListener(this); - - // TODO_ERIN too expensive - b2World_SetPreSolveCallback(m_worldId, PreSolveFcn, this); m_stepCount = 0; - b2BodyDef bodyDef = b2DefaultBodyDef(); - m_groundBodyId = b2World_CreateBody(m_worldId, &bodyDef); + m_groundBodyId = b2_nullBodyId; m_maxProfile = b2_emptyProfile; m_totalProfile = b2_emptyProfile; @@ -168,6 +146,8 @@ void Sample::MouseDown(b2Vec2 p, int button, int mod) float frequencyHz = 5.0f; float dampingRatio = 0.7f; float mass = b2Body_GetMass(queryContext.bodyId); + + m_groundBodyId = b2World_CreateBody(m_worldId, &b2_defaultBodyDef); b2MouseJointDef jd = b2DefaultMouseJointDef(); jd.bodyIdA = m_groundBodyId; @@ -189,6 +169,9 @@ void Sample::MouseUp(b2Vec2 p, int button) { b2World_DestroyJoint(m_mouseJointId); m_mouseJointId = b2_nullJointId; + + b2World_DestroyBody(m_groundBodyId); + m_groundBodyId = b2_nullBodyId; } } @@ -211,13 +194,13 @@ void Sample::ResetProfile() void Sample::Step(Settings& settings) { - float timeStep = settings.m_hertz > 0.0f ? 1.0f / settings.m_hertz : 0.0f; + float timeStep = settings.hertz > 0.0f ? 1.0f / settings.hertz : 0.0f; - if (settings.m_pause) + if (settings.pause) { - if (settings.m_singleStep) + if (settings.singleStep) { - settings.m_singleStep = 0; + settings.singleStep = 0; } else { @@ -228,30 +211,26 @@ void Sample::Step(Settings& settings) m_textLine += m_textIncrement; } - g_draw.m_debugDraw.drawShapes = settings.m_drawShapes; - g_draw.m_debugDraw.drawJoints = settings.m_drawJoints; - g_draw.m_debugDraw.drawAABBs = settings.m_drawAABBs; - g_draw.m_debugDraw.drawMass = settings.m_drawMass; - g_draw.m_debugDraw.drawGraphColors = settings.m_drawGraphColors; - - m_collectContacts = - settings.m_drawContactPoints || settings.m_drawContactNormals || settings.m_drawContactImpulse || settings.m_drawFrictionImpulse; - - b2World_EnableSleeping(m_worldId, settings.m_enableSleep); - b2World_EnableWarmStarting(m_worldId, settings.m_enableWarmStarting); - b2World_EnableContinuous(m_worldId, settings.m_enableContinuous); + g_draw.m_debugDraw.drawShapes = settings.drawShapes; + g_draw.m_debugDraw.drawJoints = settings.drawJoints; + g_draw.m_debugDraw.drawAABBs = settings.drawAABBs; + g_draw.m_debugDraw.drawMass = settings.drawMass; + g_draw.m_debugDraw.drawContacts = settings.drawContactPoints; + g_draw.m_debugDraw.drawGraphColors = settings.drawGraphColors; + g_draw.m_debugDraw.drawContactNormals = settings.drawContactNormals; + g_draw.m_debugDraw.drawContactImpulses = settings.drawContactImpulses; + g_draw.m_debugDraw.drawFrictionImpulses = settings.drawFrictionImpulses; - if (timeStep > 0.0f) - { - m_pointCount = 0; - } + b2World_EnableSleeping(m_worldId, settings.enableSleep); + b2World_EnableWarmStarting(m_worldId, settings.enableWarmStarting); + b2World_EnableContinuous(m_worldId, settings.enableContinuous); for (int32_t i = 0; i < 1; ++i) { - b2World_Step(m_worldId, timeStep, settings.m_velocityIterations, settings.m_relaxIterations); + b2World_Step(m_worldId, timeStep, settings.velocityIterations, settings.relaxIterations); m_taskCount = 0; - } + b2World_Draw(m_worldId, &g_draw.m_debugDraw); if (timeStep > 0.0f) @@ -259,7 +238,7 @@ void Sample::Step(Settings& settings) ++m_stepCount; } - if (settings.m_drawStats) + if (settings.drawStats) { b2Statistics s = b2World_GetStatistics(m_worldId); @@ -318,7 +297,7 @@ void Sample::Step(Settings& settings) m_totalProfile.continuous += p.continuous; } - if (settings.m_drawProfile) + if (settings.drawProfile) { b2Profile p = b2World_GetProfile(m_worldId); @@ -360,68 +339,9 @@ void Sample::Step(Settings& settings) m_textLine += m_textIncrement; } - if (settings.m_drawContactPoints) + if (settings.drawContactPoints) { - const float k_impulseScale = 1.0f; - const float k_axisScale = 0.3f; - b2Color speculativeColor = {0.3f, 0.3f, 0.3f, 1.0f}; - b2Color addColor = {0.3f, 0.95f, 0.3f, 1.0f}; - b2Color persistColor = {0.3f, 0.3f, 0.95f, 1.0f}; - - b2HexColor colors[b2_graphColorCount + 1] = {b2_colorRed, b2_colorOrange, b2_colorYellow, b2_colorGreen, b2_colorCyan, - b2_colorBlue, b2_colorViolet, b2_colorPink, b2_colorChocolate, b2_colorGoldenrod, - b2_colorCoral, b2_colorAqua, b2_colorBlack}; - for (int32_t i = 0; i < m_pointCount; ++i) - { - ContactPoint* point = m_points + i; - - if (settings.m_drawGraphColors && 0 <= point->color && point->color <= b2_graphColorCount) - { - // graph color - float pointSize = point->color == b2_graphColorCount ? 7.5f : 5.0f; - g_draw.DrawPoint(point->position, pointSize, b2MakeColor(colors[point->color], 1.0f)); - // g_draw.DrawString(point->position, "%d", point->color); - } - else if (point->separation > b2_linearSlop) - { - // Speculative - g_draw.DrawPoint(point->position, 5.0f, speculativeColor); - } - else if (point->persisted == false) - { - // Add - g_draw.DrawPoint(point->position, 10.0f, addColor); - } - else if (point->persisted == true) - { - // Persist - g_draw.DrawPoint(point->position, 5.0f, persistColor); - } - - if (settings.m_drawContactNormals == 1) - { - b2Vec2 p1 = point->position; - b2Vec2 p2 = b2MulAdd(p1, k_axisScale, point->normal); - g_draw.DrawSegment(p1, p2, {0.9f, 0.9f, 0.9f, 1.0f}); - } - else if (settings.m_drawContactImpulse == 1) - { - b2Vec2 p1 = point->position; - b2Vec2 p2 = b2MulAdd(p1, k_impulseScale * point->normalImpulse, point->normal); - g_draw.DrawSegment(p1, p2, {0.9f, 0.9f, 0.3f, 1.0f}); - g_draw.DrawString(p1, "%.2f", point->normalImpulse); - } - - if (settings.m_drawFrictionImpulse == 1) - { - b2Vec2 tangent = b2CrossVS(point->normal, 1.0f); - b2Vec2 p1 = point->position; - b2Vec2 p2 = b2MulAdd(p1, k_impulseScale * point->tangentImpulse, tangent); - g_draw.DrawSegment(p1, p2, {0.9f, 0.9f, 0.3f, 1.0f}); - g_draw.DrawString(p1, "%.2f", point->tangentImpulse); - } - } } } @@ -430,37 +350,6 @@ void Sample::ShiftOrigin(b2Vec2 newOrigin) // m_world->ShiftOrigin(newOrigin); } -// Thread-safe callback -bool Sample::PreSolve(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, int32_t color) -{ - long startCount = m_pointCount.fetch_add(manifold->pointCount); - if (startCount >= k_maxContactPoints) - { - m_pointCount.store(k_maxContactPoints); - return true; - } - - long endCount = B2_MIN(k_maxContactPoints, startCount + manifold->pointCount); - - int32_t j = 0; - for (long i = startCount; i < endCount; ++i) - { - ContactPoint* cp = m_points + i; - cp->shapeIdA = shapeIdA; - cp->shapeIdB = shapeIdB; - cp->normal = manifold->normal; - cp->position = manifold->points[j].point; - cp->separation = manifold->points[j].separation; - cp->normalImpulse = manifold->points[j].normalImpulse; - cp->tangentImpulse = manifold->points[j].tangentImpulse; - cp->persisted = manifold->points[j].persisted; - cp->color = color; - ++j; - } - - return true; -} - SampleEntry g_sampleEntries[MAX_SAMPLES] = {{nullptr}}; int g_sampleCount = 0; diff --git a/samples/sample.h b/samples/sample.h index 0bc49f09..549ae000 100644 --- a/samples/sample.h +++ b/samples/sample.h @@ -110,8 +110,7 @@ class Sample int32_t m_taskCount; b2BodyId m_groundBodyId; - ContactPoint m_points[k_maxContactPoints]; - std::atomic m_pointCount; + // DestructionListener m_destructionListener; int32_t m_textLine; b2WorldId m_worldId; @@ -120,7 +119,6 @@ class Sample int32_t m_textIncrement; b2Profile m_maxProfile; b2Profile m_totalProfile; - bool m_collectContacts; }; typedef Sample* SampleCreateFcn(const Settings& settings); diff --git a/samples/settings.cpp b/samples/settings.cpp index b4c12c08..062e1f6a 100644 --- a/samples/settings.cpp +++ b/samples/settings.cpp @@ -43,20 +43,20 @@ void Settings::Save() { FILE* file = fopen(fileName, "w"); fprintf(file, "{\n"); - fprintf(file, " \"sampleIndex\": %d,\n", m_sampleIndex); - fprintf(file, " \"drawShapes\": %s,\n", m_drawShapes ? "true" : "false"); - fprintf(file, " \"drawJoints\": %s,\n", m_drawJoints ? "true" : "false"); - fprintf(file, " \"drawAABBs\": %s,\n", m_drawAABBs ? "true" : "false"); - fprintf(file, " \"drawContactPoints\": %s,\n", m_drawContactPoints ? "true" : "false"); - fprintf(file, " \"drawContactNormals\": %s,\n", m_drawContactNormals ? "true" : "false"); - fprintf(file, " \"drawContactImpulse\": %s,\n", m_drawContactImpulse ? "true" : "false"); - fprintf(file, " \"drawFrictionImpulse\": %s,\n", m_drawFrictionImpulse ? "true" : "false"); - fprintf(file, " \"drawMass\": %s,\n", m_drawMass ? "true" : "false"); - fprintf(file, " \"drawStats\": %s,\n", m_drawStats ? "true" : "false"); - fprintf(file, " \"drawProfile\": %s,\n", m_drawProfile ? "true" : "false"); - fprintf(file, " \"enableWarmStarting\": %s,\n", m_enableWarmStarting ? "true" : "false"); - fprintf(file, " \"enableContinuous\": %s,\n", m_enableContinuous ? "true" : "false"); - fprintf(file, " \"enableSleep\": %s\n", m_enableSleep ? "true" : "false"); + fprintf(file, " \"sampleIndex\": %d,\n", sampleIndex); + fprintf(file, " \"drawShapes\": %s,\n", drawShapes ? "true" : "false"); + fprintf(file, " \"drawJoints\": %s,\n", drawJoints ? "true" : "false"); + fprintf(file, " \"drawAABBs\": %s,\n", drawAABBs ? "true" : "false"); + fprintf(file, " \"drawContactPoints\": %s,\n", drawContactPoints ? "true" : "false"); + fprintf(file, " \"drawContactNormals\": %s,\n", drawContactNormals ? "true" : "false"); + fprintf(file, " \"drawContactImpulses\": %s,\n", drawContactImpulses ? "true" : "false"); + fprintf(file, " \"drawFrictionImpulse\": %s,\n", drawFrictionImpulses ? "true" : "false"); + fprintf(file, " \"drawMass\": %s,\n", drawMass ? "true" : "false"); + fprintf(file, " \"drawStats\": %s,\n", drawStats ? "true" : "false"); + fprintf(file, " \"drawProfile\": %s,\n", drawProfile ? "true" : "false"); + fprintf(file, " \"enableWarmStarting\": %s,\n", enableWarmStarting ? "true" : "false"); + fprintf(file, " \"enableContinuous\": %s,\n", enableContinuous ? "true" : "false"); + fprintf(file, " \"enableSleep\": %s\n", enableSleep ? "true" : "false"); fprintf(file, "}\n"); fclose(file); } @@ -102,18 +102,18 @@ void Settings::Load() const char* s = data + tokens[i + 1].start; strncpy(buffer, s, count); char* dummy; - m_sampleIndex = (int)strtol(buffer, &dummy, 10); + sampleIndex = (int)strtol(buffer, &dummy, 10); } else if (jsoneq(data, &tokens[i], "drawShapes") == 0) { const char* s = data + tokens[i + 1].start; if (strncmp(s, "true", 4) == 0) { - m_drawShapes = true; + drawShapes = true; } else if (strncmp(s, "false", 5) == 0) { - m_drawShapes = false; + drawShapes = false; } } } diff --git a/samples/settings.h b/samples/settings.h index c12c4cd7..795d0d27 100644 --- a/samples/settings.h +++ b/samples/settings.h @@ -9,28 +9,28 @@ struct Settings void Save(); void Load(); - int m_sampleIndex = 0; - int m_windowWidth = 1920; - int m_windowHeight = 1080; - float m_hertz = 60.0f; - int m_velocityIterations = 8; - int m_relaxIterations = 3; - int m_workerCount = 1; - bool m_drawShapes = true; - bool m_drawJoints = true; - bool m_drawAABBs = false; - bool m_drawContactPoints = false; - bool m_drawContactNormals = false; - bool m_drawContactImpulse = false; - bool m_drawFrictionImpulse = false; - bool m_drawMass = false; - bool m_drawGraphColors = false; - bool m_drawStats = false; - bool m_drawProfile = false; - bool m_enableWarmStarting = true; - bool m_enableContinuous = true; - bool m_enableSleep = false; - bool m_pause = false; - bool m_singleStep = false; - bool m_restart = false; + int sampleIndex = 0; + int windowWidth = 1920; + int windowHeight = 1080; + float hertz = 60.0f; + int velocityIterations = 8; + int relaxIterations = 3; + int workerCount = 1; + bool drawShapes = true; + bool drawJoints = true; + bool drawAABBs = false; + bool drawContactPoints = false; + bool drawContactNormals = false; + bool drawContactImpulses = false; + bool drawFrictionImpulses = false; + bool drawMass = false; + bool drawGraphColors = false; + bool drawStats = false; + bool drawProfile = false; + bool enableWarmStarting = true; + bool enableContinuous = true; + bool enableSleep = false; + bool pause = false; + bool singleStep = false; + bool restart = false; }; diff --git a/src/array.h b/src/array.h index e6dee2da..750ae331 100644 --- a/src/array.h +++ b/src/array.h @@ -38,3 +38,6 @@ void b2Array_Grow(void** a, int32_t elementSize); #define b2Array_Pop(a) \ B2_ASSERT(0 < b2Array(a).count); \ b2Array(a).count-- + +#define b2GetArrayCount(a) (b2Array(a).count) +#define b2GetArrayCapacity(a) (b2Array(a).capacity) diff --git a/src/body.c b/src/body.c index e9b5a30f..42325780 100644 --- a/src/body.c +++ b/src/body.c @@ -260,7 +260,7 @@ b2BodyId b2World_CreateBody(b2WorldId worldId, const b2BodyDef* def) } // Get a validated body from a world using an id. -static b2Body* b2GetBody(b2World* world, b2BodyId id) +b2Body* b2GetBody(b2World* world, b2BodyId id) { B2_ASSERT(0 <= id.index && id.index < world->bodyPool.capacity); b2Body* body = world->bodies + id.index; @@ -441,7 +441,9 @@ static b2ShapeId b2CreateShape(b2BodyId bodyId, const b2ShapeDef* def, const voi shape->userData = def->userData; shape->isSensor = def->isSensor; shape->enlargedAABB = false; - shape->reportContacts = false; + shape->enableSensorEvents = def->enableSensorEvents; + shape->enableContactEvents = def->enableContactEvents; + shape->enablePreSolveEvents = def->enablePreSolveEvents; shape->isFast = false; shape->proxyKey = B2_NULL_INDEX; shape->localCentroid = b2GetShapeCentroid(shape); diff --git a/src/body.h b/src/body.h index 84e83f01..4e5af5b8 100644 --- a/src/body.h +++ b/src/body.h @@ -101,11 +101,8 @@ typedef struct b2SolverBody float invI; // 4 } b2SolverBody; +b2Body* b2GetBody(b2World* world, b2BodyId id); bool b2ShouldBodiesCollide(b2World* world, b2Body* bodyA, b2Body* bodyB); - -//b2ShapeId b2Body_CreatePolygon(b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon); -//void b2Body_DestroyShape(b2ShapeId shapeId); - bool b2IsBodyAwake(b2World* world, b2Body* body); static inline b2Sweep b2MakeSweep(const b2Body* body) diff --git a/src/contact.c b/src/contact.c index 52e65eaf..737bfa05 100644 --- a/src/contact.c +++ b/src/contact.c @@ -13,6 +13,7 @@ #include "world.h" #include "box2d/distance.h" +#include "box2d/event_types.h" #include "box2d/manifold.h" #include "box2d/timer.h" @@ -209,6 +210,21 @@ void b2CreateContact(b2World* world, b2Shape* shapeA, b2Shape* shapeB) contact->flags |= b2_contactSensorFlag; } + if (shapeA->enableSensorEvents || shapeB->enableSensorEvents) + { + contact->flags |= b2_contactEnableSensorEvents; + } + + if (shapeA->enableContactEvents || shapeB->enableContactEvents) + { + contact->flags |= b2_contactEnableContactEvents; + } + + if (shapeA->enablePreSolveEvents || shapeB->enablePreSolveEvents) + { + contact->flags |= b2_contactEnablePreSolveEvents; + } + contact->shapeIndexA = shapeA->object.index; contact->shapeIndexB = shapeB->object.index; contact->cache = b2_emptyDistanceCache; @@ -461,7 +477,7 @@ void b2UpdateContact(b2World* world, b2Contact* contact, b2Shape* shapeA, b2Body } } - if (touching && world->preSolveFcn) + if (touching && world->preSolveFcn && (contact->flags & b2_contactEnablePreSolveEvents) != 0) { // TODO_ERIN this call assumes thread safety int32_t colorIndex = contact->colorIndex; @@ -493,3 +509,70 @@ void b2UpdateContact(b2World* world, b2Contact* contact, b2Shape* shapeA, b2Body // world->callbacks.endContactFcn(shapeIdA, shapeIdB); // } } + +b2Contact* b2GetContact(b2World* world, b2ContactId contactId) +{ + B2_ASSERT(0 <= contactId.index && contactId.index < world->contactPool.capacity); + b2Contact* contact = world->contacts + contactId.index; + B2_ASSERT(b2ObjectValid(&contact->object)); + B2_ASSERT(contact->object.revision == contactId.revision); + return contact; +} + +b2ContactId b2Body_GetFirstContact(b2BodyId bodyId) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + + if (body->contactList == B2_NULL_INDEX) + { + return b2_nullContactId; + } + + b2Contact* contact = world->contacts + body->contactList; + b2ContactId id = {contact->object.index, bodyId.world, contact->object.revision}; + return id; +} + +b2ContactId b2Body_GetNextContact(b2BodyId bodyId, b2ContactId contactId) +{ + b2World* world = b2GetWorldFromIndex(contactId.world); + b2Body* body = b2GetBody(world, bodyId); + b2Contact* contact = b2GetContact(world, contactId); + + if (contact->edges[0].bodyIndex == body->object.index) + { + if (contact->edges[0].nextKey == B2_NULL_INDEX) + { + return b2_nullContactId; + } + + contact = world->contacts + (contact->edges[0].nextKey >> 1); + } + else + { + B2_ASSERT(contact->edges[1].bodyIndex == body->object.index); + + if (contact->edges[1].nextKey == B2_NULL_INDEX) + { + return b2_nullContactId; + } + + contact = world->contacts + (contact->edges[1].nextKey >> 1); + } + + b2ContactId id = {contact->object.index, bodyId.world, contact->object.revision}; + return id; +} + +b2ContactData b2Contact_GetData(b2ContactId contactId) +{ + b2World* world = b2GetWorldFromIndex(contactId.world); + b2Contact* contact = b2GetContact(world, contactId); + b2Shape* shapeA = world->shapes + contact->shapeIndexA; + b2Shape* shapeB = world->shapes + contact->shapeIndexB; + b2ShapeId idA = {shapeA->object.index, contactId.world, shapeA->object.revision}; + b2ShapeId idB = {shapeB->object.index, contactId.world, shapeB->object.revision}; + b2ContactData data = {idA, idB, contact->manifold}; + return data; +} diff --git a/src/contact.h b/src/contact.h index 4df090f5..88a3ce4f 100644 --- a/src/contact.h +++ b/src/contact.h @@ -29,7 +29,6 @@ typedef struct b2ContactEdge enum b2ContactFlags { // Set when the shapes are touching. - // TODO_ERIN sensor only? Overlap? b2_contactTouchingFlag = 0x00000002, // This contact can be disabled (by user) @@ -50,6 +49,15 @@ enum b2ContactFlags // This contact stopped touching b2_contactStoppedTouching = 0x00000080, + + // This contact wants sensor events + b2_contactEnableSensorEvents = 0x00000100, + + // This contact wants contact events + b2_contactEnableContactEvents = 0x00000200, + + // This contact wants presolve events + b2_contactEnablePreSolveEvents = 0x00000400, }; /// The class manages contact between two shapes. A contact exists for each overlapping diff --git a/src/distance_joint.c b/src/distance_joint.c index fb88aa7d..0a6afb61 100644 --- a/src/distance_joint.c +++ b/src/distance_joint.c @@ -308,7 +308,7 @@ void b2SolveDistanceVelocity(b2Joint* base, b2StepContext* context, bool useBias float b2DistanceJoint_GetConstraintForce(b2JointId jointId, float timeStep) { - b2Joint* base = b2GetJoint(jointId, b2_distanceJoint); + b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); b2DistanceJoint* joint = &base->distanceJoint; if (timeStep > 0.0f) @@ -322,7 +322,7 @@ float b2DistanceJoint_GetConstraintForce(b2JointId jointId, float timeStep) void b2DistanceJoint_SetLength(b2JointId jointId, float length, float minLength, float maxLength) { - b2Joint* base = b2GetJoint(jointId, b2_distanceJoint); + b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); b2DistanceJoint* joint = &base->distanceJoint; joint->length = B2_CLAMP(length, b2_linearSlop, b2_huge); @@ -339,7 +339,7 @@ void b2DistanceJoint_SetLength(b2JointId jointId, float length, float minLength, float b2DistanceJoint_GetCurrentLength(b2JointId jointId) { - b2Joint* base = b2GetJoint(jointId, b2_distanceJoint); + b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); b2World* world = b2GetWorldFromIndex(jointId.world); B2_ASSERT(world->locked == false); @@ -365,7 +365,7 @@ float b2DistanceJoint_GetCurrentLength(b2JointId jointId) void b2DistanceJoint_SetTuning(b2JointId jointId, float hertz, float dampingRatio) { - b2Joint* base = b2GetJoint(jointId, b2_distanceJoint); + b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); b2DistanceJoint* joint = &base->distanceJoint; joint->hertz = hertz; joint->dampingRatio = dampingRatio; diff --git a/src/joint.c b/src/joint.c index ef54465f..1780e94b 100644 --- a/src/joint.c +++ b/src/joint.c @@ -15,7 +15,7 @@ #include "box2d/joint_types.h" // Get joint from id with validation -b2Joint* b2GetJoint(b2JointId id, b2JointType type) +b2Joint* b2GetJointCheckType(b2JointId id, b2JointType type) { B2_MAYBE_UNUSED(type); @@ -35,6 +35,15 @@ b2Joint* b2GetJoint(b2JointId id, b2JointType type) return joint; } +b2Joint* b2GetJoint(b2World* world, b2JointId jointId) +{ + B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity); + b2Joint* joint = world->joints + jointId.index; + B2_ASSERT(b2ObjectValid(&joint->object)); + B2_ASSERT(joint->object.revision == jointId.revision); + return joint; +} + void b2LinearStiffness(float* stiffness, float* damping, float frequencyHertz, float dampingRatio, b2BodyId bodyIdA, b2BodyId bodyIdB) { @@ -705,6 +714,52 @@ void b2SolveOverflowJoints(b2SolverTaskContext* context, bool useBias) b2TracyCZoneEnd(solve_joints); } +b2JointId b2Body_GetFirstJoint(b2BodyId bodyId) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + + if (body->jointList == B2_NULL_INDEX) + { + return b2_nullJointId; + } + + b2Joint* joint = world->joints + body->jointList; + b2JointId id = {joint->object.index, bodyId.world, joint->object.revision}; + return id; +} + +b2JointId b2Body_GetNextJoint(b2BodyId bodyId, b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + b2Joint* joint = b2GetJoint(world, jointId); + + if (joint->edges[0].bodyIndex == body->object.index) + { + if (joint->edges[0].nextKey == B2_NULL_INDEX) + { + return b2_nullJointId; + } + + joint = world->joints + (joint->edges[0].nextKey >> 1); + } + else + { + B2_ASSERT(joint->edges[1].bodyIndex == body->object.index); + + if (joint->edges[1].nextKey == B2_NULL_INDEX) + { + return b2_nullJointId; + } + + joint = world->joints + (joint->edges[1].nextKey >> 1); + } + + b2JointId id = {joint->object.index, bodyId.world, joint->object.revision}; + return id; +} + extern void b2DrawDistance(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bodyB); extern void b2DrawPrismatic(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bodyB); extern void b2DrawRevolute(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bodyB); diff --git a/src/joint.h b/src/joint.h index fa5732f2..ec9cb92c 100644 --- a/src/joint.h +++ b/src/joint.h @@ -220,4 +220,4 @@ void b2SolveOverflowJoints(b2SolverTaskContext* context, bool useBias); void b2DrawJoint(b2DebugDraw* draw, b2World* world, b2Joint* joint); // Get joint from id with validation -b2Joint* b2GetJoint(b2JointId id, b2JointType type); +b2Joint* b2GetJointCheckType(b2JointId id, b2JointType type); diff --git a/src/revolute_joint.c b/src/revolute_joint.c index 9abcc626..b39d6c7a 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -350,13 +350,13 @@ void b2RevoluteJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed) float b2RevoluteJoint_GetMotorTorque(b2JointId jointId, float inverseTimeStep) { - b2Joint* joint = b2GetJoint(jointId, b2_revoluteJoint); + b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); return inverseTimeStep * joint->revoluteJoint.motorImpulse; } void b2RevoluteJoint_SetMaxMotorTorque(b2JointId jointId, float torque) { - b2Joint* joint = b2GetJoint(jointId, b2_revoluteJoint); + b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); joint->revoluteJoint.maxMotorTorque = torque; } diff --git a/src/shape.c b/src/shape.c index 5f4784a8..84e67a59 100644 --- a/src/shape.c +++ b/src/shape.c @@ -201,11 +201,8 @@ b2Shape* b2GetShape(b2World* world, b2ShapeId shapeId) b2BodyId b2Shape_GetBody(b2ShapeId shapeId) { b2World* world = b2GetWorldFromIndex(shapeId.world); - B2_ASSERT(0 <= shapeId.index && shapeId.index < world->shapePool.capacity); - b2Shape* shape = world->shapes + shapeId.index; - B2_ASSERT(b2ObjectValid(&shape->object)); + b2Shape* shape = b2GetShape(world, shapeId); - B2_ASSERT(0 <= shape->bodyIndex && shape->bodyIndex < world->bodyPool.capacity); b2Body* body = world->bodies + shape->bodyIndex; B2_ASSERT(b2ObjectValid(&body->object)); diff --git a/src/shape.h b/src/shape.h index 40ffddca..8f06ba66 100644 --- a/src/shape.h +++ b/src/shape.h @@ -7,9 +7,11 @@ #include "box2d/distance.h" #include "box2d/geometry.h" +#include "box2d/id.h" #include "box2d/types.h" typedef struct b2BroadPhase b2BroadPhase; +typedef struct b2World b2World; typedef enum { @@ -40,7 +42,9 @@ typedef struct b2Shape void* userData; bool isSensor; - bool reportContacts; + bool enableSensorEvents; + bool enableContactEvents; + bool enablePreSolveEvents; bool enlargedAABB; bool isFast; diff --git a/src/world.c b/src/world.c index 8e4ee2c7..377450f9 100644 --- a/src/world.c +++ b/src/world.c @@ -125,7 +125,6 @@ b2WorldId b2CreateWorld(const b2WorldDef* def) world->sensorEndEventArray = b2CreateArray(sizeof(b2SensorEndTouchEvent), 4); world->contactBeginArray = b2CreateArray(sizeof(b2ContactBeginTouchEvent), 4); - world->contactPersistArray = b2CreateArray(sizeof(b2ContactPersistTouchEvent), 4); world->contactEndArray = b2CreateArray(sizeof(b2ContactEndTouchEvent), 4); world->stepId = 0; @@ -199,7 +198,6 @@ void b2DestroyWorld(b2WorldId id) b2DestroyArray(world->sensorEndEventArray, sizeof(b2SensorEndTouchEvent)); b2DestroyArray(world->contactBeginArray, sizeof(b2ContactBeginTouchEvent)); - b2DestroyArray(world->contactPersistArray, sizeof(b2ContactPersistTouchEvent)); b2DestroyArray(world->contactEndArray, sizeof(b2ContactEndTouchEvent)); b2DestroyPool(&world->islandPool); @@ -404,10 +402,12 @@ static void b2Collide(b2World* world) const b2Shape* shapeB = shapes + contact->shapeIndexB; b2ShapeId shapeIdA = {shapeA->object.index, worldIndex, shapeA->object.revision}; b2ShapeId shapeIdB = {shapeB->object.index, worldIndex, shapeB->object.revision}; + uint32_t flags = contact->flags; - if (contact->flags & b2_contactDisjoint) + if (flags & b2_contactDisjoint) { - if (contact->flags & b2_contactTouchingFlag) + // Was touching? + if ((flags & b2_contactTouchingFlag) != 0 && (flags & b2_contactEnableContactEvents) != 0) { b2ContactEndTouchEvent event = {shapeIdA, shapeIdB}; b2Array_Push(world->contactEndArray, event); @@ -416,10 +416,10 @@ static void b2Collide(b2World* world) // Bounding boxes no longer overlap b2DestroyContact(world, contact); } - else if (contact->flags & b2_contactStartedTouching) + else if (flags & b2_contactStartedTouching) { B2_ASSERT(contact->islandIndex == B2_NULL_INDEX); - if (contact->flags & b2_contactSensorFlag) + if ((flags & b2_contactSensorFlag) != 0 && (flags & b2_contactEnableSensorEvents) != 0) { if (shapeA->isSensor) { @@ -435,23 +435,23 @@ static void b2Collide(b2World* world) } else { - b2ContactBeginTouchEvent event = {shapeIdA, shapeIdB, contact->manifold}; - b2Array_Push(world->contactBeginArray, event); + if (flags & b2_contactEnableContactEvents) + { + b2ContactBeginTouchEvent event = {shapeIdA, shapeIdB, contact->manifold}; + b2Array_Push(world->contactBeginArray, event); + } b2LinkContact(world, contact); b2AddContactToGraph(world, contact); } + contact->flags &= ~b2_contactStartedTouching; } else { B2_ASSERT(contact->flags & b2_contactStoppedTouching); - if (contact->flags & b2_contactSensorFlag) + if ((flags & b2_contactSensorFlag) != 0 && (flags & b2_contactEnableSensorEvents) != 0) { - const b2Shape* shapeA = shapes + contact->shapeIndexA; - const b2Shape* shapeB = shapes + contact->shapeIndexB; - b2ShapeId shapeIdA = {shapeA->object.index, worldIndex, shapeA->object.revision}; - b2ShapeId shapeIdB = {shapeB->object.index, worldIndex, shapeB->object.revision}; if (shapeA->isSensor) { b2SensorEndTouchEvent event = {shapeIdA, shapeIdB}; @@ -466,12 +466,16 @@ static void b2Collide(b2World* world) } else { - b2ContactEndTouchEvent event = {shapeIdA, shapeIdB}; - b2Array_Push(world->contactEndArray, event); + if (contact->flags & b2_contactEnableContactEvents) + { + b2ContactEndTouchEvent event = {shapeIdA, shapeIdB}; + b2Array_Push(world->contactEndArray, event); + } b2UnlinkContact(world, contact); b2RemoveContactFromGraph(world, contact); } + contact->flags &= ~b2_contactStoppedTouching; } @@ -794,6 +798,92 @@ void b2World_Draw(b2WorldId worldId, b2DebugDraw* draw) draw->DrawString(p, buffer, draw->context); } } + + if (draw->drawContacts) + { + const float k_impulseScale = 1.0f; + const float k_axisScale = 0.3f; + b2Color speculativeColor = {0.3f, 0.3f, 0.3f, 1.0f}; + b2Color addColor = {0.3f, 0.95f, 0.3f, 1.0f}; + b2Color persistColor = {0.3f, 0.3f, 0.95f, 1.0f}; + b2Color normalColor = {0.9f, 0.9f, 0.9f, 1.0f}; + b2Color impulseColor = {0.9f, 0.9f, 0.3f, 1.0f}; + b2Color frictionColor = {0.9f, 0.9f, 0.3f, 1.0f}; + + b2HexColor colors[b2_graphColorCount + 1] = { + b2_colorRed, b2_colorOrange, b2_colorYellow, b2_colorGreen, b2_colorCyan, b2_colorBlue, b2_colorViolet, + b2_colorPink, b2_colorChocolate, b2_colorGoldenrod, b2_colorCoral, b2_colorAqua, b2_colorBlack}; + + int count = b2GetArrayCount(world->awakeContactArray); + + for (int32_t i = 0; i < count; ++i) + { + int index = world->awakeContactArray[i]; + if (index == B2_NULL_INDEX) + { + continue; + } + + b2Contact* contact = world->contacts + index; + int pointCount = contact->manifold.pointCount; + int colorIndex = contact->colorIndex; + b2Vec2 normal = contact->manifold.normal; + char buffer[32]; + + for (int j = 0; j < pointCount; ++j) + { + b2ManifoldPoint* point = contact->manifold.points + j; + + if (draw->drawGraphColors && 0 <= colorIndex && colorIndex <= b2_graphColorCount) + { + // graph color + float pointSize = colorIndex == b2_graphColorCount ? 7.5f : 5.0f; + draw->DrawPoint(point->point, pointSize, b2MakeColor(colors[colorIndex], 1.0f), draw->context); + // g_draw.DrawString(point->position, "%d", point->color); + } + else if (point->separation > b2_linearSlop) + { + // Speculative + draw->DrawPoint(point->point, 5.0f, speculativeColor, draw->context); + } + else if (point->persisted == false) + { + // Add + draw->DrawPoint(point->point, 10.0f, addColor, draw->context); + } + else if (point->persisted == true) + { + // Persist + draw->DrawPoint(point->point, 5.0f, persistColor, draw->context); + } + + if (draw->drawContactNormals) + { + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd(p1, k_axisScale, normal); + draw->DrawSegment(p1, p2, normalColor, draw->context); + } + else if (draw->drawContactImpulses) + { + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd(p1, k_impulseScale * point->normalImpulse, normal); + draw->DrawSegment(p1, p2, impulseColor, draw->context); + snprintf(buffer, B2_ARRAY_COUNT(buffer), "%.2f", point->normalImpulse); + draw->DrawString(p1, buffer, draw->context); + } + + if (draw->drawFrictionImpulses) + { + b2Vec2 tangent = b2RightPerp(normal); + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd(p1, k_impulseScale * point->tangentImpulse, tangent); + draw->DrawSegment(p1, p2, frictionColor, draw->context); + snprintf(buffer, B2_ARRAY_COUNT(buffer), "%.2f", point->normalImpulse); + draw->DrawString(p1, buffer, draw->context); + } + } + } + } } b2SensorEvents b2World_GetSensorEvents(b2WorldId worldId) diff --git a/test/test_determinism.c b/test/test_determinism.c index 823e53b0..100e3a43 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -193,7 +193,7 @@ int DeterminismTest(void) b2Vec2 p1 = finalPositions[0][i]; b2Vec2 p2 = finalPositions[1][i]; float a1 = finalAngles[0][i]; - float a2 = finalAngles[0][i]; + float a2 = finalAngles[1][i]; ENSURE(p1.x == p2.x); ENSURE(p1.y == p2.y); From 38de411edd02d521ed1c7207e1657c95e85d651b Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Fri, 15 Dec 2023 22:52:51 -0800 Subject: [PATCH 04/11] finished api --- include/box2d/box2d.h | 4 +- src/body.c | 121 ++++++++++++++++++++++++++++++++++++++---- 2 files changed, 112 insertions(+), 13 deletions(-) diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index fb5fd738..231c2f31 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -43,7 +43,7 @@ BOX2D_API b2BodyId b2World_CreateBody(b2WorldId worldId, const b2BodyDef* def); BOX2D_API void b2World_DestroyBody(b2BodyId bodyId); /// Destroy a rigid body and get an array of all the shapes it was touching (on other bodies). -BOX2D_API int32_t b2World_DestroyBodyWithResults(b2BodyId bodyId, b2ShapeId* touchingShapes, int32_t maxShapes); +BOX2D_API int32_t b2World_DestroyBodyAndGetTouching(b2BodyId bodyId, b2ShapeId* touchingShapes, int32_t maxShapes); BOX2D_API b2Vec2 b2Body_GetPosition(b2BodyId bodyId); BOX2D_API float b2Body_GetAngle(b2BodyId bodyId); @@ -107,7 +107,7 @@ BOX2D_API b2ShapeId b2Body_CreatePolygon(b2BodyId bodyId, const b2ShapeDef* def, BOX2D_API void b2Body_DestroyShape(b2ShapeId shapeId); /// Destroy a shape from a body and get an array of all the other shapes this shape was touching. -BOX2D_API int32_t b2Body_DestroyShapeWithResults(b2ShapeId shapeId, b2ShapeId* touchingShapes, int32_t maxShapes); +BOX2D_API int32_t b2Body_DestroyShapeAndGetTouching(b2ShapeId shapeId, b2ShapeId* touchingShapes, int32_t maxShapes); BOX2D_API b2ChainId b2Body_CreateChain(b2BodyId bodyId, const b2ChainDef* def); BOX2D_API void b2Body_DestroyChain(b2ChainId chainId); diff --git a/src/body.c b/src/body.c index 42325780..41c7fb92 100644 --- a/src/body.c +++ b/src/body.c @@ -269,18 +269,8 @@ b2Body* b2GetBody(b2World* world, b2BodyId id) return body; } -void b2World_DestroyBody(b2BodyId bodyId) +void b2DestroyBody(b2World* world, b2Body* body) { - b2World* world = b2GetWorldFromIndex(bodyId.world); - B2_ASSERT(world->locked == false); - - if (world->locked) - { - return; - } - - b2Body* body = b2GetBody(world, bodyId); - // User must destroy joints before destroying bodies B2_ASSERT(body->jointList == B2_NULL_INDEX && body->jointCount == 0); @@ -314,6 +304,67 @@ void b2World_DestroyBody(b2BodyId bodyId) b2FreeObject(&world->bodyPool, &body->object); } +void b2World_DestroyBody(b2BodyId bodyId) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + B2_ASSERT(world->locked == false); + + if (world->locked) + { + return; + } + + b2Body* body = b2GetBody(world, bodyId); + b2DestroyBody(world, body); +} + +int32_t b2World_DestroyBodyAndGetTouching(b2BodyId bodyId, b2ShapeId* touchingShapes, int32_t maxShapes) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return 0; + } + + b2Body* body = b2GetBody(world, bodyId); + + // Find other shapes that are currently touching this body + int32_t contactKey = body->contactList; + int32_t reportCount = 0; + while (contactKey != B2_NULL_INDEX && reportCount < maxShapes) + { + int32_t contactIndex = contactKey >> 1; + int32_t edgeIndex = contactKey & 1; + + b2Contact* contact = world->contacts + contactIndex; + if (contact->flags & b2_contactTouchingFlag) + { + b2Shape* otherShape; + b2Shape* shapeA = world->shapes + contact->shapeIndexA; + if (shapeA->bodyIndex == body->object.index) + { + otherShape = world->shapes + contact->shapeIndexB; + } + else + { + B2_ASSERT(world->shapes[contact->shapeIndexB].bodyIndex == body->object.index); + otherShape = world->shapes + contact->shapeIndexA; + } + + b2ShapeId otherShapeId = {otherShape->object.index, bodyId.world, otherShape->object.revision}; + touchingShapes[reportCount] = otherShapeId; + reportCount += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + b2DestroyBody(world, body); + + return reportCount; +} + static void b2ComputeMass(b2World* world, b2Body* body) { // Compute mass data from shapes. Each shape has its own density. @@ -674,6 +725,54 @@ void b2Body_DestroyShape(b2ShapeId shapeId) b2DestroyShape(world, shape); } +// Destroy a shape on a body. This doesn't need to be called when destroying a body. +int32_t b2Body_DestroyShapeAndGetTouching(b2ShapeId shapeId, b2ShapeId* touchingShapes, int32_t maxShapes) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return 0; + } + + b2Shape* shape = b2GetShape(world, shapeId); + + // Find other shapes that are currently touching this shape + b2Body* body = world->bodies + shape->bodyIndex; + int32_t contactKey = body->contactList; + int32_t reportCount = 0; + while (contactKey != B2_NULL_INDEX && reportCount < maxShapes) + { + int32_t contactIndex = contactKey >> 1; + int32_t edgeIndex = contactKey & 1; + + b2Contact* contact = world->contacts + contactIndex; + if (contact->flags & b2_contactTouchingFlag) + { + b2Shape* otherShape; + if (contact->shapeIndexA == shape->object.index) + { + otherShape = world->shapes + contact->shapeIndexB; + } + else + { + B2_ASSERT(contact->shapeIndexB == shape->object.index); + otherShape = world->shapes + contact->shapeIndexA; + } + + b2ShapeId otherShapeId = {otherShape->object.index, shapeId.world, otherShape->object.revision}; + touchingShapes[reportCount] = otherShapeId; + reportCount += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + b2DestroyShape(world, shape); + + return reportCount; +} + void b2Body_DestroyChain(b2ChainId chainId) { b2World* world = b2GetWorldFromIndex(chainId.world); From 0bb3194f99d84aee6defd32fc0f12c715fc4a6da Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Fri, 15 Dec 2023 23:04:28 -0800 Subject: [PATCH 05/11] fix b2Body_DestroyChain --- src/body.c | 212 +++++++++++++++++++++++++++++------------------------ 1 file changed, 116 insertions(+), 96 deletions(-) diff --git a/src/body.c b/src/body.c index 41c7fb92..3c53c925 100644 --- a/src/body.c +++ b/src/body.c @@ -553,98 +553,6 @@ b2ShapeId b2Body_CreateSegment(b2BodyId bodyId, const b2ShapeDef* def, const b2S return b2CreateShape(bodyId, def, segment, b2_segmentShape); } -b2ChainId b2Body_CreateChain(b2BodyId bodyId, const b2ChainDef* def) -{ - B2_ASSERT(b2IsValid(def->friction) && def->friction >= 0.0f); - B2_ASSERT(b2IsValid(def->restitution) && def->restitution >= 0.0f); - B2_ASSERT(def->count >= 4); - - b2World* world = b2GetWorldFromIndex(bodyId.world); - B2_ASSERT(world->locked == false); - if (world->locked) - { - return b2_nullChainId; - } - - b2Body* body = b2GetBody(world, bodyId); - - b2ChainShape* chainShape = (b2ChainShape*)b2AllocObject(&world->chainPool); - world->chains = (b2ChainShape*)world->chainPool.memory; - - chainShape->bodyIndex = bodyId.index; - chainShape->nextIndex = body->chainList; - body->chainList = chainShape->object.index; - - b2ShapeDef shapeDef = b2DefaultShapeDef(); - shapeDef.userData = def->userData; - shapeDef.restitution = def->restitution; - shapeDef.friction = def->friction; - shapeDef.filter = def->filter; - - int32_t n = def->count; - const b2Vec2* points = def->points; - - if (def->loop) - { - chainShape->count = n; - chainShape->shapeIndices = b2Alloc(n * sizeof(int32_t)); - - b2SmoothSegment smoothSegment; - - int32_t prevIndex = n - 1; - for (int32_t i = 0; i < n - 2; ++i) - { - smoothSegment.ghost1 = points[prevIndex]; - smoothSegment.segment.point1 = points[i]; - smoothSegment.segment.point2 = points[i + 1]; - smoothSegment.ghost2 = points[i + 2]; - prevIndex = i; - - b2ShapeId shapeId = b2CreateShape(bodyId, &shapeDef, &smoothSegment, b2_smoothSegmentShape); - chainShape->shapeIndices[i] = shapeId.index; - } - - { - smoothSegment.ghost1 = points[n - 3]; - smoothSegment.segment.point1 = points[n - 2]; - smoothSegment.segment.point2 = points[n - 1]; - smoothSegment.ghost2 = points[0]; - b2ShapeId shapeId = b2CreateShape(bodyId, &shapeDef, &smoothSegment, b2_smoothSegmentShape); - chainShape->shapeIndices[n - 2] = shapeId.index; - } - - { - smoothSegment.ghost1 = points[n - 2]; - smoothSegment.segment.point1 = points[n - 1]; - smoothSegment.segment.point2 = points[0]; - smoothSegment.ghost2 = points[1]; - b2ShapeId shapeId = b2CreateShape(bodyId, &shapeDef, &smoothSegment, b2_smoothSegmentShape); - chainShape->shapeIndices[n - 1] = shapeId.index; - } - } - else - { - chainShape->count = n - 3; - chainShape->shapeIndices = b2Alloc(n * sizeof(int32_t)); - - b2SmoothSegment smoothSegment; - - for (int32_t i = 0; i < n - 3; ++i) - { - smoothSegment.ghost1 = points[i]; - smoothSegment.segment.point1 = points[i + 1]; - smoothSegment.segment.point2 = points[i + 2]; - smoothSegment.ghost2 = points[i + 3]; - - b2ShapeId shapeId = b2CreateShape(bodyId, &shapeDef, &smoothSegment, b2_smoothSegmentShape); - chainShape->shapeIndices[i] = shapeId.index; - } - } - - b2ChainId id = {chainShape->object.index, bodyId.world, chainShape->object.revision}; - return id; -} - // Destroy a shape on a body. This doesn't need to be called when destroying a body. static void b2DestroyShape(b2World* world, b2Shape* shape) { @@ -773,6 +681,98 @@ int32_t b2Body_DestroyShapeAndGetTouching(b2ShapeId shapeId, b2ShapeId* touching return reportCount; } +b2ChainId b2Body_CreateChain(b2BodyId bodyId, const b2ChainDef* def) +{ + B2_ASSERT(b2IsValid(def->friction) && def->friction >= 0.0f); + B2_ASSERT(b2IsValid(def->restitution) && def->restitution >= 0.0f); + B2_ASSERT(def->count >= 4); + + b2World* world = b2GetWorldFromIndex(bodyId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return b2_nullChainId; + } + + b2Body* body = b2GetBody(world, bodyId); + + b2ChainShape* chainShape = (b2ChainShape*)b2AllocObject(&world->chainPool); + world->chains = (b2ChainShape*)world->chainPool.memory; + + chainShape->bodyIndex = bodyId.index; + chainShape->nextIndex = body->chainList; + body->chainList = chainShape->object.index; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.userData = def->userData; + shapeDef.restitution = def->restitution; + shapeDef.friction = def->friction; + shapeDef.filter = def->filter; + + int32_t n = def->count; + const b2Vec2* points = def->points; + + if (def->loop) + { + chainShape->count = n; + chainShape->shapeIndices = b2Alloc(n * sizeof(int32_t)); + + b2SmoothSegment smoothSegment; + + int32_t prevIndex = n - 1; + for (int32_t i = 0; i < n - 2; ++i) + { + smoothSegment.ghost1 = points[prevIndex]; + smoothSegment.segment.point1 = points[i]; + smoothSegment.segment.point2 = points[i + 1]; + smoothSegment.ghost2 = points[i + 2]; + prevIndex = i; + + b2ShapeId shapeId = b2CreateShape(bodyId, &shapeDef, &smoothSegment, b2_smoothSegmentShape); + chainShape->shapeIndices[i] = shapeId.index; + } + + { + smoothSegment.ghost1 = points[n - 3]; + smoothSegment.segment.point1 = points[n - 2]; + smoothSegment.segment.point2 = points[n - 1]; + smoothSegment.ghost2 = points[0]; + b2ShapeId shapeId = b2CreateShape(bodyId, &shapeDef, &smoothSegment, b2_smoothSegmentShape); + chainShape->shapeIndices[n - 2] = shapeId.index; + } + + { + smoothSegment.ghost1 = points[n - 2]; + smoothSegment.segment.point1 = points[n - 1]; + smoothSegment.segment.point2 = points[0]; + smoothSegment.ghost2 = points[1]; + b2ShapeId shapeId = b2CreateShape(bodyId, &shapeDef, &smoothSegment, b2_smoothSegmentShape); + chainShape->shapeIndices[n - 1] = shapeId.index; + } + } + else + { + chainShape->count = n - 3; + chainShape->shapeIndices = b2Alloc(n * sizeof(int32_t)); + + b2SmoothSegment smoothSegment; + + for (int32_t i = 0; i < n - 3; ++i) + { + smoothSegment.ghost1 = points[i]; + smoothSegment.segment.point1 = points[i + 1]; + smoothSegment.segment.point2 = points[i + 2]; + smoothSegment.ghost2 = points[i + 3]; + + b2ShapeId shapeId = b2CreateShape(bodyId, &shapeDef, &smoothSegment, b2_smoothSegmentShape); + chainShape->shapeIndices[i] = shapeId.index; + } + } + + b2ChainId id = {chainShape->object.index, bodyId.world, chainShape->object.revision}; + return id; +} + void b2Body_DestroyChain(b2ChainId chainId) { b2World* world = b2GetWorldFromIndex(chainId.world); @@ -784,18 +784,38 @@ void b2Body_DestroyChain(b2ChainId chainId) B2_ASSERT(0 <= chainId.index && chainId.index < world->chainPool.count); - b2ChainShape* chainShape = world->chains + chainId.index; - B2_ASSERT(chainShape->object.revision == chainId.revision); + b2ChainShape* chain = world->chains + chainId.index; + B2_ASSERT(chain->object.revision == chainId.revision); - int32_t count = chainShape->count; + int32_t count = chain->count; for (int32_t i = 0; i < count; ++i) { - int32_t shapeIndex = chainShape->shapeIndices[i]; + int32_t shapeIndex = chain->shapeIndices[i]; B2_ASSERT(0 <= shapeIndex && shapeIndex < world->shapePool.count); b2Shape* shape = world->shapes + shapeIndex; b2DestroyShape(world, shape); } + + b2Free(chain->shapeIndices, count * sizeof(int32_t)); + + // Remove the chain from the body's singly linked list. + b2Body* body = world->bodies + chain->bodyIndex; + int32_t* indexPtr = &body->chainList; + bool found = false; + while (*indexPtr != B2_NULL_INDEX) + { + if (*indexPtr == chain->object.index) + { + *indexPtr = chain->nextIndex; + found = true; + break; + } + + indexPtr = &(world->chains[*indexPtr].nextIndex); + } + + b2FreeObject(&world->chainPool, &chain->object); } bool b2IsBodyAwake(b2World* world, b2Body* body) From 9175a82ef4a45983c436252a62898f5dc86a5598 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Fri, 15 Dec 2023 23:13:53 -0800 Subject: [PATCH 06/11] geometry access on shapes --- include/box2d/box2d.h | 7 +++++++ include/box2d/types.h | 10 +++++++++ src/shape.c | 47 +++++++++++++++++++++++++++++++++++++++++++ src/shape.h | 10 --------- 4 files changed, 64 insertions(+), 10 deletions(-) diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index 231c2f31..53871df6 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -122,6 +122,13 @@ BOX2D_API bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point); BOX2D_API void b2Shape_SetFriction(b2ShapeId shapeId, float friction); BOX2D_API void b2Shape_SetRestitution(b2ShapeId shapeId, float restitution); +BOX2D_API b2ShapeType b2Shape_GetType(b2ShapeId shapeId); +BOX2D_API const b2Circle* b2Shape_GetCircle(b2ShapeId shapeId); +BOX2D_API const b2Segment* b2Shape_GetSegment(b2ShapeId shapeId); +BOX2D_API const b2Polygon* b2Shape_GetSmoothSegment(b2ShapeId shapeId); +BOX2D_API const b2Capsule* b2Shape_GetCapsule(b2ShapeId shapeId); +BOX2D_API const b2Polygon* b2Shape_GetPolygon(b2ShapeId shapeId); + BOX2D_API void b2Chain_SetFriction(b2ChainId chainId, float friction); BOX2D_API void b2Chain_SetRestitution(b2ChainId chainId, float restitution); diff --git a/include/box2d/types.h b/include/box2d/types.h index c4d12e3c..9920a54d 100644 --- a/include/box2d/types.h +++ b/include/box2d/types.h @@ -266,6 +266,16 @@ typedef struct b2QueryFilter static const b2QueryFilter b2_defaultQueryFilter = {0x00000001, 0xFFFFFFFF}; +typedef enum b2ShapeType +{ + b2_capsuleShape, + b2_circleShape, + b2_polygonShape, + b2_segmentShape, + b2_smoothSegmentShape, + b2_shapeTypeCount +} b2ShapeType; + /// Used to create a shape typedef struct b2ShapeDef { diff --git a/src/shape.c b/src/shape.c index 84e67a59..0bddfac4 100644 --- a/src/shape.c +++ b/src/shape.c @@ -260,6 +260,53 @@ void b2Shape_SetRestitution(b2ShapeId shapeId, float restitution) shape->restitution = restitution; } +b2ShapeType b2Shape_GetType(b2ShapeId shapeId) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); + return shape->type; +} + +const b2Circle* b2Shape_GetCircle(b2ShapeId shapeId) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); + B2_ASSERT(shape->type == b2_circleShape); + return &shape->circle; +} + +const b2Segment* b2Shape_GetSegment(b2ShapeId shapeId) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); + B2_ASSERT(shape->type == b2_segmentShape); + return &shape->segment; +} + +const b2SmoothSegment* b2Shape_GetSmoothSegment(b2ShapeId shapeId) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); + B2_ASSERT(shape->type == b2_smoothSegmentShape); + return &shape->smoothSegment; +} + +const b2Capsule* b2Shape_GetCapsule(b2ShapeId shapeId) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); + B2_ASSERT(shape->type == b2_capsuleShape); + return &shape->capsule; +} + +const b2Polygon* b2Shape_GetPolygon(b2ShapeId shapeId) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); + B2_ASSERT(shape->type == b2_polygonShape); + return &shape->polygon; +} + void b2Chain_SetFriction(b2ChainId chainId, float friction) { b2World* world = b2GetWorldFromIndex(chainId.world); diff --git a/src/shape.h b/src/shape.h index 8f06ba66..dac5cdd0 100644 --- a/src/shape.h +++ b/src/shape.h @@ -13,16 +13,6 @@ typedef struct b2BroadPhase b2BroadPhase; typedef struct b2World b2World; -typedef enum -{ - b2_capsuleShape, - b2_circleShape, - b2_polygonShape, - b2_segmentShape, - b2_smoothSegmentShape, - b2_shapeTypeCount -} b2ShapeType; - typedef struct b2Shape { b2Object object; From 100e63788ed94ee01310f16b565ebe4af0b38176 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sat, 16 Dec 2023 14:29:51 -0800 Subject: [PATCH 07/11] get contact data from shape or body id --- include/box2d/box2d.h | 20 ++++--- include/box2d/event_types.h | 2 +- include/box2d/id.h | 3 ++ include/box2d/manifold.h | 5 +- src/body.c | 104 +++++++++++++++++++----------------- src/manifold.c | 76 ++------------------------ src/shape.c | 61 +++++++++++++++++++++ 7 files changed, 139 insertions(+), 132 deletions(-) diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index 53871df6..d10f908a 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -42,9 +42,6 @@ BOX2D_API b2BodyId b2World_CreateBody(b2WorldId worldId, const b2BodyDef* def); /// @warning This function is locked during callbacks. BOX2D_API void b2World_DestroyBody(b2BodyId bodyId); -/// Destroy a rigid body and get an array of all the shapes it was touching (on other bodies). -BOX2D_API int32_t b2World_DestroyBodyAndGetTouching(b2BodyId bodyId, b2ShapeId* touchingShapes, int32_t maxShapes); - BOX2D_API b2Vec2 b2Body_GetPosition(b2BodyId bodyId); BOX2D_API float b2Body_GetAngle(b2BodyId bodyId); BOX2D_API void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle); @@ -107,7 +104,7 @@ BOX2D_API b2ShapeId b2Body_CreatePolygon(b2BodyId bodyId, const b2ShapeDef* def, BOX2D_API void b2Body_DestroyShape(b2ShapeId shapeId); /// Destroy a shape from a body and get an array of all the other shapes this shape was touching. -BOX2D_API int32_t b2Body_DestroyShapeAndGetTouching(b2ShapeId shapeId, b2ShapeId* touchingShapes, int32_t maxShapes); +BOX2D_API int32_t b2Body_GetTouching(b2ShapeId shapeId, b2ShapeId* touchingShapes, int32_t maxShapes); BOX2D_API b2ChainId b2Body_CreateChain(b2BodyId bodyId, const b2ChainDef* def); BOX2D_API void b2Body_DestroyChain(b2ChainId chainId); @@ -133,9 +130,18 @@ BOX2D_API void b2Chain_SetFriction(b2ChainId chainId, float friction); BOX2D_API void b2Chain_SetRestitution(b2ChainId chainId, float restitution); /// Contacts -BOX2D_API b2ContactId b2Body_GetFirstContact(b2BodyId bodyId); -BOX2D_API b2ContactId b2Body_GetNextContact(b2BodyId bodyId, b2ContactId contactId); -BOX2D_API b2ContactData b2Contact_GetData(b2ContactId contactId); + +/// Get the number of touching contacts on a body +BOX2D_API int32_t b2Body_GetContactCount(b2BodyId bodyId); + +/// Get the touching contact data for a body +BOX2D_API int32_t b2Body_GetContactData(b2BodyId bodyId, b2ContactData* contactData, int32_t capacity); + +/// Get the number of touching contacts on a shape. For efficiency, this may be larger than the actual number. +BOX2D_API int32_t b2Shape_GetContactCount(b2ShapeId shapeId); + +/// Get the touching contact data for a shape. The provided shapeId will be either shapeIdA or shapeIdB on the contact data. +BOX2D_API int32_t b2Shape_GetContactData(b2ShapeId shapeId, b2ContactData* contactData, int32_t capacity); /// Create a joint BOX2D_API b2JointId b2World_CreateDistanceJoint(b2WorldId worldId, const b2DistanceJointDef* def); diff --git a/include/box2d/event_types.h b/include/box2d/event_types.h index 0173c1e7..186195a4 100644 --- a/include/box2d/event_types.h +++ b/include/box2d/event_types.h @@ -61,7 +61,7 @@ typedef struct b2ContactEvents int endCount; } b2ContactEvents; -/// This is the data you can access using a b2ContactId +/// This is the data you can access using a b2ContactId. typedef struct b2ContactData { b2ShapeId shapeIdA; diff --git a/include/box2d/id.h b/include/box2d/id.h index c1f56793..69a8e9cd 100644 --- a/include/box2d/id.h +++ b/include/box2d/id.h @@ -64,3 +64,6 @@ static const b2ChainId b2_nullChainId = {-1, -1, 0}; #define B2_IS_NULL(id) (id.index == -1) #define B2_NON_NULL(id) (id.index != -1) + +// Compare two ids for equality. Doesn't work for b2WorldId./ +#define B2_ID_EQUALS(id1, id2) (id1.index == id2.index && id1.world == id2.world && id1.revision == id2.revision) diff --git a/include/box2d/manifold.h b/include/box2d/manifold.h index fe58933c..5df52821 100644 --- a/include/box2d/manifold.h +++ b/include/box2d/manifold.h @@ -14,9 +14,6 @@ typedef struct b2Polygon b2Polygon; typedef struct b2Segment b2Segment; typedef struct b2SmoothSegment b2SmoothSegment; -// todo internal -#define B2_MAKE_ID(A, B) ((uint8_t)(A) << 8 | (uint8_t)(B)) - /// A manifold point is a contact point belonging to a contact /// manifold. It holds details related to the geometry and dynamics /// of the contact points. @@ -25,7 +22,7 @@ typedef struct b2ManifoldPoint /// world coordinates of contact point b2Vec2 point; - /// Body anchors used by solver + /// body anchors used by solver internally b2Vec2 anchorA, anchorB; /// the separation of the contact point, negative if penetrating diff --git a/src/body.c b/src/body.c index 3c53c925..919481b8 100644 --- a/src/body.c +++ b/src/body.c @@ -15,6 +15,7 @@ #include "world.h" #include "box2d/aabb.h" +#include "box2d/event_types.h" #include "box2d/id.h" static void b2CreateIslandForBody(b2World* world, b2Body* body, bool isAwake) @@ -318,6 +319,61 @@ void b2World_DestroyBody(b2BodyId bodyId) b2DestroyBody(world, body); } +int32_t b2Body_GetContactCount(b2BodyId bodyId) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return 0; + } + + b2Body* body = b2GetBody(world, bodyId); + + // Conservative and fast + return body->contactCount; +} + +int32_t b2Body_GetContactData(b2BodyId bodyId, b2ContactData* contactData, int32_t capacity) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return 0; + } + + b2Body* body = b2GetBody(world, bodyId); + + int32_t contactKey = body->contactList; + int32_t index = 0; + while (contactKey != B2_NULL_INDEX && index < capacity) + { + int32_t contactIndex = contactKey >> 1; + int32_t edgeIndex = contactKey & 1; + + b2Contact* contact = world->contacts + contactIndex; + + // Is contact touching? + if (contact->flags & b2_contactTouchingFlag) + { + b2Shape* shapeA = world->shapes + contact->shapeIndexA; + b2Shape* shapeB = world->shapes + contact->shapeIndexB; + + contactData[index].shapeIdA = (b2ShapeId){shapeA->object.index, bodyId.world, shapeA->object.revision}; + contactData[index].shapeIdB = (b2ShapeId){shapeB->object.index, bodyId.world, shapeB->object.revision}; + contactData[index].manifold = contact->manifold; + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B2_ASSERT(index < capacity); + + return index; +} + int32_t b2World_DestroyBodyAndGetTouching(b2BodyId bodyId, b2ShapeId* touchingShapes, int32_t maxShapes) { b2World* world = b2GetWorldFromIndex(bodyId.world); @@ -633,54 +689,6 @@ void b2Body_DestroyShape(b2ShapeId shapeId) b2DestroyShape(world, shape); } -// Destroy a shape on a body. This doesn't need to be called when destroying a body. -int32_t b2Body_DestroyShapeAndGetTouching(b2ShapeId shapeId, b2ShapeId* touchingShapes, int32_t maxShapes) -{ - b2World* world = b2GetWorldFromIndex(shapeId.world); - B2_ASSERT(world->locked == false); - if (world->locked) - { - return 0; - } - - b2Shape* shape = b2GetShape(world, shapeId); - - // Find other shapes that are currently touching this shape - b2Body* body = world->bodies + shape->bodyIndex; - int32_t contactKey = body->contactList; - int32_t reportCount = 0; - while (contactKey != B2_NULL_INDEX && reportCount < maxShapes) - { - int32_t contactIndex = contactKey >> 1; - int32_t edgeIndex = contactKey & 1; - - b2Contact* contact = world->contacts + contactIndex; - if (contact->flags & b2_contactTouchingFlag) - { - b2Shape* otherShape; - if (contact->shapeIndexA == shape->object.index) - { - otherShape = world->shapes + contact->shapeIndexB; - } - else - { - B2_ASSERT(contact->shapeIndexB == shape->object.index); - otherShape = world->shapes + contact->shapeIndexA; - } - - b2ShapeId otherShapeId = {otherShape->object.index, shapeId.world, otherShape->object.revision}; - touchingShapes[reportCount] = otherShapeId; - reportCount += 1; - } - - contactKey = contact->edges[edgeIndex].nextKey; - } - - b2DestroyShape(world, shape); - - return reportCount; -} - b2ChainId b2Body_CreateChain(b2BodyId bodyId, const b2ChainDef* def) { B2_ASSERT(b2IsValid(def->friction) && def->friction >= 0.0f); diff --git a/src/manifold.c b/src/manifold.c index 5a94f02b..7957e701 100644 --- a/src/manifold.c +++ b/src/manifold.c @@ -13,75 +13,7 @@ #include #include -#if 0 -b2WorldManifold b2ComputeWorldManifold(const b2Manifold* manifold, b2Transform xfA, float radiusA, b2Transform xfB, - float radiusB) -{ - b2WorldManifold wm = {{0.0f, 0.0f}, {{0.0f, 0.0f}, {0.0f, 0.0f}}, {0.0f, 0.0f}}; - - if (manifold->pointCount == 0) - { - return wm; - } - - switch (manifold->type) - { - case b2_manifoldCircles: - { - wm.normal = (b2Vec2){1.0f, 0.0f}; - b2Vec2 pointA = b2TransformPoint(xfA, manifold->localPoint); - b2Vec2 pointB = b2TransformPoint(xfB, manifold->points[0].localPoint); - if (b2DistanceSquared(pointA, pointB) > FLT_EPSILON * FLT_EPSILON) - { - wm.normal = b2Normalize(b2Sub(pointB, pointA)); - } - - b2Vec2 cA = b2MulAdd(pointA, radiusA, wm.normal); - b2Vec2 cB = b2MulAdd(pointB, -radiusB, wm.normal); - wm.points[0] = b2MulSV(0.5f, b2Add(cA, cB)); - wm.separations[0] = b2Dot(b2Sub(cB, cA), wm.normal); - } - break; - - case b2_manifoldFaceA: - { - wm.normal = b2RotateVector(xfA.q, manifold->localNormal); - b2Vec2 planePoint = b2TransformPoint(xfA, manifold->localPoint); - - for (int32_t i = 0; i < manifold->pointCount; ++i) - { - b2Vec2 clipPoint = b2TransformPoint(xfB, manifold->points[i].localPoint); - b2Vec2 cA = b2MulAdd(clipPoint, (radiusA - b2Dot(b2Sub(clipPoint, planePoint), wm.normal)), wm.normal); - b2Vec2 cB = b2MulAdd(clipPoint, -radiusB, wm.normal); - wm.points[i] = b2MulSV(0.5f, b2Add(cA, cB)); - wm.separations[i] = b2Dot(b2Sub(cB, cA), wm.normal); - } - } - break; - - case b2_manifoldFaceB: - { - wm.normal = b2RotateVector(xfB.q, manifold->localNormal); - b2Vec2 planePoint = b2TransformPoint(xfB, manifold->localPoint); - - for (int32_t i = 0; i < manifold->pointCount; ++i) - { - b2Vec2 clipPoint = b2TransformPoint(xfA, manifold->points[i].localPoint); - b2Vec2 cB = b2MulAdd(clipPoint, (radiusB - b2Dot(b2Sub(clipPoint, planePoint), wm.normal)), wm.normal); - b2Vec2 cA = b2MulAdd(clipPoint, -radiusA, wm.normal); - wm.points[i] = b2MulSV(0.5f, b2Add(cA, cB)); - wm.separations[i] = b2Dot(b2Sub(cA, cB), wm.normal); - } - - // Ensure normal points from A to B. - wm.normal = b2Neg(wm.normal); - } - break; - } - - return wm; -} -#endif +#define B2_MAKE_ID(A, B) ((uint8_t)(A) << 8 | (uint8_t)(B)) b2Manifold b2CollideCircles(const b2Circle* circleA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB) { @@ -942,16 +874,16 @@ b2Manifold b2CollideSmoothSegmentAndPolygon(const b2SmoothSegment* segmentA, b2T int32_t count = polygonB->count; b2Vec2 vertices[b2_maxPolygonVertices]; b2Vec2 normals[b2_maxPolygonVertices]; - //b2Vec2 sum = b2Vec2_zero; + // b2Vec2 sum = b2Vec2_zero; for (int32_t i = 0; i < count; ++i) { vertices[i] = b2TransformPoint(xf, polygonB->vertices[i]); normals[i] = b2RotateVector(xf.q, polygonB->normals[i]); - //sum = b2Add(sum, b2Sub(vertices[i], centroidB)); + // sum = b2Add(sum, b2Sub(vertices[i], centroidB)); } - //float sumLength = b2Length(sum); + // float sumLength = b2Length(sum); // Distance doesn't work correctly with partial polygons b2DistanceInput input; diff --git a/src/shape.c b/src/shape.c index 0bddfac4..a748354e 100644 --- a/src/shape.c +++ b/src/shape.c @@ -5,8 +5,11 @@ #include "body.h" #include "broad_phase.h" +#include "contact.h" #include "world.h" +#include "box2d/event_types.h" + b2AABB b2ComputeShapeAABB(const b2Shape* shape, b2Transform xf) { switch (shape->type) @@ -356,3 +359,61 @@ void b2Chain_SetRestitution(b2ChainId chainId, float restitution) shape->restitution = restitution; } } + +int32_t b2Shape_GetContactCount(b2ShapeId shapeId) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return 0; + } + + b2Shape* shape = b2GetShape(world, shapeId); + b2Body* body = world->bodies + shape->bodyIndex; + + // Conservative and fast + return body->contactCount; +} + +int32_t b2Shape_GetContactData(b2ShapeId shapeId, b2ContactData* contactData, int32_t capacity) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return 0; + } + + b2Shape* shape = b2GetShape(world, shapeId); + + b2Body* body = world->bodies + shape->bodyIndex; + int32_t contactKey = body->contactList; + int32_t index = 0; + while (contactKey != B2_NULL_INDEX && index < capacity) + { + int32_t contactIndex = contactKey >> 1; + int32_t edgeIndex = contactKey & 1; + + b2Contact* contact = world->contacts + contactIndex; + + // Does contact involve this shape and is it touching? + if ((contact->shapeIndexA == shapeId.index || contact->shapeIndexB == shapeId.index) && + (contact->flags & b2_contactTouchingFlag) != 0) + { + b2Shape* shapeA = world->shapes + contact->shapeIndexA; + b2Shape* shapeB = world->shapes + contact->shapeIndexB; + + contactData[index].shapeIdA = (b2ShapeId){shapeA->object.index, shapeId.world, shapeA->object.revision}; + contactData[index].shapeIdB = (b2ShapeId){shapeB->object.index, shapeId.world, shapeB->object.revision}; + contactData[index].manifold = contact->manifold; + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B2_ASSERT(index < capacity); + + return index; +} From f939d57ba8ebb9e983689c31a71182caa3f96360 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sat, 16 Dec 2023 15:35:09 -0800 Subject: [PATCH 08/11] CI fix --- src/body.c | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/body.c b/src/body.c index 919481b8..8a0493dd 100644 --- a/src/body.c +++ b/src/body.c @@ -795,18 +795,6 @@ void b2Body_DestroyChain(b2ChainId chainId) b2ChainShape* chain = world->chains + chainId.index; B2_ASSERT(chain->object.revision == chainId.revision); - int32_t count = chain->count; - - for (int32_t i = 0; i < count; ++i) - { - int32_t shapeIndex = chain->shapeIndices[i]; - B2_ASSERT(0 <= shapeIndex && shapeIndex < world->shapePool.count); - b2Shape* shape = world->shapes + shapeIndex; - b2DestroyShape(world, shape); - } - - b2Free(chain->shapeIndices, count * sizeof(int32_t)); - // Remove the chain from the body's singly linked list. b2Body* body = world->bodies + chain->bodyIndex; int32_t* indexPtr = &body->chainList; @@ -823,6 +811,22 @@ void b2Body_DestroyChain(b2ChainId chainId) indexPtr = &(world->chains[*indexPtr].nextIndex); } + B2_ASSERT(found == true); + if (found == false) + { + return; + } + + int32_t count = chain->count; + for (int32_t i = 0; i < count; ++i) + { + int32_t shapeIndex = chain->shapeIndices[i]; + B2_ASSERT(0 <= shapeIndex && shapeIndex < world->shapePool.count); + b2Shape* shape = world->shapes + shapeIndex; + b2DestroyShape(world, shape); + } + + b2Free(chain->shapeIndices, count * sizeof(int32_t)); b2FreeObject(&world->chainPool, &chain->object); } From d662daaa768f7916bb0f5bfc7ba20346c26b4bbc Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sat, 16 Dec 2023 20:56:06 -0800 Subject: [PATCH 09/11] contact events forces and impulses contact event sample fixed infinite loop --- include/box2d/box2d.h | 59 ++++- include/box2d/geometry.h | 2 + include/box2d/math.h | 23 +- include/box2d/types.h | 2 + samples/collection/sample_events.cpp | 334 ++++++++++++++++++++++++++- src/body.c | 201 ++++++++++++++-- src/dynamic_tree.c | 2 + src/geometry.c | 15 ++ src/graph.c | 1 + src/world.c | 20 ++ 10 files changed, 607 insertions(+), 52 deletions(-) diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index d10f908a..e38cb2ee 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -42,8 +42,19 @@ BOX2D_API b2BodyId b2World_CreateBody(b2WorldId worldId, const b2BodyDef* def); /// @warning This function is locked during callbacks. BOX2D_API void b2World_DestroyBody(b2BodyId bodyId); +/// Get sensor events for the current time step. Do not store a reference to this data. +BOX2D_API b2SensorEvents b2World_GetSensorEvents(b2WorldId worldId); +BOX2D_API b2ContactEvents b2World_GetContactEvents(b2WorldId worldId); + +BOX2D_API b2BodyType b2Body_GetType(b2BodyId bodyId); +BOX2D_API void b2Body_SetType(b2BodyId bodyId, b2BodyType type); + +/// Get the user data stored in a body +BOX2D_API void* b2Body_GetUserData(b2BodyId bodyId); + BOX2D_API b2Vec2 b2Body_GetPosition(b2BodyId bodyId); BOX2D_API float b2Body_GetAngle(b2BodyId bodyId); +BOX2D_API b2Transform b2Body_GetTransform(b2BodyId bodyId); BOX2D_API void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle); BOX2D_API b2Vec2 b2Body_GetLocalPoint(b2BodyId bodyId, b2Vec2 globalPoint); @@ -57,11 +68,42 @@ BOX2D_API float b2Body_GetAngularVelocity(b2BodyId bodyId); BOX2D_API void b2Body_SetLinearVelocity(b2BodyId bodyId, b2Vec2 linearVelocity); BOX2D_API void b2Body_SetAngularVelocity(b2BodyId bodyId, float angularVelocity); -BOX2D_API b2BodyType b2Body_GetType(b2BodyId bodyId); -BOX2D_API void b2Body_SetType(b2BodyId bodyId, b2BodyType type); - -/// Get the user data stored in a body -BOX2D_API void* b2Body_GetUserData(b2BodyId bodyId); +/// Apply a force at a world point. If the force is not +/// applied at the center of mass, it will generate a torque and +/// affect the angular velocity. This wakes up the body. +/// @param force the world force vector, usually in Newtons (N). +/// @param point the world position of the point of application. +/// @param wake also wake up the body +BOX2D_API void b2Body_ApplyForce(b2BodyId bodyId, b2Vec2 force, b2Vec2 point, bool wake); + +/// Apply a force to the center of mass. This wakes up the body. +/// @param force the world force vector, usually in Newtons (N). +/// @param wake also wake up the body +BOX2D_API void b2Body_ApplyForceToCenter(b2BodyId bodyId, b2Vec2 force, bool wake); + +/// Apply a torque. This affects the angular velocity +/// without affecting the linear velocity of the center of mass. +/// @param torque about the z-axis (out of the screen), usually in N-m. +/// @param wake also wake up the body +BOX2D_API void b2Body_ApplyTorque(b2BodyId bodyId, float torque, bool wake); + +/// Apply an impulse at a point. This immediately modifies the velocity. +/// It also modifies the angular velocity if the point of application +/// is not at the center of mass. This wakes up the body. +/// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. +/// @param point the world position of the point of application. +/// @param wake also wake up the body +BOX2D_API void b2Body_ApplyLinearImpulse(b2BodyId bodyId, b2Vec2 impulse, b2Vec2 point, bool wake); + +/// Apply an impulse to the center of mass. This immediately modifies the velocity. +/// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. +/// @param wake also wake up the body +BOX2D_API void b2Body_ApplyLinearImpulseToCenter(b2BodyId bodyId, b2Vec2 impulse, bool wake); + +/// Apply an angular impulse. +/// @param impulse the angular impulse in units of kg*m*m/s +/// @param wake also wake up the body +BOX2D_API void b2Body_ApplyAngularImpulse(b2BodyId bodyId, float impulse, bool wake); /// Get the mass of the body (kilograms) BOX2D_API float b2Body_GetMass(b2BodyId bodyId); @@ -81,7 +123,7 @@ BOX2D_API b2Vec2 b2Body_GetWorldCenterOfMass(b2BodyId bodyId); BOX2D_API void b2Body_SetMassData(b2BodyId bodyId, b2MassData massData); /// Is this body awake? -BOX2D_API void b2Body_IsAwake(b2BodyId bodyId); +BOX2D_API bool b2Body_IsAwake(b2BodyId bodyId); /// Wake a body from sleep. This wakes the entire island the body is touching. BOX2D_API void b2Body_Wake(b2BodyId bodyId); @@ -209,11 +251,6 @@ BOX2D_API void b2World_CapsuleCast(b2WorldId worldId, const b2Capsule* capsule, BOX2D_API void b2World_PolygonCast(b2WorldId worldId, const b2Polygon* polygon, b2Transform originTransform, b2Vec2 translation, b2QueryFilter filter, b2RayResultFcn* fcn, void* context); -/// World events - -/// Get sensor events for the current time step. Do not store a reference to this data. -BOX2D_API b2SensorEvents b2World_GetSensorEvents(b2WorldId worldId); - /// Id validation. These allow validation for up 64K allocations. BOX2D_API bool b2World_IsValid(b2WorldId id); BOX2D_API bool b2Body_IsValid(b2BodyId id); diff --git a/include/box2d/geometry.h b/include/box2d/geometry.h index 4e22f6ac..c3d96c46 100644 --- a/include/box2d/geometry.h +++ b/include/box2d/geometry.h @@ -91,6 +91,8 @@ BOX2D_API b2Polygon b2MakeRoundedBox(float hx, float hy, float radius); BOX2D_API b2Polygon b2MakeOffsetBox(float hx, float hy, b2Vec2 center, float angle); BOX2D_API b2Polygon b2MakeCapsule(b2Vec2 p1, b2Vec2 p2, float radius); +BOX2D_API b2Polygon b2TransformPolygon(b2Transform transform, const b2Polygon* polygon); + /// Compute mass properties BOX2D_API b2MassData b2ComputeCircleMass(const b2Circle* shape, float density); BOX2D_API b2MassData b2ComputeCapsuleMass(const b2Capsule* shape, float density); diff --git a/include/box2d/math.h b/include/box2d/math.h index 73616a05..c774f292 100644 --- a/include/box2d/math.h +++ b/include/box2d/math.h @@ -3,6 +3,7 @@ #pragma once +#include "api.h" #include "types.h" #include @@ -24,9 +25,6 @@ static const b2Transform b2Transform_identity = {{0.0f, 0.0f}, {0.0f, 1.0f}}; static const b2Mat22 b2Mat22_zero = {{0.0f, 0.0f}, {0.0f, 0.0f}}; static const b2Mat33 b2Mat33_zero = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}; -bool b2IsValid(float a); -bool b2IsValidVec2(b2Vec2 v); - /// Make a vector static inline b2Vec2 b2MakeVec2(float x, float y) { @@ -167,14 +165,6 @@ static inline b2Vec2 b2Clamp(b2Vec2 v, b2Vec2 a, b2Vec2 b) return c; } -/// Convert this vector into a unit vector -b2Vec2 b2Normalize(b2Vec2 v); - -/// This asserts of the vector is too short -b2Vec2 b2NormalizeChecked(b2Vec2 v); - -b2Vec2 b2GetLengthAndNormalize(float* length, b2Vec2 v); - /// Get the length of this vector (the norm). static inline float b2Length(b2Vec2 v) { @@ -417,3 +407,14 @@ static inline b2Vec3 b2Solve33(b2Mat33 A, b2Vec3 b) #ifdef __cplusplus } #endif + +BOX2D_API bool b2IsValid(float a); +BOX2D_API bool b2IsValidVec2(b2Vec2 v); + +/// Convert this vector into a unit vector +BOX2D_API b2Vec2 b2Normalize(b2Vec2 v); + +/// This asserts of the vector is too short +BOX2D_API b2Vec2 b2NormalizeChecked(b2Vec2 v); + +BOX2D_API b2Vec2 b2GetLengthAndNormalize(float* length, b2Vec2 v); diff --git a/include/box2d/types.h b/include/box2d/types.h index 9920a54d..66242180 100644 --- a/include/box2d/types.h +++ b/include/box2d/types.h @@ -349,6 +349,8 @@ typedef struct b2ChainDef b2Filter filter; } b2ChainDef; +static const b2ChainDef b2_defaultChainDef = {NULL, 0, false, NULL, 0.6f, 0.0f, {0x00000001, 0xFFFFFFFF, 0}}; + /// Make a world definition with default values. static inline b2WorldDef b2DefaultWorldDef(void) { diff --git a/samples/collection/sample_events.cpp b/samples/collection/sample_events.cpp index a015e087..8b4bd035 100644 --- a/samples/collection/sample_events.cpp +++ b/samples/collection/sample_events.cpp @@ -7,10 +7,9 @@ #include "box2d/box2d.h" #include "box2d/geometry.h" #include "box2d/hull.h" - -// #include #include "box2d/math.h" +#include #include #define SIDES 7 @@ -22,7 +21,7 @@ struct Ring bool valid; }; -class Sensor : public Sample +class SensorEvent : public Sample { public: enum @@ -30,7 +29,7 @@ class Sensor : public Sample e_count = 16 }; - Sensor(const Settings& settings) + SensorEvent(const Settings& settings) : Sample(settings) { { @@ -117,7 +116,7 @@ class Sensor : public Sample b2BodyDef bodyDef = b2_defaultBodyDef; bodyDef.position = {0.0f, y}; bodyDef.type = b2_dynamicBody; - + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); b2Polygon box = b2MakeBox(6.0f, 0.5f); @@ -264,7 +263,7 @@ class Sensor : public Sample for (int i = 0; i < sensorEvents.beginCount; ++i) { b2SensorBeginTouchEvent event = sensorEvents.beginEvents[i]; - b2ShapeId visitorId = sensorEvents.beginEvents[i].visitorShapeId; + b2ShapeId visitorId = event.visitorShapeId; b2BodyId ringBodyId = b2Shape_GetBody(visitorId); Ring* ring = (Ring*)b2Body_GetUserData(ringBodyId); if (ring != nullptr && ring->valid) @@ -299,7 +298,7 @@ class Sensor : public Sample static Sample* Create(const Settings& settings) { - return new Sensor(settings); + return new SensorEvent(settings); } Ring m_rings[e_count]; @@ -307,4 +306,323 @@ class Sensor : public Sample float m_side; }; -static int sampleSensor = RegisterSample("Events", "Sensor", Sensor::Create); +static int sampleSensorEvent = RegisterSample("Events", "Sensor", SensorEvent::Create); + +struct BodyUserData +{ + int index; +}; + +class ContactEvent : public Sample +{ +public: + enum + { + e_count = 20 + }; + + ContactEvent(const Settings& settings) + : Sample(settings) + { + if (settings.restart == false) + { + g_camera.m_center = {0.0f, 0.0f}; + g_camera.m_zoom = 1.75f; + } + + { + b2BodyId groundId = b2World_CreateBody(m_worldId, &b2_defaultBodyDef); + + b2Vec2 points[] = {{40.0f, -40.0f}, {-40.0f, -40.0f}, {-40.0f, 40.0f}, {40.0f, 40.0f}}; + + b2ChainDef chainDef = b2_defaultChainDef; + chainDef.count = 4; + chainDef.points = points; + chainDef.loop = true; + + b2Body_CreateChain(groundId, &chainDef); + } + + // Player + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.gravityScale = 0.0f; + bodyDef.linearDamping = 0.5f; + bodyDef.angularDamping = 0.5f; + m_playerId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Circle circle = {{0.0f, 0.0f}, 1.0f}; + b2ShapeDef shapeDef = b2_defaultShapeDef; + + // Enable contact events for the player shape + shapeDef.enableContactEvents = true; + + m_coreShapeId = b2Body_CreateCircle(m_playerId, &shapeDef, &circle); + } + + for (int i = 0; i < e_count; ++i) + { + m_debrisIds[i] = b2_nullBodyId; + m_bodyUserData[i].index = i; + } + + m_wait = 0.5f; + m_force = 100.0f; + } + + void SpawnDebris() + { + int index = -1; + for (int i = 0; i < e_count; ++i) + { + if (B2_IS_NULL(m_debrisIds[i])) + { + index = i; + break; + } + } + + if (index == -1) + { + return; + } + + // Debris + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = {RandomFloat(-38.0f, 38.0f), RandomFloat(-38.0f, 38.0f)}; + bodyDef.angle = RandomFloat(-b2_pi, b2_pi); + bodyDef.gravityScale = 0.0f; + bodyDef.linearDamping = 0.5f; + bodyDef.angularDamping = 0.5f; + bodyDef.userData = m_bodyUserData + index; + m_debrisIds[index] = b2World_CreateBody(m_worldId, &bodyDef); + + b2ShapeDef shapeDef = b2_defaultShapeDef; + + // No events when debris hits debris + shapeDef.enableContactEvents = false; + + if ((index + 1) % 3 == 0) + { + b2Circle circle = {{0.0f, 0.0f}, 0.5f}; + b2Body_CreateCircle(m_debrisIds[index], &shapeDef, &circle); + } + else if ((index + 1) % 2 == 0) + { + b2Capsule capsule = {{0.0f, -0.25f}, {0.0f, 0.25f}, 0.25f}; + b2Body_CreateCapsule(m_debrisIds[index], &shapeDef, &capsule); + } + else + { + b2Polygon box = b2MakeBox(0.4f, 0.6f); + b2Body_CreatePolygon(m_debrisIds[index], &shapeDef, &box); + } + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 400.0f)); + ImGui::SetNextWindowSize(ImVec2(200.0f, 60.0f)); + ImGui::Begin("Sample Controls", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize); + + ImGui::SliderFloat("force", &m_force, 100.0f, 500.0f, "%.1f"); + + ImGui::End(); + } + + void Step(Settings& settings) override + { + if (glfwGetKey(g_mainWindow, GLFW_KEY_A) == GLFW_PRESS) + { + b2Body_ApplyForceToCenter(m_playerId, {-m_force, 0.0f}, true); + } + + if (glfwGetKey(g_mainWindow, GLFW_KEY_D) == GLFW_PRESS) + { + b2Body_ApplyForceToCenter(m_playerId, {m_force, 0.0f}, true); + } + + if (glfwGetKey(g_mainWindow, GLFW_KEY_W) == GLFW_PRESS) + { + b2Body_ApplyForceToCenter(m_playerId, {0.0f, m_force}, true); + } + + if (glfwGetKey(g_mainWindow, GLFW_KEY_S) == GLFW_PRESS) + { + b2Body_ApplyForceToCenter(m_playerId, {0.0f, -m_force}, true); + } + + Sample::Step(settings); + + // Discover rings that touch the bottom sensor + int debrisToAttach[e_count] = {}; + b2ShapeId shapesToDestroy[e_count] = {b2_nullShapeId}; + int attachCount = 0; + int destroyCount = 0; + + b2ContactEvents contactEvents = b2World_GetContactEvents(m_worldId); + for (int i = 0; i < contactEvents.beginCount; ++i) + { + b2ContactBeginTouchEvent event = contactEvents.beginEvents[i]; + b2BodyId bodyIdA = b2Shape_GetBody(event.shapeIdA); + b2BodyId bodyIdB = b2Shape_GetBody(event.shapeIdB); + + if (B2_ID_EQUALS(bodyIdA, m_playerId)) + { + BodyUserData* userDataB = static_cast(b2Body_GetUserData(bodyIdB)); + if (userDataB == nullptr) + { + if (B2_ID_EQUALS(event.shapeIdA, m_coreShapeId) == false && destroyCount < e_count) + { + // player non-core shape hit the wall + + bool found = false; + for (int j = 0; j < destroyCount; ++j) + { + if (B2_ID_EQUALS(event.shapeIdA, shapesToDestroy[j])) + { + found = true; + break; + } + } + + // avoid double deletion + if (found == false) + { + shapesToDestroy[destroyCount] = event.shapeIdA; + destroyCount += 1; + } + } + } + else if (attachCount < e_count) + { + debrisToAttach[attachCount] = userDataB->index; + attachCount += 1; + } + } + else + { + // Only expect events for the player + assert(B2_ID_EQUALS(bodyIdB, m_playerId)); + BodyUserData* userDataA = static_cast(b2Body_GetUserData(bodyIdA)); + if (userDataA == nullptr) + { + if (B2_ID_EQUALS(event.shapeIdB, m_coreShapeId) == false && destroyCount < e_count) + { + // player non-core shape hit the wall + + bool found = false; + for (int j = 0; j < destroyCount; ++j) + { + if (B2_ID_EQUALS(event.shapeIdB, shapesToDestroy[j])) + { + found = true; + break; + } + } + + // avoid double deletion + if (found == false) + { + shapesToDestroy[destroyCount] = event.shapeIdB; + destroyCount += 1; + } + } + } + else if (attachCount < e_count) + { + debrisToAttach[attachCount] = userDataA->index; + attachCount += 1; + } + } + } + + // Attach debris to player body + for (int i = 0; i < attachCount; ++i) + { + int index = debrisToAttach[i]; + b2BodyId debrisId = m_debrisIds[index]; + if (B2_IS_NULL(debrisId)) + { + continue; + } + + b2Transform playerTransform = b2Body_GetTransform(m_playerId); + b2Transform debrisTransform = b2Body_GetTransform(debrisId); + b2Transform relativeTransform = b2InvMulTransforms(playerTransform, debrisTransform); + + b2ShapeId shapeId = b2Body_GetFirstShape(debrisId); + b2ShapeType type = b2Shape_GetType(shapeId); + + b2ShapeDef shapeDef = b2_defaultShapeDef; + shapeDef.enableContactEvents = true; + + switch (type) + { + case b2_circleShape: + { + b2Circle circle = *b2Shape_GetCircle(shapeId); + circle.point = b2TransformPoint(relativeTransform, circle.point); + + b2Body_CreateCircle(m_playerId, &shapeDef, &circle); + } + break; + + case b2_capsuleShape: + { + b2Capsule capsule = *b2Shape_GetCapsule(shapeId); + capsule.point1 = b2TransformPoint(relativeTransform, capsule.point1); + capsule.point2 = b2TransformPoint(relativeTransform, capsule.point2); + + b2Body_CreateCapsule(m_playerId, &shapeDef, &capsule); + } + break; + + case b2_polygonShape: + { + b2Polygon polygon = b2TransformPolygon(relativeTransform, b2Shape_GetPolygon(shapeId)); + + b2Body_CreatePolygon(m_playerId, &shapeDef, &polygon); + } + break; + + default: + assert(false); + } + + b2World_DestroyBody(debrisId); + m_debrisIds[index] = b2_nullBodyId; + } + + for (int i = 0; i < destroyCount; ++i) + { + b2Body_DestroyShape(shapesToDestroy[i]); + } + + if (settings.hertz > 0.0f && settings.pause == false) + { + m_wait -= 1.0f / settings.hertz; + if (m_wait < 0.0f) + { + SpawnDebris(); + m_wait += 0.5f; + } + } + } + + static Sample* Create(const Settings& settings) + { + return new ContactEvent(settings); + } + + b2BodyId m_playerId; + b2ShapeId m_coreShapeId; + b2BodyId m_debrisIds[e_count]; + BodyUserData m_bodyUserData[e_count]; + float m_force; + float m_wait; +}; + +static int sampleWeeble = RegisterSample("Events", "Contact", ContactEvent::Create); diff --git a/src/body.c b/src/body.c index 8a0493dd..aa5d046a 100644 --- a/src/body.c +++ b/src/body.c @@ -270,6 +270,30 @@ b2Body* b2GetBody(b2World* world, b2BodyId id) return body; } +bool b2IsBodyAwake(b2World* world, b2Body* body) +{ + if (body->islandIndex != B2_NULL_INDEX) + { + b2Island* island = world->islands + body->islandIndex; + return island->awakeIndex != B2_NULL_INDEX; + } + + return false; +} + +void b2WakeBody(b2World* world, b2Body* body) +{ + if (body->islandIndex != B2_NULL_INDEX) + { + int32_t islandIndex = body->islandIndex; + B2_ASSERT(0 <= islandIndex && islandIndex < world->islandPool.capacity); + b2WakeIsland(world->islands + islandIndex); + return; + } + + B2_ASSERT(body->type == b2_staticBody); +} + void b2DestroyBody(b2World* world, b2Body* body) { // User must destroy joints before destroying bodies @@ -681,10 +705,7 @@ void b2Body_DestroyShape(b2ShapeId shapeId) return; } - B2_ASSERT(0 <= shapeId.index && shapeId.index < world->shapePool.count); - - b2Shape* shape = world->shapes + shapeId.index; - B2_ASSERT(shape->object.revision == shapeId.revision); + b2Shape* shape = b2GetShape(world, shapeId); b2DestroyShape(world, shape); } @@ -830,18 +851,6 @@ void b2Body_DestroyChain(b2ChainId chainId) b2FreeObject(&world->chainPool, &chain->object); } -bool b2IsBodyAwake(b2World* world, b2Body* body) -{ - if (body->islandIndex != B2_NULL_INDEX) - { - b2Island* island = world->islands + body->islandIndex; - return island->awakeIndex != B2_NULL_INDEX; - } - - B2_ASSERT(body->type == b2_staticBody); - return false; -} - b2Vec2 b2Body_GetPosition(b2BodyId bodyId) { b2World* world = b2GetWorldFromIndex(bodyId.world); @@ -852,8 +861,15 @@ b2Vec2 b2Body_GetPosition(b2BodyId bodyId) float b2Body_GetAngle(b2BodyId bodyId) { b2World* world = b2GetWorldFromIndex(bodyId.world); - B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity); - return world->bodies[bodyId.index].angle; + b2Body* body = b2GetBody(world, bodyId); + return body->angle; +} + +b2Transform b2Body_GetTransform(b2BodyId bodyId) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + return body->transform; } b2Vec2 b2Body_GetLocalPoint(b2BodyId bodyId, b2Vec2 globalPoint) @@ -938,6 +954,11 @@ void b2Body_SetLinearVelocity(b2BodyId bodyId, b2Vec2 linearVelocity) { b2World* world = b2GetWorldFromIndex(bodyId.world); b2Body* body = b2GetBody(world, bodyId); + if (body->type == b2_staticBody || body->isEnabled == false) + { + return; + } + body->linearVelocity = linearVelocity; } @@ -945,7 +966,139 @@ void b2Body_SetAngularVelocity(b2BodyId bodyId, float angularVelocity) { b2World* world = b2GetWorldFromIndex(bodyId.world); b2Body* body = b2GetBody(world, bodyId); + if (body->type == b2_staticBody || body->isEnabled == false) + { + return; + } + body->angularVelocity = angularVelocity; + + if (angularVelocity != 0.0f) + { + b2WakeBody(world, body); + } +} + +void b2Body_ApplyForce(b2BodyId bodyId, b2Vec2 force, b2Vec2 point, bool wake) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + if (body->type == b2_staticBody || body->isEnabled == false) + { + return; + } + + if (wake) + { + b2WakeBody(world, body); + } + + if (b2IsBodyAwake(world, body)) + { + body->force = b2Add(body->force, force); + body->torque += b2Cross(b2Sub(point, body->position), force); + } +} + +void b2Body_ApplyForceToCenter(b2BodyId bodyId, b2Vec2 force, bool wake) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + if (body->type == b2_staticBody || body->isEnabled == false) + { + return; + } + + if (wake) + { + b2WakeBody(world, body); + } + + if (b2IsBodyAwake(world, body)) + { + body->force = b2Add(body->force, force); + } +} + +void b2Body_ApplyTorque(b2BodyId bodyId, float torque, bool wake) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + if (body->type == b2_staticBody || body->isEnabled == false) + { + return; + } + + if (wake) + { + b2WakeBody(world, body); + } + + if (b2IsBodyAwake(world, body)) + { + body->torque += torque; + } +} + +void b2Body_ApplyLinearImpulse(b2BodyId bodyId, b2Vec2 impulse, b2Vec2 point, bool wake) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + if (body->type == b2_staticBody || body->isEnabled == false) + { + return; + } + + if (wake) + { + b2WakeBody(world, body); + } + + if (b2IsBodyAwake(world, body)) + { + body->linearVelocity = b2MulAdd(body->linearVelocity, body->invMass, impulse); + body->angularVelocity += body->invI * b2Cross(b2Sub(point, body->position), impulse); + } +} + +void b2Body_ApplyLinearImpulseToCenter(b2BodyId bodyId, b2Vec2 impulse, bool wake) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + if (body->type == b2_staticBody || body->isEnabled == false) + { + return; + } + + if (wake) + { + b2WakeBody(world, body); + } + + if (b2IsBodyAwake(world, body)) + { + body->linearVelocity = b2MulAdd(body->linearVelocity, body->invMass, impulse); + } +} + +void b2Body_ApplyAngularImpulse(b2BodyId bodyId, float impulse, bool wake) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + if (body->type == b2_staticBody || body->isEnabled == false) + { + return; + } + + if (wake) + { + b2WakeBody(world, body); + } + + if (b2IsBodyAwake(world, body)) + { + body->angularVelocity += impulse; + } } b2BodyType b2Body_GetType(b2BodyId bodyId) @@ -1032,6 +1185,13 @@ void b2Body_SetMassData(b2BodyId bodyId, b2MassData massData) body->invI = body->I > 0.0f ? 1.0f / body->I : 0.0f; } +bool b2Body_IsAwake(b2BodyId bodyId) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + return b2IsBodyAwake(world, body); +} + void b2Body_Wake(b2BodyId bodyId) { b2World* world = b2GetWorldFromIndex(bodyId.world); @@ -1041,10 +1201,7 @@ void b2Body_Wake(b2BodyId bodyId) return; } - int32_t islandIndex = body->islandIndex; - B2_ASSERT(0 <= islandIndex && islandIndex < world->islandPool.capacity); - - b2WakeIsland(world->islands + islandIndex); + b2WakeBody(world, body); } bool b2Body_IsEnabled(b2BodyId bodyId) diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c index 93a10435..8cab5f22 100644 --- a/src/dynamic_tree.c +++ b/src/dynamic_tree.c @@ -1920,6 +1920,8 @@ int32_t b2DynamicTree_Rebuild(b2DynamicTree* tree, bool fullBuild) // Gather all proxy nodes that have grown and all internal nodes that haven't grown. Both are // considered leaves in the tree rebuild. // Free all internal nodes that have grown. + // todo use a node growth metric instead of simply enlarged to reduce rebuild size and frequency + // this should be weighed against b2_aabbMargin while (true) { if (node->height == 0 || (node->enlarged == false && fullBuild == false)) diff --git a/src/geometry.c b/src/geometry.c index fb0cd6c8..a2f49b20 100644 --- a/src/geometry.c +++ b/src/geometry.c @@ -181,6 +181,21 @@ b2Polygon b2MakeCapsule(b2Vec2 p1, b2Vec2 p2, float radius) return shape; } +b2Polygon b2TransformPolygon(b2Transform transform, const b2Polygon* polygon) +{ + b2Polygon p = *polygon; + + for (int i = 0; i < p.count; ++i) + { + p.vertices[i] = b2TransformPoint(transform, p.vertices[i]); + p.normals[i] = b2RotateVector(transform.q, p.normals[i]); + } + + p.centroid = b2TransformPoint(transform, p.centroid); + + return p; +} + b2MassData b2ComputeCircleMass(const b2Circle* shape, float density) { float rr = shape->radius * shape->radius; diff --git a/src/graph.c b/src/graph.c index 00b8dcee..869aaa43 100644 --- a/src/graph.c +++ b/src/graph.c @@ -2235,6 +2235,7 @@ void b2Solve(b2World* world, b2StepContext* context) b2Shape* shape = shapes + shapeIndex; if (shape->enlargedAABB == false) { + shapeIndex = shape->nextShapeIndex; continue; } diff --git a/src/world.c b/src/world.c index 377450f9..2a461e61 100644 --- a/src/world.c +++ b/src/world.c @@ -902,6 +902,23 @@ b2SensorEvents b2World_GetSensorEvents(b2WorldId worldId) return events; } +b2ContactEvents b2World_GetContactEvents(b2WorldId worldId) +{ + b2World* world = b2GetWorldFromId(worldId); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return (b2ContactEvents){0}; + } + + int beginCount = b2Array(world->contactBeginArray).count; + int endCount = b2Array(world->contactEndArray).count; + + b2ContactEvents events = {world->contactBeginArray, world->contactEndArray, beginCount, endCount}; + return events; + +} + bool b2World_IsValid(b2WorldId id) { if (id.index < 0 || b2_maxWorlds <= id.index) @@ -1336,6 +1353,9 @@ void b2World_RayCast(b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2Que } b2RayCastInput input = {origin, translation, 1.0f}; + + // todo validate input + WorldRayCastContext worldContext = {world, fcn, filter, 1.0f, context}; for (int32_t i = 0; i < b2_bodyTypeCount; ++i) From 08fa53645e1427cb9370f466eb7976377be110f1 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sat, 16 Dec 2023 22:00:31 -0800 Subject: [PATCH 10/11] fix dynamic tree test clean up --- include/box2d/api.h | 15 +++------------ include/box2d/box2d.h | 10 ---------- include/box2d/id.h | 10 ++++++++++ samples/collection/sample_bodies.cpp | 1 + samples/collection/sample_dynamic_tree.cpp | 6 +++--- samples/collection/sample_events.cpp | 3 +++ 6 files changed, 20 insertions(+), 25 deletions(-) diff --git a/include/box2d/api.h b/include/box2d/api.h index 49cb3d4b..73ae18be 100644 --- a/include/box2d/api.h +++ b/include/box2d/api.h @@ -30,19 +30,10 @@ typedef void b2FreeFcn(void* mem); // Return 0 to typedef int b2AssertFcn(const char* condition, const char* fileName, int lineNumber); -#ifdef __cplusplus -extern "C" -{ -#endif - /// Default allocation functions -void b2SetAllocator(b2AllocFcn* allocFcn, b2FreeFcn* freeFcn); +BOX2D_API void b2SetAllocator(b2AllocFcn* allocFcn, b2FreeFcn* freeFcn); /// Total bytes allocated by Box2D -uint32_t b2GetByteCount(void); +BOX2D_API uint32_t b2GetByteCount(void); -extern b2AssertFcn* Box2DAssertCallback; - -#ifdef __cplusplus -} -#endif +BOX2D_API b2AssertFcn* Box2DAssertCallback; diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index e38cb2ee..f554cbbb 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -145,9 +145,6 @@ BOX2D_API b2ShapeId b2Body_CreateCapsule(b2BodyId bodyId, const b2ShapeDef* def, BOX2D_API b2ShapeId b2Body_CreatePolygon(b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon); BOX2D_API void b2Body_DestroyShape(b2ShapeId shapeId); -/// Destroy a shape from a body and get an array of all the other shapes this shape was touching. -BOX2D_API int32_t b2Body_GetTouching(b2ShapeId shapeId, b2ShapeId* touchingShapes, int32_t maxShapes); - BOX2D_API b2ChainId b2Body_CreateChain(b2BodyId bodyId, const b2ChainDef* def); BOX2D_API void b2Body_DestroyChain(b2ChainId chainId); @@ -251,13 +248,6 @@ BOX2D_API void b2World_CapsuleCast(b2WorldId worldId, const b2Capsule* capsule, BOX2D_API void b2World_PolygonCast(b2WorldId worldId, const b2Polygon* polygon, b2Transform originTransform, b2Vec2 translation, b2QueryFilter filter, b2RayResultFcn* fcn, void* context); -/// Id validation. These allow validation for up 64K allocations. -BOX2D_API bool b2World_IsValid(b2WorldId id); -BOX2D_API bool b2Body_IsValid(b2BodyId id); -BOX2D_API bool b2Shape_IsValid(b2ShapeId id); -BOX2D_API bool b2Chain_IsValid(b2ChainId id); -BOX2D_API bool b2Joint_IsValid(b2JointId id); - /// Advanced API for testing and special cases /// Enable/disable sleep. diff --git a/include/box2d/id.h b/include/box2d/id.h index 69a8e9cd..23719e7b 100644 --- a/include/box2d/id.h +++ b/include/box2d/id.h @@ -3,6 +3,9 @@ #pragma once +#include "api.h" + +#include #include /// These ids serve as handles to internal Box2D objects. These should be considered opaque data and passed by value. @@ -67,3 +70,10 @@ static const b2ChainId b2_nullChainId = {-1, -1, 0}; // Compare two ids for equality. Doesn't work for b2WorldId./ #define B2_ID_EQUALS(id1, id2) (id1.index == id2.index && id1.world == id2.world && id1.revision == id2.revision) + +/// Id validation. These allow validation for up 64K allocations. +BOX2D_API bool b2World_IsValid(b2WorldId id); +BOX2D_API bool b2Body_IsValid(b2BodyId id); +BOX2D_API bool b2Shape_IsValid(b2ShapeId id); +BOX2D_API bool b2Chain_IsValid(b2ChainId id); +BOX2D_API bool b2Joint_IsValid(b2JointId id); diff --git a/samples/collection/sample_bodies.cpp b/samples/collection/sample_bodies.cpp index fad64208..f798884b 100644 --- a/samples/collection/sample_bodies.cpp +++ b/samples/collection/sample_bodies.cpp @@ -361,6 +361,7 @@ class Weeble : public Sample b2BodyDef bodyDef = b2_defaultBodyDef; bodyDef.type = b2_dynamicBody; bodyDef.position = {0.0f, 3.0f}; + bodyDef.angle = 0.25f * b2_pi; m_weebleId = b2World_CreateBody(m_worldId, &bodyDef); b2Capsule capsule = {{0.0f, -1.0f}, {0.0f, 1.0f}, 1.0f}; diff --git a/samples/collection/sample_dynamic_tree.cpp b/samples/collection/sample_dynamic_tree.cpp index 6e4e75f2..9430dde6 100644 --- a/samples/collection/sample_dynamic_tree.cpp +++ b/samples/collection/sample_dynamic_tree.cpp @@ -251,8 +251,8 @@ class DynamicTree : public Sample g_draw.DrawAABB(box, {1.0f, 1.0f, 1.0f, 1.0f}); } - // m_startPoint = {-42.0f, -6.0f}; - // m_endPoint = {-38.0f, -2.0f}; + //m_startPoint = {-1.0f, 0.5f}; + //m_endPoint = {7.0f, 0.5f}; if (m_rayDrag) { @@ -299,7 +299,7 @@ class DynamicTree : public Sample if (b2AABB_Contains(p->fatBox, p->box) == false) { p->fatBox.lowerBound = b2Sub(p->box.lowerBound, aabbMargin); - p->fatBox.upperBound = b2Add(p->box.lowerBound, aabbMargin); + p->fatBox.upperBound = b2Add(p->box.upperBound, aabbMargin); p->moved = true; } else diff --git a/samples/collection/sample_events.cpp b/samples/collection/sample_events.cpp index 8b4bd035..8c13d1d0 100644 --- a/samples/collection/sample_events.cpp +++ b/samples/collection/sample_events.cpp @@ -434,6 +434,9 @@ class ContactEvent : public Sample void Step(Settings& settings) override { + g_draw.DrawString(5, m_textLine, "move using WASD"); + m_textLine += m_textIncrement; + if (glfwGetKey(g_mainWindow, GLFW_KEY_A) == GLFW_PRESS) { b2Body_ApplyForceToCenter(m_playerId, {-m_force, 0.0f}, true); From f1f2a7a9bc788032c8ecbeccee5c7d7a458255aa Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sat, 16 Dec 2023 22:04:32 -0800 Subject: [PATCH 11/11] CI fix --- include/box2d/api.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/include/box2d/api.h b/include/box2d/api.h index 73ae18be..7460e6d5 100644 --- a/include/box2d/api.h +++ b/include/box2d/api.h @@ -36,4 +36,9 @@ BOX2D_API void b2SetAllocator(b2AllocFcn* allocFcn, b2FreeFcn* freeFcn); /// Total bytes allocated by Box2D BOX2D_API uint32_t b2GetByteCount(void); -BOX2D_API b2AssertFcn* Box2DAssertCallback; +#ifdef __cplusplus +extern "C" b2AssertFcn* Box2DAssertCallback; +#else +extern b2AssertFcn* Box2DAssertCallback; +#endif +