From 0076e587d16bd089741a428f727b676b44e91330 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sat, 24 Feb 2024 18:07:27 -0800 Subject: [PATCH] Many more API functions (#119) #83, #95, #100, #101, #102, #103, #104, #110 --- include/box2d/api.h | 37 +- include/box2d/box2d.h | 251 +++++++++-- include/box2d/callbacks.h | 50 ++- include/box2d/color.h | 20 +- include/box2d/debug_draw.h | 2 +- include/box2d/distance.h | 4 +- include/box2d/dynamic_tree.h | 11 +- include/box2d/geometry.h | 47 ++- include/box2d/joint_types.h | 83 ++-- include/box2d/joint_util.h | 15 - include/box2d/math.h | 98 ++--- include/box2d/math_cpp.h | 2 +- include/box2d/math_types.h | 47 +++ include/box2d/types.h | 197 ++------- samples/collection/donut.cpp | 4 +- samples/collection/sample_dynamic_tree.cpp | 2 +- samples/collection/sample_events.cpp | 7 +- samples/collection/sample_joints.cpp | 77 +++- samples/collection/sample_manifold.cpp | 25 +- samples/collection/sample_ray_cast.cpp | 34 +- samples/collection/sample_robustness.cpp | 1 - samples/collection/sample_shape_cast.cpp | 2 +- samples/collection/sample_shapes.cpp | 183 +++++++- samples/sample.cpp | 32 +- samples/settings.h | 2 +- src/CMakeLists.txt | 14 +- src/aabb.c | 5 +- src/aabb.h | 4 +- src/body.c | 82 +++- src/body.h | 3 + src/broad_phase.c | 6 +- src/broad_phase.h | 2 + src/constraint_graph.c | 401 ++++++++++++++++++ src/{graph.h => constraint_graph.h} | 2 - src/contact.c | 3 + src/contact_solver.c | 4 +- src/contact_solver.h | 7 +- src/core.h | 4 +- src/distance.c | 6 +- src/distance_joint.c | 58 ++- src/geometry.c | 34 +- src/implementation.c | 8 + src/island.c | 4 +- src/joint.c | 351 +++++++++++----- src/joint.h | 7 +- src/math.c | 15 + src/motor_joint.c | 87 ++-- src/mouse_joint.c | 27 +- src/pool.h | 2 +- src/prismatic_joint.c | 160 ++++--- src/revolute_joint.c | 138 ++++--- src/shape.c | 204 +++++++-- src/shape.h | 4 +- src/{graph.c => solver.c} | 412 +------------------ src/{solver_data.h => solver.h} | 4 +- src/{arena_allocator.c => stack_allocator.c} | 2 +- src/{arena_allocator.h => stack_allocator.h} | 0 src/types.c | 59 +++ src/weld_joint.c | 87 ++-- src/wheel_joint.c | 215 ++++++---- src/world.c | 80 ++-- src/world.h | 2 +- test/test_collision.c | 2 +- test/test_distance.c | 2 +- test/test_shape.c | 6 +- 65 files changed, 2403 insertions(+), 1343 deletions(-) delete mode 100644 include/box2d/joint_util.h create mode 100644 include/box2d/math_types.h create mode 100644 src/constraint_graph.c rename src/{graph.h => constraint_graph.h} (96%) create mode 100644 src/implementation.c rename src/{graph.c => solver.c} (81%) rename src/{solver_data.h => solver.h} (97%) rename src/{arena_allocator.c => stack_allocator.c} (99%) rename src/{arena_allocator.h => stack_allocator.h} (100%) diff --git a/include/box2d/api.h b/include/box2d/api.h index d2008681..9ac97c1f 100644 --- a/include/box2d/api.h +++ b/include/box2d/api.h @@ -6,23 +6,46 @@ #include #if defined(_WIN32) && defined(box2d_EXPORTS) - // building the Box2D DLL + // build the Windows DLL #define BOX2D_EXPORT __declspec(dllexport) #elif defined(_WIN32) && defined(BOX2D_DLL) - // using the Box2D DLL + // using the Windows DLL #define BOX2D_EXPORT __declspec(dllimport) -#elif defined(__GNUC__) && defined(box2d_EXPORTS) - // building the Box2D shared library +#elif defined(box2d_EXPORTS) + // building or using the Box2D shared library #define BOX2D_EXPORT __attribute__((visibility("default"))) #else // static library #define BOX2D_EXPORT #endif +#if defined(BOX2D_IMPLEMENTATION) + #pragma message("BOX2D_IMPLEMENTATION") + #if defined(_WIN32) && defined(box2d_EXPORTS) + // build the Windows DLL + #define BOX2D_INLINE __declspec(dllexport) extern inline + #elif defined(_WIN32) && defined(BOX2D_DLL) + // using the Windows DLL + #define BOX2D_INLINE __declspec(dllimport) + #elif defined(box2d_EXPORTS) + #define BOX2D_INLINE __attribute__((visibility("default"))) + #else + #define BOX2D_INLINE extern inline + #endif +#else + // #pragma message("BOX2D inline") + #define BOX2D_INLINE inline +#endif + #ifdef __cplusplus #define B2_API extern "C" BOX2D_EXPORT + #define B2_INLINE extern "C" BOX2D_INLINE + #define B2_LITERAL(T) T #else #define B2_API BOX2D_EXPORT + #define B2_INLINE BOX2D_INLINE + /// Used for C literals like (b2Vec2){1.0f, 2.0f} where C++ requires b2Vec2{1.0f, 2.0f} + #define B2_LITERAL(T) (T) #endif /// Prototype for user allocation function. @@ -34,6 +57,9 @@ typedef void* b2AllocFcn(uint32_t size, int32_t alignment); /// @param mem the memory previously allocated through `b2AllocFcn` typedef void b2FreeFcn(void* mem); +/// Prototype for the user assert callback. Return 0 to skip the debugger break. +typedef int b2AssertFcn(const char* condition, const char* fileName, int lineNumber); + /// This allows the user to override the allocation functions. These should be /// set during application startup. B2_API void b2SetAllocator(b2AllocFcn* allocFcn, b2FreeFcn* freeFcn); @@ -41,9 +67,6 @@ B2_API void b2SetAllocator(b2AllocFcn* allocFcn, b2FreeFcn* freeFcn); /// Total bytes allocated by Box2D B2_API uint32_t b2GetByteCount(void); -/// Prototype for the user assert callback. Return 0 to skip the debugger break. -typedef int b2AssertFcn(const char* condition, const char* fileName, int lineNumber); - /// Override the default assert callback. /// @param assertFcn a non-null assert callback B2_API void b2SetAssertFcn(b2AssertFcn* assertFcn); diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index c16cb3a8..d39993e0 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -11,10 +11,12 @@ #include "joint_types.h" #include "types.h" +#include + typedef struct b2Capsule b2Capsule; typedef struct b2Circle b2Circle; -typedef struct b2Polygon b2Polygon; typedef struct b2DebugDraw b2DebugDraw; +typedef struct b2Polygon b2Polygon; typedef struct b2Segment b2Segment; /** @@ -102,6 +104,9 @@ B2_API void b2World_EnableContinuous(b2WorldId worldId, bool flag); /// Adjust the restitution threshold. Advanced feature for testing. B2_API void b2World_SetRestitutionThreshold(b2WorldId worldId, float value); +/// Register the pre-solve callback. This is optional. +B2_API void b2World_SetPreSolveCallback(b2WorldId worldId, b2PreSolveFcn* fcn, void* context); + /// Adjust contact tuning parameters: /// - hertz is the contact stiffness (cycles per second) /// - damping ratio is the contact bounciness with 1 being critical damping (non-dimensional) @@ -127,7 +132,8 @@ B2_API b2Counters b2World_GetCounters(b2WorldId worldId); /// @warning This function is locked during callbacks. B2_API b2BodyId b2CreateBody(b2WorldId worldId, const b2BodyDef* def); -/// Destroy a rigid body given an id. +/// Destroy a rigid body given an id. This destroys all shapes attached to the body +/// but does not destroy the joints. /// @warning This function is locked during callbacks. B2_API void b2DestroyBody(b2BodyId bodyId); @@ -145,6 +151,9 @@ B2_API b2BodyType b2Body_GetType(b2BodyId bodyId); /// Set the type of a body. This has a similar cost to re-creating the body. B2_API void b2Body_SetType(b2BodyId bodyId, b2BodyType type); +/// Set the user data for a body +B2_API void b2Body_SetUserData(b2BodyId bodyId, void* userData); + /// Get the user data stored in a body B2_API void* b2Body_GetUserData(b2BodyId bodyId); @@ -209,18 +218,25 @@ B2_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. +/// This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. /// @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 B2_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. +/// This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. /// @param wake also wake up the body B2_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 +/// This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. +/// @param impulse the angular impulse in units of +/// kg*m*m/s /// @param wake also wake up the body B2_API void b2Body_ApplyAngularImpulse(b2BodyId bodyId, float impulse, bool wake); @@ -244,6 +260,11 @@ B2_API void b2Body_SetMassData(b2BodyId bodyId, b2MassData massData); /// Get the mass data for a body. B2_API b2MassData b2Body_GetMassData(b2BodyId bodyId); +/// This resets the mass properties to the sum of the mass properties of the fixtures. +/// This normally does not need to be called unless you called SetMassData to override +/// the mass and you later want to reset the mass. +B2_API void b2Body_ResetMassData(b2BodyId bodyId); + /// Adjust the linear damping. Normally this is set in b2BodyDef before creation. B2_API void b2Body_SetLinearDamping(b2BodyId bodyId, float linearDamping); @@ -268,6 +289,12 @@ B2_API bool b2Body_IsAwake(b2BodyId bodyId); /// Wake a body from sleep. This wakes the entire island the body is touching. B2_API void b2Body_Wake(b2BodyId bodyId); +/// Enable or disable sleeping this body. If sleeping is disabled the body will wake. +B2_API void b2Body_EnableSleep(b2BodyId bodyId, bool enableSleep); + +/// @return is sleeping enabled for this body? +B2_API bool b2Body_IsSleepEnabled(b2BodyId bodyId); + /// Is this body enabled? B2_API bool b2Body_IsEnabled(b2BodyId bodyId); @@ -299,22 +326,22 @@ B2_API b2AABB b2Body_ComputeAABB(b2BodyId bodyId); * @{ */ -/// Create a circle shape and attach it to a body. The shape defintion and geometry are fully cloned. +/// Create a circle shape and attach it to a body. The shape definition and geometry are fully cloned. /// Contacts are not created until the next time step. /// @return the shape id for accessing the shape B2_API b2ShapeId b2CreateCircleShape(b2BodyId bodyId, const b2ShapeDef* def, const b2Circle* circle); -/// Create a line segment shape and attach it to a body. The shape defintion and geometry are fully cloned. +/// Create a line segment shape and attach it to a body. The shape definition and geometry are fully cloned. /// Contacts are not created until the next time step. /// @return the shape id for accessing the shape B2_API b2ShapeId b2CreateSegmentShape(b2BodyId bodyId, const b2ShapeDef* def, const b2Segment* segment); -/// Create a capsule shape and attach it to a body. The shape defintion and geometry are fully cloned. +/// Create a capsule shape and attach it to a body. The shape definition and geometry are fully cloned. /// Contacts are not created until the next time step. /// @return the shape id for accessing the shape B2_API b2ShapeId b2CreateCapsuleShape(b2BodyId bodyId, const b2ShapeDef* def, const b2Capsule* capsule); -/// Create a polygon shape and attach it to a body. The shape defintion and geometry are fully cloned. +/// Create a polygon shape and attach it to a body. The shape definition and geometry are fully cloned. /// Contacts are not created until the next time step. /// @return the shape id for accessing the shape B2_API b2ShapeId b2CreatePolygonShape(b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon); @@ -325,16 +352,6 @@ B2_API void b2DestroyShape(b2ShapeId shapeId); /// Shape identifier validation. Provides validation for up to 64K allocations. B2_API bool b2Shape_IsValid(b2ShapeId id); -/// Create a chain shape -/// @see b2ChainDef for details -B2_API b2ChainId b2CreateChain(b2BodyId bodyId, const b2ChainDef* def); - -/// Destroy a chain shape -B2_API void b2DestroyChain(b2ChainId chainId); - -/// Chain identifier validation. Provides validation for up to 64K allocations. -B2_API bool b2Chain_IsValid(b2ChainId id); - /// Get the type of a shape. B2_API b2ShapeType b2Shape_GetType(b2ShapeId shapeId); @@ -344,12 +361,16 @@ B2_API b2BodyId b2Shape_GetBody(b2ShapeId shapeId); /// Is this shape a sensor? See b2ShapeDef. B2_API bool b2Shape_IsSensor(b2ShapeId shapeId); +/// Set the user data for a shape. +B2_API void b2Shape_SetUserData(b2ShapeId shapeId, void* userData); + /// Get the user data for a shape. This is useful when you get a shape id -/// from an event or query +/// from an event or query. B2_API void* b2Shape_GetUserData(b2ShapeId shapeId); /// Set the density on a shape. Normally this is specified in b2ShapeDef. -/// This will recompute the mass properties on the parent body. +/// This will not update the mass properties on the parent body until you +/// call b2Body_ResetMassData. B2_API void b2Shape_SetDensity(b2ShapeId shapeId, float density); /// Get the density on a shape. @@ -376,20 +397,37 @@ B2_API void b2Shape_SetFilter(b2ShapeId shapeId, b2Filter filter); /// Test a point for overlap with a shape B2_API bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point); -/// Access the circle geometry of a shape. -B2_API const b2Circle* b2Shape_GetCircle(b2ShapeId shapeId); +/// Ray cast a shape directly +B2_API b2CastOutput b2Shape_RayCast(b2ShapeId shapeId, b2Vec2 origin, b2Vec2 translation); -/// Access the line segment geometry of a shape. -B2_API const b2Segment* b2Shape_GetSegment(b2ShapeId shapeId); +/// Access the circle geometry of a shape. Asserts the type is correct. +B2_API const b2Circle b2Shape_GetCircle(b2ShapeId shapeId); + +/// Access the line segment geometry of a shape. Asserts the type is correct. +B2_API const b2Segment b2Shape_GetSegment(b2ShapeId shapeId); /// Access the smooth line segment geometry of a shape. These come from chain shapes. -B2_API const b2SmoothSegment* b2Shape_GetSmoothSegment(b2ShapeId shapeId); +/// Asserts the type is correct. +B2_API const b2SmoothSegment b2Shape_GetSmoothSegment(b2ShapeId shapeId); + +/// Access the capsule geometry of a shape. Asserts the type is correct. +B2_API const b2Capsule b2Shape_GetCapsule(b2ShapeId shapeId); -/// Access the capsule geometry of a shape. -B2_API const b2Capsule* b2Shape_GetCapsule(b2ShapeId shapeId); +/// Access the convex polygon geometry of a shape. Asserts the type is correct. +B2_API const b2Polygon b2Shape_GetPolygon(b2ShapeId shapeId); -/// Access the convex polygon geometry of a shape. -B2_API const b2Polygon* b2Shape_GetPolygon(b2ShapeId shapeId); +/// Allows you to change a shape to be a circle or update the current circle. +/// This does not modify the mass properties. +B2_API void b2Shape_SetCircle(b2ShapeId shapeId, b2Circle circle); + +/// Allows you to change a shape to be a capsule or update the current capsule. +B2_API void b2Shape_SetCapsule(b2ShapeId shapeId, b2Capsule capsule); + +/// Allows you to change a shape to be a segment or update the current segment. +B2_API void b2Shape_SetSegment(b2ShapeId shapeId, b2Segment segment); + +/// Allows you to change a shape to be a segment or update the current segment. +B2_API void b2Shape_SetPolygon(b2ShapeId shapeId, b2Polygon polygon); /// If the type is b2_smoothSegmentShape then you can get the parent chain id. /// If the shape is not a smooth segment then this will return b2_nullChainId. @@ -410,6 +448,18 @@ B2_API int32_t b2Shape_GetContactData(b2ShapeId shapeId, b2ContactData* contactD /// Get the current world AABB B2_API b2AABB b2Shape_GetAABB(b2ShapeId shapeId); +/// Chain Shape + +/// Create a chain shape +/// @see b2ChainDef for details +B2_API b2ChainId b2CreateChain(b2BodyId bodyId, const b2ChainDef* def); + +/// Destroy a chain shape +B2_API void b2DestroyChain(b2ChainId chainId); + +/// Chain identifier validation. Provides validation for up to 64K allocations. +B2_API bool b2Chain_IsValid(b2ChainId id); + /** @} */ /** @@ -455,129 +505,258 @@ B2_API bool b2Joint_IsValid(b2JointId id); /// Get the joint type B2_API b2JointType b2Joint_GetType(b2JointId jointId); - /// Get body A on a joint +/// Get body A on a joint B2_API b2BodyId b2Joint_GetBodyA(b2JointId jointId); /// Get body B on a joint B2_API b2BodyId b2Joint_GetBodyB(b2JointId jointId); +/// Get local anchor on bodyA +B2_API b2Vec2 b2Joint_GetLocalAnchorA(b2JointId jointId); + +/// Get local anchor on bodyB +B2_API b2Vec2 b2Joint_GetLocalAnchorB(b2JointId jointId); + +/// Toggle collision between connected bodies +B2_API void b2Joint_SetCollideConnected(b2JointId jointId, bool shouldCollide); + +/// Is collision allowed between connected bodies? +B2_API bool b2Joint_GetCollideConnected(b2JointId jointId); + +/// Set the user data on a joint +B2_API void b2Joint_SetUserData(b2JointId jointId, void* userData); + +/// Get the user data on a joint +B2_API void* b2Joint_GetUserData(b2JointId jointId); + +/// Distance Joint + /// Get the constraint force on a distance joint B2_API float b2DistanceJoint_GetConstraintForce(b2JointId jointId, float timeStep); -/// Set the length parameters of a distance joint -/// @see b2DistanceJointDef for details -B2_API void b2DistanceJoint_SetLength(b2JointId jointId, float length, float minLength, float maxLength); +/// Set the rest length of a distance joint +B2_API void b2DistanceJoint_SetLength(b2JointId jointId, float length); + +/// Get the rest length of a distance joint +B2_API float b2DistanceJoint_GetLength(b2JointId jointId); + +/// Set the minimum and maximum length parameters of a distance joint +B2_API void b2DistanceJoint_SetLengthRange(b2JointId jointId, float minLength, float maxLength); + +/// Get the minimum distance joint length +B2_API float b2DistanceJoint_GetMinLength(b2JointId jointId); + +/// Get the maximum distance joint length +B2_API float b2DistanceJoint_GetMaxLength(b2JointId jointId); /// Get the current length of a distance joint B2_API float b2DistanceJoint_GetCurrentLength(b2JointId jointId); -/// Adjust the softness of a distance joint -/// @see b2DistanceJointDef for details +/// Adjust the softness parameters of a distance joint B2_API void b2DistanceJoint_SetTuning(b2JointId jointId, float hertz, float dampingRatio); -/// Set the linear offset target for a motor joint +/// Get the Hertz of a distance joint +B2_API float b2DistanceJoint_GetHertz(b2JointId jointId); + +/// Get the damping ratio of a distance joint +B2_API float b2DistanceJoint_GetDampingRatio(b2JointId jointId); + +/// Motor Joint + +/// Set/Get the linear offset target for a motor joint B2_API void b2MotorJoint_SetLinearOffset(b2JointId jointId, b2Vec2 linearOffset); +/// @return the linear offset target for a motor joint +B2_API b2Vec2 b2MotorJoint_GetLinearOffset(b2JointId jointId); + /// Set the angular offset target for a motor joint in radians B2_API void b2MotorJoint_SetAngularOffset(b2JointId jointId, float angularOffset); +/// @return the angular offset target for a motor joint in radians +B2_API float b2MotorJoint_GetAngularOffset(b2JointId jointId); + /// Set the maximum force for a motor joint B2_API void b2MotorJoint_SetMaxForce(b2JointId jointId, float maxForce); +/// @return the maximum force for a motor joint +B2_API float b2MotorJoint_GetMaxForce(b2JointId jointId); + /// Set the maximum torque for a motor joint B2_API void b2MotorJoint_SetMaxTorque(b2JointId jointId, float maxTorque); +/// @return the maximum torque for a motor joint +B2_API float b2MotorJoint_GetMaxTorque(b2JointId jointId); + /// Set the correction factor for a motor joint B2_API void b2MotorJoint_SetCorrectionFactor(b2JointId jointId, float correctionFactor); +/// @return the correction factor for a motor joint +B2_API float b2MotorJoint_GetCorrectionFactor(b2JointId jointId); + /// Get the current constraint force for a motor joint B2_API b2Vec2 b2MotorJoint_GetConstraintForce(b2JointId jointId); /// Get the current constraint torque for a motor joint B2_API float b2MotorJoint_GetConstraintTorque(b2JointId jointId); +/// Mouse Joint + /// Set the target for a mouse joint B2_API void b2MouseJoint_SetTarget(b2JointId jointId, b2Vec2 target); +/// @return the target for a mouse joint +B2_API b2Vec2 b2MouseJoint_GetTarget(b2JointId jointId); + +/// Adjust the softness parameters of a mouse joint +B2_API void b2MouseJoint_SetTuning(b2JointId jointId, float hertz, float dampingRatio); + +/// Get the Hertz of a mouse joint +B2_API float b2MouseJoint_GetHertz(b2JointId jointId); + +/// Get the damping ratio of a mouse joint +B2_API float b2MouseJoint_GetDampingRatio(b2JointId jointId); + +/// Prismatic Joint + /// Enable/disable a prismatic joint limit B2_API void b2PrismaticJoint_EnableLimit(b2JointId jointId, bool enableLimit); +/// @return is the prismatic joint limit enabled +B2_API bool b2PrismaticJoint_IsLimitEnabled(b2JointId jointId); + /// Enable/disable a prismatic joint motor B2_API void b2PrismaticJoint_EnableMotor(b2JointId jointId, bool enableMotor); +/// @return is the prismatic joint motor enabled +B2_API bool b2PrismaticJoint_IsMotorEnabled(b2JointId jointId); + /// Set the motor speed for a prismatic joint B2_API void b2PrismaticJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed); +/// @return the motor speed for a prismatic joint +B2_API float b2PrismaticJoint_GetMotorSpeed(b2JointId jointId); + /// Get the current motor force for a prismatic joint B2_API float b2PrismaticJoint_GetMotorForce(b2JointId jointId); -/// Set the maximum force for a pristmatic joint motor +/// Set the maximum force for a prismatic joint motor B2_API void b2PrismaticJoint_SetMaxMotorForce(b2JointId jointId, float force); +/// @return the maximum force for a prismatic joint motor +B2_API float b2PrismaticJoint_GetMaxMotorForce(b2JointId jointId); + /// Get the current constraint force for a prismatic joint B2_API b2Vec2 b2PrismaticJoint_GetConstraintForce(b2JointId jointId); /// Get the current constraint torque for a prismatic joint B2_API float b2PrismaticJoint_GetConstraintTorque(b2JointId jointId); +/// Revolute Joint + /// Enable/disable a revolute joint limit B2_API void b2RevoluteJoint_EnableLimit(b2JointId jointId, bool enableLimit); +/// @return is the revolute joint limit enabled +B2_API bool b2RevoluteJoint_IsLimitEnabled(b2JointId jointId); + /// Enable/disable a revolute joint motor B2_API void b2RevoluteJoint_EnableMotor(b2JointId jointId, bool enableMotor); +/// @return is the revolute joint motor enabled +B2_API bool b2RevoluteJoint_IsMotorEnabled(b2JointId jointId); + /// Set the motor speed for a revolute joint in radians per second B2_API void b2RevoluteJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed); +/// @return the motor speed for a revolute joint in radians per second +B2_API float b2RevoluteJoint_GetMotorSpeed(b2JointId jointId); + /// Get the current motor torque for a revolute joint B2_API float b2RevoluteJoint_GetMotorTorque(b2JointId jointId); /// Set the maximum torque for a revolute joint motor B2_API void b2RevoluteJoint_SetMaxMotorTorque(b2JointId jointId, float torque); +/// @return the maximum torque for a revolute joint motor +B2_API float b2RevoluteJoint_GetMaxMotorTorque(b2JointId jointId); + /// Get the current constraint force for a revolute joint B2_API b2Vec2 b2RevoluteJoint_GetConstraintForce(b2JointId jointId); /// Get the current constraint torque for a revolute joint B2_API float b2RevoluteJoint_GetConstraintTorque(b2JointId jointId); +/// Wheel Joint + /// Set the wheel joint stiffness in Hertz B2_API void b2WheelJoint_SetSpringHertz(b2JointId jointId, float hertz); +/// @return the wheel joint stiffness in Hertz +B2_API float b2WheelJoint_GetSpringHertz(b2JointId jointId); + /// Set the wheel joint damping ratio (non-dimensional) B2_API void b2WheelJoint_SetSpringDampingRatio(b2JointId jointId, float dampingRatio); +/// @return the wheel joint damping ratio (non-dimensional) +B2_API float b2WheelJoint_GetSpringDampingRatio(b2JointId jointId); + /// Enable/disable the wheel joint limit B2_API void b2WheelJoint_EnableLimit(b2JointId jointId, bool enableLimit); +/// @return is the wheel joint limit enabled +B2_API bool b2WheelJoint_IsLimitEnabled(b2JointId jointId); + /// Enable/disable the wheel joint motor B2_API void b2WheelJoint_EnableMotor(b2JointId jointId, bool enableMotor); +/// @return is the wheel joint motor enabled +B2_API bool b2WheelJoint_IsMotorEnabled(b2JointId jointId); + /// Set the wheel joint motor speed in radians per second B2_API void b2WheelJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed); +/// @return the wheel joint motor speed in radians per second +B2_API float b2WheelJoint_GetMotorSpeed(b2JointId jointId); + /// Get the wheel joint current motor torque B2_API float b2WheelJoint_GetMotorTorque(b2JointId jointId); /// Set the wheel joint maximum motor torque B2_API void b2WheelJoint_SetMaxMotorTorque(b2JointId jointId, float torque); +/// @return the wheel joint maximum motor torque +B2_API float b2WheelJoint_GetMaxMotorTorque(b2JointId jointId); + /// Get the current wheel joint constraint force B2_API b2Vec2 b2WheelJoint_GetConstraintForce(b2JointId jointId); /// Get the current wheel joint constraint torque B2_API float b2WheelJoint_GetConstraintTorque(b2JointId jointId); +/// Weld Joint + /// Set weld joint linear stiffness in Hertz. 0 is rigid. B2_API void b2WeldJoint_SetLinearHertz(b2JointId jointId, float hertz); +/// @return the weld joint linear stiffness in Hertz. +B2_API float b2WeldJoint_GetLinearHertz(b2JointId jointId); + /// Set weld joint linear damping ratio (non-dimensional) B2_API void b2WeldJoint_SetLinearDampingRatio(b2JointId jointId, float dampingRatio); +/// @return the weld joint linear damping ratio (non-dimensional) +B2_API float b2WeldJoint_GetLinearDampingRatio(b2JointId jointId); + /// Set weld joint angular stiffness in Hertz. 0 is rigid. B2_API void b2WeldJoint_SetAngularHertz(b2JointId jointId, float hertz); +/// @return the weld joint angular stiffness in Hertz. +B2_API float b2WeldJoint_GetAngularHertz(b2JointId jointId); + /// Set weld joint angular damping ratio (non-dimensional) B2_API void b2WeldJoint_SetAngularDampingRatio(b2JointId jointId, float dampingRatio); +/// @return the weld joint angular damping ratio (non-dimensional) +B2_API float b2WeldJoint_GetAngularDampingRatio(b2JointId jointId); + /** @} */ diff --git a/include/box2d/callbacks.h b/include/box2d/callbacks.h index 5fd334b4..e2ad0dc9 100644 --- a/include/box2d/callbacks.h +++ b/include/box2d/callbacks.h @@ -3,12 +3,44 @@ #pragma once -#include "api.h" #include "id.h" -#include "types.h" +#include "math_types.h" + +#include typedef struct b2Manifold b2Manifold; +/// Task interface +/// This is prototype for a Box2D task. Your task system is expected to invoke the Box2D task with these arguments. +/// The task spans a range of the parallel-for: [startIndex, endIndex) +/// The worker index must correctly identify each worker in the user thread pool, expected in [0, workerCount). +/// A worker must only exist on only one thread at a time and is analogous to the thread index. +/// The task context is the context pointer sent from Box2D when it is enqueued. +/// The startIndex and endIndex are expected in the range [0, itemCount) where itemCount is the argument to b2EnqueueTaskCallback +/// below. Box2D expects startIndex < endIndex and will execute a loop like this: +/// +/// for (int i = startIndex; i < endIndex; ++i) +/// { +/// DoWork(); +/// } +typedef void b2TaskCallback(int32_t startIndex, int32_t endIndex, uint32_t workerIndex, void* taskContext); + +/// These functions can be provided to Box2D to invoke a task system. These are designed to work well with enkiTS. +/// Returns a pointer to the user's task object. May be nullptr. A nullptr indicates to Box2D that the work was executed +/// serially within the callback and there is no need to call b2FinishTaskCallback. +/// The itemCount is the number of Box2D work items that are to be partitioned among workers by the user's task system. +/// This is essentially a parallel-for. The minRange parameter is a suggestion of the minimum number of items to assign +/// per worker to reduce overhead. For example, suppose the task is small and that itemCount is 16. A minRange of 8 suggests +/// that your task system should split the work items among just two workers, even if you have more available. +/// In general the range [startIndex, endIndex) send to b2TaskCallback should obey: +/// endIndex - startIndex >= minRange +/// The exception of course is when itemCount < minRange. +typedef void* b2EnqueueTaskCallback(b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext, + void* userContext); + +/// Finishes a user task object that wraps a Box2D task. +typedef void b2FinishTaskCallback(void* userTask, void* userContext); + /// Prototype for a pre-solve callback. /// This is called after a contact is updated. This allows you to inspect a /// contact before it goes to the solver. If you are careful, you can modify the @@ -22,9 +54,6 @@ typedef struct b2Manifold b2Manifold; /// Return false if you want to disable the contact this step typedef bool b2PreSolveFcn(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, void* context); -/// Register the pre-solve callback. This is optional. -B2_API void b2World_SetPreSolveCallback(b2WorldId worldId, b2PreSolveFcn* fcn, void* context); - /// Prototype callback for AABB queries. /// See b2World_Query /// Called for each shape found in the query AABB. @@ -35,6 +64,7 @@ typedef bool b2QueryResultFcn(b2ShapeId shapeId, void* context); /// See b2World::RayCast /// Called for each shape found in the query. You control how the ray cast /// proceeds by returning a float: +/// #todo rework this to return penetration /// return -1: ignore this shape and continue /// return 0: terminate the ray cast /// return fraction: clip the ray to this point @@ -46,13 +76,3 @@ typedef bool b2QueryResultFcn(b2ShapeId shapeId, void* context); /// @return -1 to filter, 0 to terminate, fraction to clip the ray for /// closest hit, 1 to continue typedef float b2CastResultFcn(b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* context); - -/// Use an instance of this structure and the callback below to get the closest hit. -typedef struct b2RayResult -{ - b2ShapeId shapeId; - b2Vec2 point; - b2Vec2 normal; - float fraction; - bool hit; -} b2RayResult; diff --git a/include/box2d/color.h b/include/box2d/color.h index 24a80292..34e99fdb 100644 --- a/include/box2d/color.h +++ b/include/box2d/color.h @@ -3,11 +3,8 @@ #pragma once -/// Color for debug drawing. Each value has the range [0,1]. -typedef struct b2Color -{ - float r, g, b, a; -} b2Color; +#include "api.h" +#include "types.h" /// All the colors! Credit to wherever I got this from, I forget. typedef enum b2HexColor @@ -581,7 +578,18 @@ typedef enum b2HexColor } b2HexColor; /// Make a color from a hex code -static inline b2Color b2MakeColor(enum b2HexColor hexCode, float alpha) +B2_INLINE b2Color b2MakeColor(enum b2HexColor hexCode) +{ + b2Color color; + color.r = ((hexCode >> 16) & 0xFF) / 255.0f; + color.g = ((hexCode >> 8) & 0xFF) / 255.0f; + color.b = (hexCode & 0xFF) / 255.0f; + color.a = 1.0f; + return color; +} + +/// Make a color from a hex code and alpha +B2_INLINE b2Color b2MakeColorAlpha(enum b2HexColor hexCode, float alpha) { b2Color color; color.r = ((hexCode >> 16) & 0xFF) / 255.0f; diff --git a/include/box2d/debug_draw.h b/include/box2d/debug_draw.h index 8a28efa6..56a10b40 100644 --- a/include/box2d/debug_draw.h +++ b/include/box2d/debug_draw.h @@ -3,7 +3,7 @@ #pragma once -#include "types.h" +#include "math_types.h" /// This struct holds callbacks you can implement to draw a box2d world. typedef struct b2DebugDraw diff --git a/include/box2d/distance.h b/include/box2d/distance.h index c2dac4e9..a9b76551 100644 --- a/include/box2d/distance.h +++ b/include/box2d/distance.h @@ -5,7 +5,7 @@ #include "api.h" #include "constants.h" -#include "types.h" +#include "geometry.h" /// Result of computing the distance between two line segments typedef struct b2SegmentDistanceResult @@ -79,7 +79,7 @@ typedef struct b2ShapeCastPairInput /// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction. /// @returns true if hit, false if there is no hit or an initial overlap -B2_API b2RayCastOutput b2ShapeCast(const b2ShapeCastPairInput* input); +B2_API b2CastOutput b2ShapeCast(const b2ShapeCastPairInput* input); /// Make a proxy for use in GJK and related functions. B2_API b2DistanceProxy b2MakeProxy(const b2Vec2* vertices, int32_t count, float radius); diff --git a/include/box2d/dynamic_tree.h b/include/box2d/dynamic_tree.h index 71587287..ecf1d3a6 100644 --- a/include/box2d/dynamic_tree.h +++ b/include/box2d/dynamic_tree.h @@ -5,11 +5,16 @@ #include "api.h" #include "constants.h" -#include "types.h" +#include "math_types.h" + +#include #define b2_defaultCategoryBits (0x00000001) #define b2_defaultMaskBits (0xFFFFFFFF) +typedef struct b2RayCastInput b2RayCastInput; +typedef struct b2ShapeCastInput b2ShapeCastInput; + /// A node in the dynamic tree. The user does not interact with this directly. /// 16 + 16 + 8 + pad(8) typedef struct b2TreeNode @@ -160,13 +165,13 @@ B2_API void b2DynamicTree_ShiftOrigin(b2DynamicTree* tree, b2Vec2 newOrigin); /// Get proxy user data /// @return the proxy user data or 0 if the id is invalid -static inline int32_t b2DynamicTree_GetUserData(const b2DynamicTree* tree, int32_t proxyId) +B2_INLINE int32_t b2DynamicTree_GetUserData(const b2DynamicTree* tree, int32_t proxyId) { return tree->nodes[proxyId].userData; } /// Get the AABB of a proxy -static inline b2AABB b2DynamicTree_GetAABB(const b2DynamicTree* tree, int32_t proxyId) +B2_INLINE b2AABB b2DynamicTree_GetAABB(const b2DynamicTree* tree, int32_t proxyId) { return tree->nodes[proxyId].aabb; } diff --git a/include/box2d/geometry.h b/include/box2d/geometry.h index fd1349e3..1b90cb52 100644 --- a/include/box2d/geometry.h +++ b/include/box2d/geometry.h @@ -8,8 +8,33 @@ #include "types.h" typedef struct b2Hull b2Hull; -typedef struct b2CastOutput b2RayCastOutput; -typedef struct b2RayCastInput b2RayCastInput; + +/// Low level ray-cast input data +typedef struct b2RayCastInput +{ + b2Vec2 origin, translation; + float maxFraction; +} b2RayCastInput; + +/// Low level shape cast input in generic form +typedef struct b2ShapeCastInput +{ + b2Vec2 points[b2_maxPolygonVertices]; + int32_t count; + float radius; + b2Vec2 translation; + float maxFraction; +} b2ShapeCastInput; + +/// Low level ray-cast or shape-cast output data +typedef struct b2CastOutput +{ + b2Vec2 normal; + b2Vec2 point; + float fraction; + int32_t iterations; + bool hit; +} b2CastOutput; /// This holds the mass data computed for a shape. typedef struct b2MassData @@ -98,7 +123,7 @@ B2_API b2Polygon b2MakeRoundedBox(float hx, float hy, float radius); /// Make an offset box, bypassing the need for a convex hull. B2_API b2Polygon b2MakeOffsetBox(float hx, float hy, b2Vec2 center, float angle); -/// Transform a polygon. This is useful for transfering a shape from one body to another. +/// Transform a polygon. This is useful for transferring a shape from one body to another. B2_API b2Polygon b2TransformPolygon(b2Transform transform, const b2Polygon* polygon); /// Compute mass properties of a circle @@ -132,26 +157,26 @@ B2_API bool b2PointInCapsule(b2Vec2 point, const b2Capsule* shape); B2_API bool b2PointInPolygon(b2Vec2 point, const b2Polygon* shape); /// Ray cast versus circle in shape local space. Initial overlap is treated as a miss. -B2_API b2RayCastOutput b2RayCastCircle(const b2RayCastInput* input, const b2Circle* shape); +B2_API b2CastOutput b2RayCastCircle(const b2RayCastInput* input, const b2Circle* shape); /// Ray cast versus capsule in shape local space. Initial overlap is treated as a miss. -B2_API b2RayCastOutput b2RayCastCapsule(const b2RayCastInput* input, const b2Capsule* shape); +B2_API b2CastOutput b2RayCastCapsule(const b2RayCastInput* input, const b2Capsule* shape); /// Ray cast versus segment in shape local space. Optionally treat the segment as one-sided with hits from /// the left side being treated as a miss. -B2_API b2RayCastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* shape, bool oneSided); +B2_API b2CastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* shape, bool oneSided); /// Ray cast versus polygon in shape local space. Initial overlap is treated as a miss. -B2_API b2RayCastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* shape); +B2_API b2CastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* shape); /// Shape cast versus a circle. Initial overlap is treated as a miss. -B2_API b2RayCastOutput b2ShapeCastCircle(const b2ShapeCastInput* input, const b2Circle* shape); +B2_API b2CastOutput b2ShapeCastCircle(const b2ShapeCastInput* input, const b2Circle* shape); /// Shape cast versus a capsule. Initial overlap is treated as a miss. -B2_API b2RayCastOutput b2ShapeCastCapsule(const b2ShapeCastInput* input, const b2Capsule* shape); +B2_API b2CastOutput b2ShapeCastCapsule(const b2ShapeCastInput* input, const b2Capsule* shape); /// Shape cast versus a line segment. Initial overlap is treated as a miss. -B2_API b2RayCastOutput b2ShapeCastSegment(const b2ShapeCastInput* input, const b2Segment* shape); +B2_API b2CastOutput b2ShapeCastSegment(const b2ShapeCastInput* input, const b2Segment* shape); /// Shape cast versus a convex polygon. Initial overlap is treated as a miss. -B2_API b2RayCastOutput b2ShapeCastPolygon(const b2ShapeCastInput* input, const b2Polygon* shape); +B2_API b2CastOutput b2ShapeCastPolygon(const b2ShapeCastInput* input, const b2Polygon* shape); diff --git a/include/box2d/joint_types.h b/include/box2d/joint_types.h index 32176460..0285ff5f 100644 --- a/include/box2d/joint_types.h +++ b/include/box2d/joint_types.h @@ -52,16 +52,13 @@ typedef struct b2DistanceJointDef /// Set this flag to true if the attached bodies should collide. bool collideConnected; + /// User data pointer + void* userData; + } b2DistanceJointDef; /// Use this to initialize your joint definition -static inline b2DistanceJointDef b2DefaultDistanceJointDef() -{ - b2DistanceJointDef def = B2_ZERO_INIT; - def.length = 1.0f; - def.maxLength = b2_huge; - return def; -} +B2_API b2DistanceJointDef b2DefaultDistanceJointDef(); /// A motor joint is used to control the relative motion /// between two bodies. A typical usage is to control the movement @@ -88,17 +85,17 @@ typedef struct b2MotorJointDef /// Position correction factor in the range [0,1]. float correctionFactor; + + /// Set this flag to true if the attached bodies should collide. + bool collideConnected; + + /// User data pointer + void* userData; + } b2MotorJointDef; /// Use this to initialize your joint definition -static inline b2MotorJointDef b2DefaultMotorJointDef() -{ - b2MotorJointDef def = B2_ZERO_INIT; - def.maxForce = 1.0f; - def.maxTorque = 1.0f; - def.correctionFactor = 0.3f; - return def; -} +B2_API b2MotorJointDef b2DefaultMotorJointDef(); /// A mouse joint is used to make a point on a body track a /// specified world point. This a soft constraint and allows the constraint to stretch without @@ -119,16 +116,17 @@ typedef struct b2MouseJointDef /// Damping ratio, non-dimensional float dampingRatio; + + /// Set this flag to true if the attached bodies should collide. + bool collideConnected; + + /// User data pointer + void* userData; + } b2MouseJointDef; /// Use this to initialize your joint definition -static inline b2MouseJointDef b2DefaultMouseJointDef() -{ - b2MouseJointDef def = B2_ZERO_INIT; - def.hertz = 4.0f; - def.dampingRatio = 1.0f; - return def; -} +B2_API b2MouseJointDef b2DefaultMouseJointDef(); /// Prismatic joint definition. This requires defining a line of /// motion using an axis and an anchor point. The definition uses local @@ -175,15 +173,13 @@ typedef struct b2PrismaticJointDef /// Set this flag to true if the attached bodies should collide. bool collideConnected; + + /// User data pointer + void* userData; } b2PrismaticJointDef; /// Use this to initialize your joint definition -static inline b2PrismaticJointDef b2DefaultPrismaticJointDef() -{ - b2PrismaticJointDef def = B2_ZERO_INIT; - def.localAxisA = B2_LITERAL(b2Vec2){1.0f, 0.0f}; - return def; -} +B2_API b2PrismaticJointDef b2DefaultPrismaticJointDef(); /// Revolute joint definition. This requires defining an anchor point where the /// bodies are joined. The definition uses local anchor points so that the @@ -237,15 +233,13 @@ typedef struct b2RevoluteJointDef /// Set this flag to true if the attached bodies should collide. bool collideConnected; + + /// User data pointer + void* userData; } b2RevoluteJointDef; /// Use this to initialize your joint definition -static inline b2RevoluteJointDef b2DefaultRevoluteJointDef() -{ - b2RevoluteJointDef def = B2_ZERO_INIT; - def.drawSize = 0.25f; - return def; -} +B2_API b2RevoluteJointDef b2DefaultRevoluteJointDef(); /// A weld joint connect to bodies together rigidly. This constraint can be made soft to mimic /// soft-body simulation. @@ -281,14 +275,13 @@ typedef struct b2WeldJointDef /// Set this flag to true if the attached bodies should collide. bool collideConnected; + + /// User data pointer + void* userData; } b2WeldJointDef; /// Use this to initialize your joint definition -static const b2WeldJointDef b2DefaultWeldJointDef() -{ - b2WeldJointDef def = B2_ZERO_INIT; - return def; -} +B2_API b2WeldJointDef b2DefaultWeldJointDef(); /// Wheel joint definition. This requires defining a line of /// motion using an axis and an anchor point. The definition uses local @@ -339,14 +332,10 @@ typedef struct b2WheelJointDef /// Set this flag to true if the attached bodies should collide. bool collideConnected; + + /// User data pointer + void* userData; } b2WheelJointDef; /// Use this to initialize your joint definition -static inline b2WheelJointDef b2DefaultWheelJointDef() -{ - b2WheelJointDef def = B2_ZERO_INIT; - def.localAxisA.y = 1.0f; - def.hertz = 1.0f; - def.dampingRatio = 0.7f; - return def; -} +B2_API b2WheelJointDef b2DefaultWheelJointDef(); diff --git a/include/box2d/joint_util.h b/include/box2d/joint_util.h deleted file mode 100644 index 0509603d..00000000 --- a/include/box2d/joint_util.h +++ /dev/null @@ -1,15 +0,0 @@ -// SPDX-FileCopyrightText: 2023 Erin Catto -// SPDX-License-Identifier: MIT - -#pragma once - -#include "api.h" -#include "id.h" - -/// Utility to compute linear stiffness values from frequency and damping ratio -B2_API void b2LinearStiffness(float* stiffness, float* damping, float frequencyHertz, float dampingRatio, b2BodyId bodyA, - b2BodyId bodyB); - -/// Utility to compute angular stiffness values from frequency and damping ratio -B2_API void b2AngularStiffness(float* stiffness, float* damping, float frequencyHertz, float dampingRatio, b2BodyId bodyA, - b2BodyId bodyB); diff --git a/include/box2d/math.h b/include/box2d/math.h index 56cf766c..701432a0 100644 --- a/include/box2d/math.h +++ b/include/box2d/math.h @@ -4,10 +4,11 @@ #pragma once #include "api.h" -#include "types.h" +#include "math_types.h" #include #include +#include /// Macro to get the minimum of two values #define B2_MIN(A, B) ((A) < (B) ? (A) : (B)) @@ -27,94 +28,94 @@ 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}}; /// Vector dot product -static inline float b2Dot(b2Vec2 a, b2Vec2 b) +B2_INLINE float b2Dot(b2Vec2 a, b2Vec2 b) { return a.x * b.x + a.y * b.y; } /// Vector cross product. In 2D this yields a scalar. -static inline float b2Cross(b2Vec2 a, b2Vec2 b) +B2_INLINE float b2Cross(b2Vec2 a, b2Vec2 b) { return a.x * b.y - a.y * b.x; } /// Perform the cross product on a vector and a scalar. In 2D this produces /// a vector. -static inline b2Vec2 b2CrossVS(b2Vec2 v, float s) +B2_INLINE b2Vec2 b2CrossVS(b2Vec2 v, float s) { return B2_LITERAL(b2Vec2){s * v.y, -s * v.x}; } /// Perform the cross product on a scalar and a vector. In 2D this produces /// a vector. -static inline b2Vec2 b2CrossSV(float s, b2Vec2 v) +B2_INLINE b2Vec2 b2CrossSV(float s, b2Vec2 v) { return B2_LITERAL(b2Vec2){-s * v.y, s * v.x}; } /// Get a left pointing perpendicular vector. Equivalent to b2CrossSV(1.0f, v) -static inline b2Vec2 b2LeftPerp(b2Vec2 v) +B2_INLINE b2Vec2 b2LeftPerp(b2Vec2 v) { return B2_LITERAL(b2Vec2){-v.y, v.x}; } /// Get a right pointing perpendicular vector. Equivalent to b2CrossVS(v, 1.0f) -static inline b2Vec2 b2RightPerp(b2Vec2 v) +B2_INLINE b2Vec2 b2RightPerp(b2Vec2 v) { return B2_LITERAL(b2Vec2){v.y, -v.x}; } /// Vector addition -static inline b2Vec2 b2Add(b2Vec2 a, b2Vec2 b) +B2_INLINE b2Vec2 b2Add(b2Vec2 a, b2Vec2 b) { return B2_LITERAL(b2Vec2){a.x + b.x, a.y + b.y}; } /// Vector subtraction -static inline b2Vec2 b2Sub(b2Vec2 a, b2Vec2 b) +B2_INLINE b2Vec2 b2Sub(b2Vec2 a, b2Vec2 b) { return B2_LITERAL(b2Vec2){a.x - b.x, a.y - b.y}; } /// Vector subtraction -static inline b2Vec2 b2Neg(b2Vec2 a) +B2_INLINE b2Vec2 b2Neg(b2Vec2 a) { return B2_LITERAL(b2Vec2){-a.x, -a.y}; } /// Vector linear interpolation /// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ -static inline b2Vec2 b2Lerp(b2Vec2 a, b2Vec2 b, float t) +B2_INLINE b2Vec2 b2Lerp(b2Vec2 a, b2Vec2 b, float t) { return B2_LITERAL(b2Vec2){(1.0f - t) * a.x + t * b.x, (1.0f - t) * a.y + t * b.y}; } /// Component-wise multiplication -static inline b2Vec2 b2Mul(b2Vec2 a, b2Vec2 b) +B2_INLINE b2Vec2 b2Mul(b2Vec2 a, b2Vec2 b) { return B2_LITERAL(b2Vec2){a.x * b.x, a.y * b.y}; } /// Multiply a scalar and vector -static inline b2Vec2 b2MulSV(float s, b2Vec2 v) +B2_INLINE b2Vec2 b2MulSV(float s, b2Vec2 v) { return B2_LITERAL(b2Vec2){s * v.x, s * v.y}; } /// a + s * b -static inline b2Vec2 b2MulAdd(b2Vec2 a, float s, b2Vec2 b) +B2_INLINE b2Vec2 b2MulAdd(b2Vec2 a, float s, b2Vec2 b) { return B2_LITERAL(b2Vec2){a.x + s * b.x, a.y + s * b.y}; } /// a - s * b -static inline b2Vec2 b2MulSub(b2Vec2 a, float s, b2Vec2 b) +B2_INLINE b2Vec2 b2MulSub(b2Vec2 a, float s, b2Vec2 b) { return B2_LITERAL(b2Vec2){a.x - s * b.x, a.y - s * b.y}; } /// Component-wise absolute vector -static inline b2Vec2 b2Abs(b2Vec2 a) +B2_INLINE b2Vec2 b2Abs(b2Vec2 a) { b2Vec2 b; b.x = B2_ABS(a.x); @@ -123,7 +124,7 @@ static inline b2Vec2 b2Abs(b2Vec2 a) } /// Component-wise absolute vector -static inline b2Vec2 b2Min(b2Vec2 a, b2Vec2 b) +B2_INLINE b2Vec2 b2Min(b2Vec2 a, b2Vec2 b) { b2Vec2 c; c.x = B2_MIN(a.x, b.x); @@ -132,7 +133,7 @@ static inline b2Vec2 b2Min(b2Vec2 a, b2Vec2 b) } /// Component-wise absolute vector -static inline b2Vec2 b2Max(b2Vec2 a, b2Vec2 b) +B2_INLINE b2Vec2 b2Max(b2Vec2 a, b2Vec2 b) { b2Vec2 c; c.x = B2_MAX(a.x, b.x); @@ -141,7 +142,7 @@ static inline b2Vec2 b2Max(b2Vec2 a, b2Vec2 b) } /// Component-wise clamp vector so v into the range [a, b] -static inline b2Vec2 b2Clamp(b2Vec2 v, b2Vec2 a, b2Vec2 b) +B2_INLINE b2Vec2 b2Clamp(b2Vec2 v, b2Vec2 a, b2Vec2 b) { b2Vec2 c; c.x = B2_CLAMP(v.x, a.x, b.x); @@ -150,18 +151,18 @@ static inline b2Vec2 b2Clamp(b2Vec2 v, b2Vec2 a, b2Vec2 b) } /// Get the length of this vector (the norm). -static inline float b2Length(b2Vec2 v) +B2_INLINE float b2Length(b2Vec2 v) { return sqrtf(v.x * v.x + v.y * v.y); } /// Get the length of this vector (the norm). -static inline float b2LengthSquared(b2Vec2 v) +B2_INLINE float b2LengthSquared(b2Vec2 v) { return v.x * v.x + v.y * v.y; } -static inline float b2Distance(b2Vec2 a, b2Vec2 b) +B2_INLINE float b2Distance(b2Vec2 a, b2Vec2 b) { float dx = b.x - a.x; float dy = b.y - a.y; @@ -169,14 +170,14 @@ static inline float b2Distance(b2Vec2 a, b2Vec2 b) } /// Get the length of this vector (the norm). -static inline float b2DistanceSquared(b2Vec2 a, b2Vec2 b) +B2_INLINE float b2DistanceSquared(b2Vec2 a, b2Vec2 b) { b2Vec2 c = {b.x - a.x, b.y - a.y}; return c.x * c.x + c.y * c.y; } /// Set using an angle in radians. -static inline b2Rot b2MakeRot(float angle) +B2_INLINE b2Rot b2MakeRot(float angle) { // todo determinism b2Rot q = {sinf(angle), cosf(angle)}; @@ -184,7 +185,7 @@ static inline b2Rot b2MakeRot(float angle) } /// Normalize rotation -static inline b2Rot b2NormalizeRot(b2Rot q) +B2_INLINE b2Rot b2NormalizeRot(b2Rot q) { float mag = sqrtf(q.s * q.s + q.c * q.c); float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; @@ -193,7 +194,7 @@ static inline b2Rot b2NormalizeRot(b2Rot q) } /// Is this rotation normalized? -static inline bool b2IsNormalized(b2Rot q) +B2_INLINE bool b2IsNormalized(b2Rot q) { float qq = q.s * q.s + q.c * q.c; return 1.0f - 0.0004f < qq && qq < 1.0f + 0.0004f; @@ -201,7 +202,7 @@ static inline bool b2IsNormalized(b2Rot q) /// Normalized linear interpolation /// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ -static inline b2Rot b2NLerp(b2Rot q1, b2Rot q2, float t) +B2_INLINE b2Rot b2NLerp(b2Rot q1, b2Rot q2, float t) { float omt = 1.0f - t; b2Rot q = { @@ -215,7 +216,7 @@ static inline b2Rot b2NLerp(b2Rot q1, b2Rot q2, float t) /// Integration rotation from angular velocity /// @param q1 initial rotation /// @param deltaAngle the angular displacement in radians -static inline b2Rot b2IntegrateRotation(b2Rot q1, float deltaAngle) +B2_INLINE b2Rot b2IntegrateRotation(b2Rot q1, float deltaAngle) { // ds/dt = omega * cos(t) // dc/dt = -omega * sin(t) @@ -233,7 +234,7 @@ static inline b2Rot b2IntegrateRotation(b2Rot q1, float deltaAngle) /// @param q1 initial rotation /// @param q2 final rotation /// @param inv_h inverse time step -static inline float b2ComputeAngularVelocity(b2Rot q1, b2Rot q2, float inv_h) +B2_INLINE float b2ComputeAngularVelocity(b2Rot q1, b2Rot q2, float inv_h) { // ds/dt = omega * cos(t) // dc/dt = -omega * sin(t) @@ -250,28 +251,28 @@ static inline float b2ComputeAngularVelocity(b2Rot q1, b2Rot q2, float inv_h) } /// Get the angle in radians -static inline float b2Rot_GetAngle(b2Rot q) +B2_INLINE float b2Rot_GetAngle(b2Rot q) { // todo determinism return atan2f(q.s, q.c); } /// Get the x-axis -static inline b2Vec2 b2Rot_GetXAxis(b2Rot q) +B2_INLINE b2Vec2 b2Rot_GetXAxis(b2Rot q) { b2Vec2 v = {q.c, q.s}; return v; } /// Get the y-axis -static inline b2Vec2 b2Rot_GetYAxis(b2Rot q) +B2_INLINE b2Vec2 b2Rot_GetYAxis(b2Rot q) { b2Vec2 v = {-q.s, q.c}; return v; } /// Multiply two rotations: q * r -static inline b2Rot b2MulRot(b2Rot q, b2Rot r) +B2_INLINE b2Rot b2MulRot(b2Rot q, b2Rot r) { // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] @@ -284,7 +285,7 @@ static inline b2Rot b2MulRot(b2Rot q, b2Rot r) } /// Transpose multiply two rotations: qT * r -static inline b2Rot b2InvMulRot(b2Rot q, b2Rot r) +B2_INLINE b2Rot b2InvMulRot(b2Rot q, b2Rot r) { // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] @@ -297,7 +298,7 @@ static inline b2Rot b2InvMulRot(b2Rot q, b2Rot r) } // relative angle between b and a (rot_b * inv(rot_a)) -static inline float b2RelativeAngle(b2Rot b, b2Rot a) +B2_INLINE float b2RelativeAngle(b2Rot b, b2Rot a) { // sin(b - a) = bs * ac - bc * as // cos(b - a) = bc * ac + bs * as @@ -307,19 +308,19 @@ static inline float b2RelativeAngle(b2Rot b, b2Rot a) } /// Rotate a vector -static inline b2Vec2 b2RotateVector(b2Rot q, b2Vec2 v) +B2_INLINE b2Vec2 b2RotateVector(b2Rot q, b2Vec2 v) { return B2_LITERAL(b2Vec2){q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y}; } /// Inverse rotate a vector -static inline b2Vec2 b2InvRotateVector(b2Rot q, b2Vec2 v) +B2_INLINE b2Vec2 b2InvRotateVector(b2Rot q, b2Vec2 v) { return B2_LITERAL(b2Vec2){q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y}; } /// Transform a point (e.g. local space to world space) -static inline b2Vec2 b2TransformPoint(b2Transform xf, const b2Vec2 p) +B2_INLINE b2Vec2 b2TransformPoint(b2Transform xf, const b2Vec2 p) { float x = (xf.q.c * p.x - xf.q.s * p.y) + xf.p.x; float y = (xf.q.s * p.x + xf.q.c * p.y) + xf.p.y; @@ -328,7 +329,7 @@ static inline b2Vec2 b2TransformPoint(b2Transform xf, const b2Vec2 p) } /// Inverse transform a point (e.g. world space to local space) -static inline b2Vec2 b2InvTransformPoint(b2Transform xf, const b2Vec2 p) +B2_INLINE b2Vec2 b2InvTransformPoint(b2Transform xf, const b2Vec2 p) { float vx = p.x - xf.p.x; float vy = p.y - xf.p.y; @@ -337,7 +338,7 @@ static inline b2Vec2 b2InvTransformPoint(b2Transform xf, const b2Vec2 p) /// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p /// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p -static inline b2Transform b2MulTransforms(b2Transform A, b2Transform B) +B2_INLINE b2Transform b2MulTransforms(b2Transform A, b2Transform B) { b2Transform C; C.q = b2MulRot(A.q, B.q); @@ -347,7 +348,7 @@ static inline b2Transform b2MulTransforms(b2Transform A, b2Transform B) /// v2 = A.q' * (B.q * v1 + B.p - A.p) /// = A.q' * B.q * v1 + A.q' * (B.p - A.p) -static inline b2Transform b2InvMulTransforms(b2Transform A, b2Transform B) +B2_INLINE b2Transform b2InvMulTransforms(b2Transform A, b2Transform B) { b2Transform C; C.q = b2InvMulRot(A.q, B.q); @@ -356,7 +357,7 @@ static inline b2Transform b2InvMulTransforms(b2Transform A, b2Transform B) } /// Multiply a 2-by-2 matrix times a 2D vector -static inline b2Vec2 b2MulMV(b2Mat22 A, b2Vec2 v) +B2_INLINE b2Vec2 b2MulMV(b2Mat22 A, b2Vec2 v) { b2Vec2 u = { A.cx.x * v.x + A.cy.x * v.y, @@ -366,7 +367,7 @@ static inline b2Vec2 b2MulMV(b2Mat22 A, b2Vec2 v) } /// Get the inverse of a 2-by-2 matrix -static inline b2Mat22 b2GetInverse22(b2Mat22 A) +B2_INLINE b2Mat22 b2GetInverse22(b2Mat22 A) { float a = A.cx.x, b = A.cy.x, c = A.cx.y, d = A.cy.y; float det = a * d - b * c; @@ -384,7 +385,7 @@ static inline b2Mat22 b2GetInverse22(b2Mat22 A) /// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. -static inline b2Vec2 b2Solve22(b2Mat22 A, b2Vec2 b) +B2_INLINE b2Vec2 b2Solve22(b2Mat22 A, b2Vec2 b) { float a11 = A.cx.x, a12 = A.cy.x, a21 = A.cx.y, a22 = A.cy.y; float det = a11 * a22 - a12 * a21; @@ -397,7 +398,7 @@ static inline b2Vec2 b2Solve22(b2Mat22 A, b2Vec2 b) } /// Does a fully contain b -static inline bool b2AABB_Contains(b2AABB a, b2AABB b) +B2_INLINE bool b2AABB_Contains(b2AABB a, b2AABB b) { bool s = true; s = s && a.lowerBound.x <= b.lowerBound.x; @@ -408,21 +409,21 @@ static inline bool b2AABB_Contains(b2AABB a, b2AABB b) } /// Get the center of the AABB. -static inline b2Vec2 b2AABB_Center(b2AABB a) +B2_INLINE b2Vec2 b2AABB_Center(b2AABB a) { b2Vec2 b = {0.5f * (a.lowerBound.x + a.upperBound.x), 0.5f * (a.lowerBound.y + a.upperBound.y)}; return b; } /// Get the extents of the AABB (half-widths). -static inline b2Vec2 b2AABB_Extents(b2AABB a) +B2_INLINE b2Vec2 b2AABB_Extents(b2AABB a) { b2Vec2 b = {0.5f * (a.upperBound.x - a.lowerBound.x), 0.5f * (a.upperBound.y - a.lowerBound.y)}; return b; } /// Union of two AABBs -static inline b2AABB b2AABB_Union(b2AABB a, b2AABB b) +B2_INLINE b2AABB b2AABB_Union(b2AABB a, b2AABB b) { b2AABB c; c.lowerBound.x = B2_MIN(a.lowerBound.x, b.lowerBound.x); @@ -434,6 +435,7 @@ static inline b2AABB b2AABB_Union(b2AABB a, b2AABB b) B2_API bool b2IsValid(float a); B2_API bool b2Vec2_IsValid(b2Vec2 v); +B2_API bool b2Rot_IsValid(b2Rot q); B2_API bool b2AABB_IsValid(b2AABB aabb); /// Convert this vector into a unit vector diff --git a/include/box2d/math_cpp.h b/include/box2d/math_cpp.h index b6cdb963..7c5a6299 100644 --- a/include/box2d/math_cpp.h +++ b/include/box2d/math_cpp.h @@ -37,7 +37,7 @@ inline b2Vec2 operator+(b2Vec2 a, b2Vec2 b) inline b2Vec2 operator-(b2Vec2 a, b2Vec2 b) { - return {a.x + b.x, a.y + b.y}; + return {a.x - b.x, a.y - b.y}; } inline b2Vec2 operator*(float a, b2Vec2 b) diff --git a/include/box2d/math_types.h b/include/box2d/math_types.h new file mode 100644 index 00000000..08b5fa5d --- /dev/null +++ b/include/box2d/math_types.h @@ -0,0 +1,47 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +/// 2D vector +/// This can be used to represent a point or free vector +typedef struct b2Vec2 +{ + /// coordinates + float x, y; +} b2Vec2; + +/// 2D rotation +/// This is similar to using a complex number for rotation +typedef struct b2Rot +{ + /// sine and cosine + float s, c; +} b2Rot; + +/// A 2D rigid transform +typedef struct b2Transform +{ + b2Vec2 p; + b2Rot q; +} b2Transform; + +/// A 2-by-2 Matrix +typedef struct b2Mat22 +{ + /// columns + b2Vec2 cx, cy; +} b2Mat22; + +/// Axis-aligned bounding box +typedef struct b2AABB +{ + b2Vec2 lowerBound; + b2Vec2 upperBound; +} b2AABB; + +/// Color for debug drawing. Each value has the range [0,1]. +typedef struct b2Color +{ + float r, g, b, a; +} b2Color; diff --git a/include/box2d/types.h b/include/box2d/types.h index 970d83fd..7674eefe 100644 --- a/include/box2d/types.h +++ b/include/box2d/types.h @@ -5,128 +5,37 @@ * @file types.h * @brief types used by the Box2D API * - * Mostly definition structs + * Mostly definition structures * @see http://www.box2d.org */ #pragma once -#include "color.h" +#include "api.h" +#include "callbacks.h" #include "constants.h" #include "id.h" +#include "math_types.h" #include #include #include -// clang-format off -#ifdef __cplusplus - #define B2_LITERAL(T) T -#else - /// Used for C literals like (b2Vec2){1.0f, 2.0f} where C++ requires b2Vec2{1.0f, 2.0f} - #define B2_LITERAL(T) (T) -#endif -// clang-format on - /// Returns the number of elements of an array #define B2_ARRAY_COUNT(A) (int)(sizeof(A) / sizeof(A[0])) /// Used to prevent the compiler from warning about unused variables #define B2_MAYBE_UNUSED(x) ((void)(x)) -/// A 2D vector -/// This can be used to represent a point or free vector. -typedef struct b2Vec2 -{ - /// coordinates - float x, y; -} b2Vec2; - -/// 2D rotation -typedef struct b2Rot -{ - /// sine and cosine - float s, c; -} b2Rot; - -/// A 2D rigid transform -typedef struct b2Transform -{ - b2Vec2 p; - b2Rot q; -} b2Transform; - -/// A 2-by-2 Matrix -typedef struct b2Mat22 -{ - /// columns - b2Vec2 cx, cy; -} b2Mat22; - -/// Axis-aligned bounding box -typedef struct b2AABB -{ - b2Vec2 lowerBound; - b2Vec2 upperBound; -} b2AABB; - -/// Low level ray-cast input data -typedef struct b2RayCastInput -{ - b2Vec2 origin, translation; - float maxFraction; -} b2RayCastInput; - -/// Low level hape cast input in generic form -typedef struct b2ShapeCastInput +/// Result from b2World_RayCastClosest +typedef struct b2RayResult { - b2Vec2 points[b2_maxPolygonVertices]; - int32_t count; - float radius; - b2Vec2 translation; - float maxFraction; -} b2ShapeCastInput; - -/// Low level ray-cast or shape-cast output data -typedef struct b2CastOutput -{ - b2Vec2 normal; + b2ShapeId shapeId; b2Vec2 point; + b2Vec2 normal; float fraction; - int32_t iterations; bool hit; -} b2RayCastOutput; - -/// Task interface -/// This is prototype for a Box2D task. Your task system is expected to invoke the Box2D task with these arguments. -/// The task spans a range of the parallel-for: [startIndex, endIndex) -/// The worker index must correctly identify each worker in the user thread pool, expected in [0, workerCount). -/// A worker must only exist on only one thread at a time and is analogous to the thread index. -/// The task context is the context pointer sent from Box2D when it is enqueued. -/// The startIndex and endIndex are expected in the range [0, itemCount) where itemCount is the argument to b2EnqueueTaskCallback -/// below. Box2D expects startIndex < endIndex and will execute a loop like this: -/// -/// for (int i = startIndex; i < endIndex; ++i) -/// { -/// DoWork(); -/// } -typedef void b2TaskCallback(int32_t startIndex, int32_t endIndex, uint32_t workerIndex, void* taskContext); - -/// These functions can be provided to Box2D to invoke a task system. These are designed to work well with enkiTS. -/// Returns a pointer to the user's task object. May be nullptr. A nullptr indicates to Box2D that the work was executed -/// serially within the callback and there is no need to call b2FinishTaskCallback. -/// The itemCount is the number of Box2D work items that are to be partitioned among workers by the user's task system. -/// This is essentially a parallel-for. The minRange parameter is a suggestion of the minimum number of items to assign -/// per worker to reduce overhead. For example, suppose the task is small and that itemCount is 16. A minRange of 8 suggests -/// that your task system should split the work items among just two workers, even if you have more available. -/// In general the range [startIndex, endIndex) send to b2TaskCallback should obey: -/// endIndex - startIndex >= minRange -/// The exception of course is when itemCount < minRange. -typedef void* b2EnqueueTaskCallback(b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext, - void* userContext); - -/// Finishes a user task object that wraps a Box2D task. -typedef void b2FinishTaskCallback(void* userTask, void* userContext); +} b2RayResult; /// World definition used to create a simulation world. Must be initialized using b2DefaultWorldDef. typedef struct b2WorldDef @@ -189,24 +98,6 @@ typedef struct b2WorldDef void* userTaskContext; } b2WorldDef; -/// Use this to initialize your world definition -static inline b2WorldDef b2DefaultWorldDef() -{ - b2WorldDef def = B2_ZERO_INIT; - def.gravity.x = 0.0f; - def.gravity.y = -10.0f; - def.restitutionThreshold = 1.0f * b2_lengthUnitsPerMeter; - def.contactPushoutVelocity = 3.0f * b2_lengthUnitsPerMeter; - def.contactHertz = 30.0; - def.contactDampingRatio = 10.0f; - def.jointHertz = 60.0; - def.jointDampingRatio = 2.0f; - def.enableSleep = true; - def.enableContinous = true; - def.stackAllocatorCapacity = 1024 * 1024; - return def; -} - /// The body type. /// static: zero mass, zero velocity, may be manually moved /// kinematic: zero mass, non-zero velocity set by user, moved by solver @@ -270,18 +161,6 @@ typedef struct b2BodyDef bool isEnabled; } b2BodyDef; -/// Use this to initialize your body definition -static inline b2BodyDef b2DefaultBodyDef() -{ - b2BodyDef def = B2_ZERO_INIT; - def.type = b2_staticBody; - def.gravityScale = 1.0f; - def.enableSleep = true; - def.isAwake = true; - def.isEnabled = true; - return def; -} - /// This holds contact filtering data. typedef struct b2Filter { @@ -298,13 +177,6 @@ typedef struct b2Filter int32_t groupIndex; } b2Filter; -/// Use this to initialize your filter -static inline b2Filter b2DefaultFilter() -{ - b2Filter filter = {0x00000001, 0xFFFFFFFF, 0}; - return filter; -} - /// This holds contact filtering data. typedef struct b2QueryFilter { @@ -316,20 +188,13 @@ typedef struct b2QueryFilter uint32_t maskBits; } b2QueryFilter; -/// Use this to initialize your query filter -static inline b2QueryFilter b2DefaultQueryFilter() -{ - b2QueryFilter filter = {0x00000001, 0xFFFFFFFF}; - return filter; -} - /// Shape type typedef enum b2ShapeType { - b2_capsuleShape, b2_circleShape, - b2_polygonShape, + b2_capsuleShape, b2_segmentShape, + b2_polygonShape, b2_smoothSegmentShape, b2_shapeTypeCount } b2ShapeType; @@ -367,18 +232,6 @@ typedef struct b2ShapeDef } b2ShapeDef; -/// Use this to initialize your shape definition -static inline b2ShapeDef b2DefaultShapeDef() -{ - b2ShapeDef def = B2_ZERO_INIT; - def.friction = 0.6f; - def.density = 1.0f; - def.filter = b2DefaultFilter(); - def.enableSensorEvents = true; - def.enableContactEvents = true; - return def; -} - /// Used to create a chain of edges. This is designed to eliminate ghost collisions with some limitations. /// - DO NOT use chain shapes unless you understand the limitations. This is an advanced feature! /// - chains are one-sided @@ -391,6 +244,7 @@ static inline b2ShapeDef b2DefaultShapeDef() /// - an open chain shape has NO COLLISION on the first and final edge /// - you may overlap two open chains on their first three and/or last three points to get smooth collision /// - a chain shape creates multiple hidden shapes on the body +/// https://en.wikipedia.org/wiki/Polygonal_chain typedef struct b2ChainDef { /// An array of at least 4 points. These are cloned and may be temporary. @@ -415,15 +269,6 @@ typedef struct b2ChainDef b2Filter filter; } b2ChainDef; -/// Use this to initialize your chain definition -static inline b2ChainDef b2DefaultChainDef() -{ - b2ChainDef def = B2_ZERO_INIT; - def.friction = 0.6f; - def.filter = b2DefaultFilter(); - return def; -} - /// Profiling data. Times are in milliseconds. typedef struct b2Profile { @@ -453,3 +298,21 @@ typedef struct b2Counters int32_t taskCount; int32_t colorCounts[b2_graphColorCount + 1]; } b2Counters; + +/// Use this to initialize your world definition +B2_API b2WorldDef b2DefaultWorldDef(); + +/// Use this to initialize your body definition +B2_API b2BodyDef b2DefaultBodyDef(); + +/// Use this to initialize your filter +B2_API b2Filter b2DefaultFilter(); + +/// Use this to initialize your query filter +B2_API b2QueryFilter b2DefaultQueryFilter(); + +/// Use this to initialize your shape definition +B2_API b2ShapeDef b2DefaultShapeDef(); + +/// Use this to initialize your chain definition +B2_API b2ChainDef b2DefaultChainDef(); diff --git a/samples/collection/donut.cpp b/samples/collection/donut.cpp index d034743b..c9e3314a 100644 --- a/samples/collection/donut.cpp +++ b/samples/collection/donut.cpp @@ -86,13 +86,13 @@ void Donut::Despawn() for (int i = 0; i < e_sides; ++i) { - b2DestroyJoint(m_jointIds[i]); + //b2DestroyJoint(m_jointIds[i]); m_jointIds[i] = b2_nullJointId; } for (int i = 0; i < e_sides; ++i) { - b2DestroyBody(m_bodyIds[i]); + b2DestroyBodyAndJoints(m_bodyIds[i]); m_bodyIds[i] = b2_nullBodyId; } diff --git a/samples/collection/sample_dynamic_tree.cpp b/samples/collection/sample_dynamic_tree.cpp index c27a2781..5356cbd1 100644 --- a/samples/collection/sample_dynamic_tree.cpp +++ b/samples/collection/sample_dynamic_tree.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT -//#include "aabb.h" #include "sample.h" #include "box2d/dynamic_tree.h" +#include "box2d/geometry.h" #include "box2d/math.h" #include diff --git a/samples/collection/sample_events.cpp b/samples/collection/sample_events.cpp index 76cb0897..81cd3bf2 100644 --- a/samples/collection/sample_events.cpp +++ b/samples/collection/sample_events.cpp @@ -582,7 +582,7 @@ class ContactEvent : public Sample { case b2_circleShape: { - b2Circle circle = *b2Shape_GetCircle(shapeId); + b2Circle circle = b2Shape_GetCircle(shapeId); circle.point = b2TransformPoint(relativeTransform, circle.point); b2CreateCircleShape(m_playerId, &shapeDef, &circle); @@ -591,7 +591,7 @@ class ContactEvent : public Sample case b2_capsuleShape: { - b2Capsule capsule = *b2Shape_GetCapsule(shapeId); + b2Capsule capsule = b2Shape_GetCapsule(shapeId); capsule.point1 = b2TransformPoint(relativeTransform, capsule.point1); capsule.point2 = b2TransformPoint(relativeTransform, capsule.point2); @@ -601,7 +601,8 @@ class ContactEvent : public Sample case b2_polygonShape: { - b2Polygon polygon = b2TransformPolygon(relativeTransform, b2Shape_GetPolygon(shapeId)); + b2Polygon originalPolygon = b2Shape_GetPolygon(shapeId); + b2Polygon polygon = b2TransformPolygon(relativeTransform, &originalPolygon); b2CreatePolygonShape(m_playerId, &shapeDef, &polygon); } diff --git a/samples/collection/sample_joints.cpp b/samples/collection/sample_joints.cpp index e8239955..887af900 100644 --- a/samples/collection/sample_joints.cpp +++ b/samples/collection/sample_joints.cpp @@ -7,9 +7,9 @@ #include "settings.h" #include "box2d/box2d.h" +#include "box2d/color.h" #include "box2d/geometry.h" #include "box2d/hull.h" -#include "box2d/joint_util.h" #include "box2d/math.h" #include @@ -101,7 +101,6 @@ class DistanceJoint : public Sample jointDef.length = m_length; jointDef.minLength = m_minLength; jointDef.maxLength = m_maxLength; - jointDef.collideConnected = true; m_jointIds[i] = b2CreateDistanceJoint(m_worldId, &jointDef); prevBodyId = m_bodyIds[i]; @@ -116,15 +115,19 @@ class DistanceJoint : public Sample if (ImGui::SliderFloat("length", &m_length, 0.1f, 4.0f, "%3.1f")) { - if (m_fixedLength) + for (int32_t i = 0; i < m_count; ++i) { - m_minLength = m_length; - m_maxLength = m_length; + b2DistanceJoint_SetLength(m_jointIds[i], m_length); } - for (int32_t i = 0; i < m_count; ++i) + if (m_fixedLength) { - b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); + m_minLength = m_length; + m_maxLength = m_length; + for (int32_t i = 0; i < m_count; ++i) + { + b2DistanceJoint_SetLengthRange(m_jointIds[i], m_minLength, m_maxLength); + } } } @@ -136,7 +139,8 @@ class DistanceJoint : public Sample m_maxLength = m_length; for (int32_t i = 0; i < m_count; ++i) { - b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); + b2DistanceJoint_SetLength(m_jointIds[i], m_length); + b2DistanceJoint_SetLengthRange(m_jointIds[i], m_minLength, m_maxLength); } } } @@ -147,7 +151,7 @@ class DistanceJoint : public Sample { for (int32_t i = 0; i < m_count; ++i) { - b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); + b2DistanceJoint_SetLengthRange(m_jointIds[i], m_minLength, m_maxLength); } } @@ -155,7 +159,7 @@ class DistanceJoint : public Sample { for (int32_t i = 0; i < m_count; ++i) { - b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); + b2DistanceJoint_SetLengthRange(m_jointIds[i], m_minLength, m_maxLength); } } @@ -724,6 +728,7 @@ class Bridge : public Sample b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); int32_t jointIndex = 0; m_frictionTorque = 200.0f; + m_gravityScale = 1.0f; float xbase = -80.0f; @@ -735,19 +740,19 @@ class Bridge : public Sample bodyDef.position = {xbase + 0.5f + 1.0f * i, 20.0f}; bodyDef.linearDamping = 0.1f; bodyDef.angularDamping = 0.1f; - b2BodyId bodyId = b2CreateBody(m_worldId, &bodyDef); - b2CreatePolygonShape(bodyId, &shapeDef, &box); + m_bodyIds[i] = b2CreateBody(m_worldId, &bodyDef); + b2CreatePolygonShape(m_bodyIds[i], &shapeDef, &box); b2Vec2 pivot = {xbase + 1.0f * i, 20.0f}; jointDef.bodyIdA = prevBodyId; - jointDef.bodyIdB = bodyId; + jointDef.bodyIdB = m_bodyIds[i]; jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); jointDef.enableMotor = true; jointDef.maxMotorTorque = m_frictionTorque; m_jointIds[jointIndex++] = b2CreateRevoluteJoint(m_worldId, &jointDef); - prevBodyId = bodyId; + prevBodyId = m_bodyIds[i]; } b2Vec2 pivot = {xbase + 1.0f * e_count, 20.0f}; @@ -812,6 +817,14 @@ class Bridge : public Sample } } + if (ImGui::SliderFloat("gravity scale", &m_gravityScale, -1.0f, 1.0f, "%.1f")) + { + for (int32_t i = 0; i < e_count; ++i) + { + b2Body_SetGravityScale(m_bodyIds[i], m_gravityScale); + } + } + ImGui::End(); } @@ -820,8 +833,10 @@ class Bridge : public Sample return new Bridge(settings); } + b2BodyId m_bodyIds[e_count]; b2JointId m_jointIds[e_count + 1]; float m_frictionTorque; + float m_gravityScale; }; static int sampleBridgeIndex = RegisterSample("Joints", "Bridge", Bridge::Create); @@ -962,6 +977,8 @@ class Cantilever : public Sample m_linearDampingRatio = 0.5f; m_angularHertz = 5.0f; m_angularDampingRatio = 0.5f; + m_gravityScale = 1.0f; + m_collideConnected = false; float hx = 0.5f; b2Capsule capsule = {{-hx, 0.0f}, {hx, 0.0f}, 0.125f}; @@ -976,21 +993,22 @@ class Cantilever : public Sample b2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = b2_dynamicBody; bodyDef.position = {(1.0f + 2.0f * i) * hx, 0.0f}; - b2BodyId bodyId = b2CreateBody(m_worldId, &bodyDef); - b2CreateCapsuleShape(bodyId, &shapeDef, &capsule); + m_bodyIds[i] = b2CreateBody(m_worldId, &bodyDef); + b2CreateCapsuleShape(m_bodyIds[i], &shapeDef, &capsule); b2Vec2 pivot = {(2.0f * i) * hx, 0.0f}; jointDef.bodyIdA = prevBodyId; - jointDef.bodyIdB = bodyId; + jointDef.bodyIdB = m_bodyIds[i]; jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); jointDef.linearHertz = m_linearHertz; jointDef.linearDampingRatio = m_linearDampingRatio; jointDef.angularHertz = m_angularHertz; jointDef.angularDampingRatio = m_angularDampingRatio; + jointDef.collideConnected = m_collideConnected; m_jointIds[i] = b2CreateWeldJoint(m_worldId, &jointDef); - prevBodyId = bodyId; + prevBodyId = m_bodyIds[i]; } m_tipId = prevBodyId; @@ -1035,6 +1053,22 @@ class Cantilever : public Sample } } + if (ImGui::Checkbox("collide connected", &m_collideConnected)) + { + for (int32_t i = 0; i < e_count; ++i) + { + b2Joint_SetCollideConnected(m_jointIds[i], m_collideConnected); + } + } + + if (ImGui::SliderFloat("gravity scale", &m_gravityScale, -1.0f, 1.0f, "%.1f")) + { + for (int32_t i = 0; i < e_count; ++i) + { + b2Body_SetGravityScale(m_bodyIds[i], m_gravityScale); + } + } + ImGui::End(); } @@ -1056,8 +1090,11 @@ class Cantilever : public Sample float m_linearDampingRatio; float m_angularHertz; float m_angularDampingRatio; + float m_gravityScale; b2BodyId m_tipId; + b2BodyId m_bodyIds[e_count]; b2JointId m_jointIds[e_count]; + bool m_collideConnected; }; static int sampleCantileverIndex = RegisterSample("Joints", "Cantilever", Cantilever::Create); @@ -1356,12 +1393,12 @@ class UserConstraint : public Sample float C = length - slackLength; if (C < 0.0f || length < 0.001f) { - g_draw.DrawSegment(anchorA, anchorB, b2MakeColor(b2_colorLightCyan, 1.0f)); + g_draw.DrawSegment(anchorA, anchorB, b2MakeColor(b2_colorLightCyan)); m_impulses[i] = 0.0f; continue; } - g_draw.DrawSegment(anchorA, anchorB, b2MakeColor(b2_colorViolet, 1.0f)); + g_draw.DrawSegment(anchorA, anchorB, b2MakeColor(b2_colorViolet)); b2Vec2 axis = b2Normalize(deltaAnchor); b2Vec2 rB = b2Sub(anchorB, pB); diff --git a/samples/collection/sample_manifold.cpp b/samples/collection/sample_manifold.cpp index b3fc82c3..ec5132c9 100644 --- a/samples/collection/sample_manifold.cpp +++ b/samples/collection/sample_manifold.cpp @@ -3,6 +3,7 @@ #include "sample.h" +#include "box2d/color.h" #include "box2d/distance.h" #include "box2d/geometry.h" #include "box2d/hull.h" @@ -549,9 +550,9 @@ class Manifold : public Sample b2Vec2 g2 = b2TransformPoint(xf1, segment.ghost2); b2Vec2 p1 = b2TransformPoint(xf1, segment.segment.point1); b2Vec2 p2 = b2TransformPoint(xf1, segment.segment.point2); - g_draw.DrawSegment(g1, p1, b2MakeColor(b2_colorLightGray, 0.5f)); + g_draw.DrawSegment(g1, p1, b2MakeColor(b2_colorLightGray)); g_draw.DrawSegment(p1, p2, color1); - g_draw.DrawSegment(p2, g2, b2MakeColor(b2_colorLightGray, 0.5f)); + g_draw.DrawSegment(p2, g2, b2MakeColor(b2_colorLightGray)); b2Vec2 c2 = b2TransformPoint(xf2, circle.point); b2Vec2 axis2 = b2RotateVector(xf2.q, {1.0f, 0.0f}); @@ -584,11 +585,11 @@ class Manifold : public Sample b2Vec2 g2 = b2TransformPoint(xf1, segment1.ghost2); b2Vec2 p1 = b2TransformPoint(xf1, segment1.segment.point1); b2Vec2 p2 = b2TransformPoint(xf1, segment1.segment.point2); - //g_draw.DrawSegment(g1, p1, b2MakeColor(b2_colorLightGray, 0.5f)); + //g_draw.DrawSegment(g1, p1, b2MakeColor(b2_colorLightGray)); g_draw.DrawSegment(p1, p2, color1); g_draw.DrawPoint(p1, 4.0f, color1); g_draw.DrawPoint(p2, 4.0f, color1); - g_draw.DrawSegment(p2, g2, b2MakeColor(b2_colorLightGray, 0.5f)); + g_draw.DrawSegment(p2, g2, b2MakeColor(b2_colorLightGray)); } { @@ -596,11 +597,11 @@ class Manifold : public Sample b2Vec2 g2 = b2TransformPoint(xf1, segment2.ghost2); b2Vec2 p1 = b2TransformPoint(xf1, segment2.segment.point1); b2Vec2 p2 = b2TransformPoint(xf1, segment2.segment.point2); - g_draw.DrawSegment(g1, p1, b2MakeColor(b2_colorLightGray, 0.5f)); + g_draw.DrawSegment(g1, p1, b2MakeColor(b2_colorLightGray)); g_draw.DrawSegment(p1, p2, color1); g_draw.DrawPoint(p1, 4.0f, color1); g_draw.DrawPoint(p2, 4.0f, color1); - //g_draw.DrawSegment(p2, g2, b2MakeColor(b2_colorLightGray, 0.5f)); + //g_draw.DrawSegment(p2, g2, b2MakeColor(b2_colorLightGray)); } b2Vec2 vertices[b2_maxPolygonVertices]; @@ -619,7 +620,7 @@ class Manifold : public Sample g_draw.DrawSolidPolygon(vertices, rox.count, color2); } - g_draw.DrawPoint(b2TransformPoint(xf2, rox.centroid), 5.0f, b2MakeColor(b2_colorGainsboro, 1.0f)); + g_draw.DrawPoint(b2TransformPoint(xf2, rox.centroid), 5.0f, b2MakeColor(b2_colorGainsboro)); DrawManifold(&m1); DrawManifold(&m2); @@ -644,11 +645,11 @@ class Manifold : public Sample b2Vec2 g2 = b2TransformPoint(xf1, segment1.ghost2); b2Vec2 p1 = b2TransformPoint(xf1, segment1.segment.point1); b2Vec2 p2 = b2TransformPoint(xf1, segment1.segment.point2); - //g_draw.DrawSegment(g1, p1, b2MakeColor(b2_colorLightGray, 0.5f)); + //g_draw.DrawSegment(g1, p1, b2MakeColor(b2_colorLightGray)); g_draw.DrawSegment(p1, p2, color1); g_draw.DrawPoint(p1, 4.0f, color1); g_draw.DrawPoint(p2, 4.0f, color1); - g_draw.DrawSegment(p2, g2, b2MakeColor(b2_colorLightGray, 0.5f)); + g_draw.DrawSegment(p2, g2, b2MakeColor(b2_colorLightGray)); } { @@ -656,18 +657,18 @@ class Manifold : public Sample b2Vec2 g2 = b2TransformPoint(xf1, segment2.ghost2); b2Vec2 p1 = b2TransformPoint(xf1, segment2.segment.point1); b2Vec2 p2 = b2TransformPoint(xf1, segment2.segment.point2); - g_draw.DrawSegment(g1, p1, b2MakeColor(b2_colorLightGray, 0.5f)); + g_draw.DrawSegment(g1, p1, b2MakeColor(b2_colorLightGray)); g_draw.DrawSegment(p1, p2, color1); g_draw.DrawPoint(p1, 4.0f, color1); g_draw.DrawPoint(p2, 4.0f, color1); - //g_draw.DrawSegment(p2, g2, b2MakeColor(b2_colorLightGray, 0.5f)); + //g_draw.DrawSegment(p2, g2, b2MakeColor(b2_colorLightGray)); } b2Vec2 p1 = b2TransformPoint(xf2, capsule.point1); b2Vec2 p2 = b2TransformPoint(xf2, capsule.point2); g_draw.DrawSolidCapsule(p1, p2, capsule.radius, color2); - g_draw.DrawPoint(b2Lerp(p1, p2, 0.5f), 5.0f, b2MakeColor(b2_colorGainsboro, 1.0f)); + g_draw.DrawPoint(b2Lerp(p1, p2, 0.5f), 5.0f, b2MakeColor(b2_colorGainsboro)); DrawManifold(&m1); DrawManifold(&m2); diff --git a/samples/collection/sample_ray_cast.cpp b/samples/collection/sample_ray_cast.cpp index 3e51ffea..aa89e078 100644 --- a/samples/collection/sample_ray_cast.cpp +++ b/samples/collection/sample_ray_cast.cpp @@ -4,6 +4,7 @@ #include "sample.h" #include "box2d/box2d.h" +#include "box2d/color.h" #include "box2d/geometry.h" #include "box2d/hull.h" #include "box2d/math.h" @@ -139,7 +140,7 @@ class RayCast : public Sample } } - void DrawRay(const b2RayCastOutput* output) + void DrawRay(const b2CastOutput* output) { b2Color white = {1.0f, 1.0f, 1.0f, 1.0f}; b2Color green = {0.0f, 1.0f, 0.0f, 1.0f}; @@ -194,7 +195,7 @@ class RayCast : public Sample b2Color color1 = {0.3f, 0.8f, 0.6f, 1.0f}; b2Color dim1 = {0.5f * color1.r, 0.5f * color1.g, 0.5f * color1.b, 1.0f}; - b2RayCastOutput output = {0}; + b2CastOutput output = {0}; float maxFraction = 1.0f; // circle @@ -208,7 +209,7 @@ class RayCast : public Sample b2Vec2 translation = b2InvRotateVector(xf.q, b2Sub(m_rayEnd, m_rayStart)); b2RayCastInput input = {start, translation, maxFraction}; - b2RayCastOutput localOutput = b2RayCastCircle(&input, &m_circle); + b2CastOutput localOutput = b2RayCastCircle(&input, &m_circle); if (localOutput.hit) { output = localOutput; @@ -231,7 +232,7 @@ class RayCast : public Sample b2Vec2 translation = b2InvRotateVector(xf.q, b2Sub(m_rayEnd, m_rayStart)); b2RayCastInput input = {start, translation, maxFraction}; - b2RayCastOutput localOutput = b2RayCastCapsule(&input, &m_capsule); + b2CastOutput localOutput = b2RayCastCapsule(&input, &m_capsule); if (localOutput.hit) { output = localOutput; @@ -258,7 +259,7 @@ class RayCast : public Sample b2Vec2 translation = b2InvRotateVector(xf.q, b2Sub(m_rayEnd, m_rayStart)); b2RayCastInput input = {start, translation, maxFraction}; - b2RayCastOutput localOutput = b2RayCastPolygon(&input, &m_box); + b2CastOutput localOutput = b2RayCastPolygon(&input, &m_box); if (localOutput.hit) { output = localOutput; @@ -285,7 +286,7 @@ class RayCast : public Sample b2Vec2 translation = b2InvRotateVector(xf.q, b2Sub(m_rayEnd, m_rayStart)); b2RayCastInput input = {start, translation, maxFraction}; - b2RayCastOutput localOutput = b2RayCastPolygon(&input, &m_triangle); + b2CastOutput localOutput = b2RayCastPolygon(&input, &m_triangle); if (localOutput.hit) { output = localOutput; @@ -309,7 +310,7 @@ class RayCast : public Sample b2Vec2 translation = b2InvRotateVector(xf.q, b2Sub(m_rayEnd, m_rayStart)); b2RayCastInput input = {start, translation, maxFraction}; - b2RayCastOutput localOutput = b2RayCastSegment(&input, &m_segment, false); + b2CastOutput localOutput = b2RayCastSegment(&input, &m_segment, false); if (localOutput.hit) { output = localOutput; @@ -806,9 +807,9 @@ class RayCastWorld : public Sample b2Color color1 = {0.4f, 0.9f, 0.4f, 1.0f}; b2Color color2 = {0.8f, 0.8f, 0.8f, 1.0f}; b2Color color3 = {0.9f, 0.9f, 0.4f, 1.0f}; - b2Color green = b2MakeColor(b2_colorGreen, 0.7f); - b2Color yellow = b2MakeColor(b2_colorYellow, 0.7f); - b2Color gray = b2MakeColor(b2_colorGray, 0.7f); + b2Color green = b2MakeColorAlpha(b2_colorGreen, 0.7f); + b2Color yellow = b2MakeColorAlpha(b2_colorYellow, 0.7f); + b2Color gray = b2MakeColorAlpha(b2_colorGray, 0.7f); b2Vec2 rayTranslation = b2Sub(m_rayEnd, m_rayStart); @@ -856,7 +857,8 @@ class RayCastWorld : public Sample m_textLine += m_textIncrement; - b2CastResultFcn* fcns[] = {RayCastAnyCallback, RayCastClosestCallback, RayCastMultipleCallback, RayCastSortedCallback}; + b2CastResultFcn* fcns[] = {RayCastAnyCallback, RayCastClosestCallback, RayCastMultipleCallback, + RayCastSortedCallback}; b2CastResultFcn* modeFcn = fcns[m_mode]; RayCastContext context = {0}; @@ -882,7 +884,8 @@ class RayCastWorld : public Sample break; case e_capsuleCast: - b2World_CapsuleCast(m_worldId, &capsule, transform, rayTranslation, b2DefaultQueryFilter(), modeFcn, &context); + b2World_CapsuleCast(m_worldId, &capsule, transform, rayTranslation, b2DefaultQueryFilter(), modeFcn, + &context); break; case e_polygonCast: @@ -893,8 +896,7 @@ class RayCastWorld : public Sample if (context.count > 0) { assert(context.count <= 3); - b2Color colors[3] = {b2MakeColor(b2_colorRed, 1.0f), b2MakeColor(b2_colorGreen, 1.0f), - b2MakeColor(b2_colorBlue, 1.0f)}; + b2Color colors[3] = {b2MakeColor(b2_colorRed), b2MakeColor(b2_colorGreen), b2MakeColor(b2_colorBlue)}; for (int i = 0; i < context.count; ++i) { b2Vec2 c = b2MulAdd(m_rayStart, context.fractions[i], rayTranslation); @@ -1281,12 +1283,12 @@ class OverlapWorld : public Sample g_draw.DrawString(5, m_textLine, "left mouse button: drag query shape"); m_textLine += m_textIncrement; - g_draw.DrawString(5, m_textLine, "left moust button + shift: rotate query shape"); + g_draw.DrawString(5, m_textLine, "left mouse button + shift: rotate query shape"); m_textLine += m_textIncrement; m_doomCount = 0; - b2Color color = b2MakeColor(b2_colorWhite, 1.0f); + b2Color color = b2MakeColor(b2_colorWhite); b2Transform transform = {m_position, b2MakeRot(m_angle)}; if (m_shapeType == e_circleShape) diff --git a/samples/collection/sample_robustness.cpp b/samples/collection/sample_robustness.cpp index a1c331a0..bff5be3f 100644 --- a/samples/collection/sample_robustness.cpp +++ b/samples/collection/sample_robustness.cpp @@ -7,7 +7,6 @@ #include "box2d/box2d.h" #include "box2d/geometry.h" #include "box2d/hull.h" -#include "box2d/joint_util.h" #include #include diff --git a/samples/collection/sample_shape_cast.cpp b/samples/collection/sample_shape_cast.cpp index 46274116..bb8a44ce 100644 --- a/samples/collection/sample_shape_cast.cpp +++ b/samples/collection/sample_shape_cast.cpp @@ -85,7 +85,7 @@ class ShapeCast : public Sample input.transformB = m_transformB; input.translationB = m_translationB; - b2RayCastOutput output = b2ShapeCast(&input); + b2CastOutput output = b2ShapeCast(&input); b2Transform transformB2; transformB2.q = m_transformB.q; diff --git a/samples/collection/sample_shapes.cpp b/samples/collection/sample_shapes.cpp index 67a8bb80..a42c5376 100644 --- a/samples/collection/sample_shapes.cpp +++ b/samples/collection/sample_shapes.cpp @@ -5,6 +5,7 @@ #include "settings.h" #include "box2d/box2d.h" +#include "box2d/color.h" #include "box2d/geometry.h" #include "box2d/hull.h" #include "box2d/math.h" @@ -205,8 +206,8 @@ class ChainShape : public Sample { Sample::Step(settings); - g_draw.DrawSegment(b2Vec2_zero, {0.5f, 0.0f}, b2MakeColor(b2_colorRed, 1.0f)); - g_draw.DrawSegment(b2Vec2_zero, {0.0f, 0.5f}, b2MakeColor(b2_colorGreen, 1.0f)); + g_draw.DrawSegment(b2Vec2_zero, {0.5f, 0.0f}, b2MakeColor(b2_colorRed)); + g_draw.DrawSegment(b2Vec2_zero, {0.0f, 0.5f}, b2MakeColor(b2_colorGreen)); } static Sample* Create(const Settings& settings) @@ -363,7 +364,7 @@ class CompoundShapes : public Sample bodyDef.type = b2_dynamicBody; bodyDef.position = b2Body_GetPosition(m_ship1Id); bodyDef.angle = b2Body_GetAngle(m_ship1Id); - //bodyDef.gravityScale = 0.0f; + // bodyDef.gravityScale = 0.0f; b2BodyId bodyId = b2CreateBody(m_worldId, &bodyDef); b2Circle circle = {{0.0f, 2.0f}, 0.5f}; @@ -376,7 +377,7 @@ class CompoundShapes : public Sample bodyDef.type = b2_dynamicBody; bodyDef.position = b2Body_GetPosition(m_ship2Id); bodyDef.angle = b2Body_GetAngle(m_ship2Id); - //bodyDef.gravityScale = 0.0f; + // bodyDef.gravityScale = 0.0f; b2BodyId bodyId = b2CreateBody(m_worldId, &bodyDef); b2Circle circle = {{0.0f, 2.0f}, 0.5f}; @@ -408,24 +409,22 @@ class CompoundShapes : public Sample { b2AABB aabb; - b2Color yellow = b2MakeColor(b2_colorYellow3, 0.5f); + b2Color yellow = b2MakeColorAlpha(b2_colorYellow3, 0.5f); aabb = b2Body_ComputeAABB(m_table1Id); g_draw.DrawAABB(aabb, yellow); - + aabb = b2Body_ComputeAABB(m_table2Id); g_draw.DrawAABB(aabb, yellow); - + aabb = b2Body_ComputeAABB(m_ship1Id); g_draw.DrawAABB(aabb, yellow); - + aabb = b2Body_ComputeAABB(m_ship2Id); g_draw.DrawAABB(aabb, yellow); } } - - static Sample* Create(const Settings& settings) { return new CompoundShapes(settings); @@ -443,7 +442,6 @@ static int sampleCompoundShape = RegisterSample("Shapes", "Compound Shapes", Com class ShapeFilter : public Sample { public: - enum CollisionBits { GROUND = 0x00000001, @@ -478,7 +476,7 @@ class ShapeFilter : public Sample { b2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = b2_dynamicBody; - + bodyDef.position = {0.0f, 4.0f}; m_player1Id = b2CreateBody(m_worldId, &bodyDef); @@ -487,7 +485,7 @@ class ShapeFilter : public Sample bodyDef.position = {0.0f, 12.0f}; m_player3Id = b2CreateBody(m_worldId, &bodyDef); - + b2Polygon box = b2MakeBox(2.0f, 1.0f); b2ShapeDef shapeDef = b2DefaultShapeDef(); @@ -832,3 +830,162 @@ class Friction : public Sample }; static int sampleIndex3 = RegisterSample("Shapes", "Friction", Friction::Create); + +// This sample shows how to modify the geometry on an existing shape. This is only supported on +// dynamic and kinematic shapes because static shapes don't look for new collisions. +class ModifyGeometry : public Sample +{ +public: + ModifyGeometry(const Settings& settings) + : Sample(settings) + { + if (settings.restart == false) + { + g_camera.m_zoom = 0.25f; + g_camera.m_center = {0.0f, 5.0f}; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody(m_worldId, &bodyDef); + b2Polygon box = b2MakeOffsetBox(10.0f, 1.0f, {0.0f, -1.0f}, 0.0f); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape(groundId, &shapeDef, &box); + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = {0.0f, 4.0f}; + b2BodyId bodyId = b2CreateBody(m_worldId, &bodyDef); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeBox(1.0f, 1.0f); + b2CreatePolygonShape(bodyId, &shapeDef, &box); + } + + { + m_shapeType = b2_circleShape; + m_scale = 1.0f; + m_circle = {{0.0f, 0.0f}, 0.5f}; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_kinematicBody; + bodyDef.position = {0.0f, 1.0f}; + b2BodyId bodyId = b2CreateBody(m_worldId, &bodyDef); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + m_shapeId = b2CreateCircleShape(bodyId, &shapeDef, &m_circle); + } + } + + void UpdateShape() + { + switch (m_shapeType) + { + case b2_circleShape: + m_circle = {{0.0f, 0.0f}, 0.5f * m_scale}; + b2Shape_SetCircle(m_shapeId, m_circle); + break; + + case b2_capsuleShape: + m_capsule = {{-0.5f * m_scale, 0.0f}, {0.0f, 0.5f * m_scale}, 0.5f * m_scale}; + b2Shape_SetCapsule(m_shapeId, m_capsule); + break; + + case b2_segmentShape: + m_segment = {{-0.5f * m_scale, 0.0f}, {0.75f * m_scale, 0.0f}}; + b2Shape_SetSegment(m_shapeId, m_segment); + break; + + case b2_polygonShape: + m_polygon = b2MakeBox(0.5f * m_scale, 0.75f * m_scale); + b2Shape_SetPolygon(m_shapeId, m_polygon); + break; + + default: + assert(false); + break; + } + + b2BodyId bodyId = b2Shape_GetBody(m_shapeId); + b2Body_ResetMassData(bodyId); + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); + ImGui::SetNextWindowSize(ImVec2(250.0f, 240.0f)); + ImGui::Begin("Modify Geometry", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize); + + if (ImGui::RadioButton("Circle", m_shapeType == b2_circleShape)) + { + m_shapeType = b2_circleShape; + UpdateShape(); + } + + if (ImGui::RadioButton("Capsule", m_shapeType == b2_capsuleShape)) + { + m_shapeType = b2_capsuleShape; + UpdateShape(); + } + + if (ImGui::RadioButton("Segment", m_shapeType == b2_segmentShape)) + { + m_shapeType = b2_segmentShape; + UpdateShape(); + } + + if (ImGui::RadioButton("Polygon", m_shapeType == b2_polygonShape)) + { + m_shapeType = b2_polygonShape; + UpdateShape(); + } + + if (ImGui::SliderFloat("Scale", &m_scale, 0.1f, 10.0f, "%.2f")) + { + UpdateShape(); + } + + b2BodyId bodyId = b2Shape_GetBody(m_shapeId); + b2BodyType bodyType = b2Body_GetType(bodyId); + + if (ImGui::RadioButton("Static", bodyType == b2_staticBody)) + { + b2Body_SetType(bodyId, b2_staticBody); + } + + if (ImGui::RadioButton("Kinematic", bodyType == b2_kinematicBody)) + { + b2Body_SetType(bodyId, b2_kinematicBody); + } + + if (ImGui::RadioButton("Dynamic", bodyType == b2_dynamicBody)) + { + b2Body_SetType(bodyId, b2_dynamicBody); + } + + ImGui::End(); + } + + void Step(Settings& settings) override + { + Sample::Step(settings); + } + + static Sample* Create(const Settings& settings) + { + return new ModifyGeometry(settings); + } + + b2ShapeId m_shapeId; + b2ShapeType m_shapeType; + float m_scale; + + union + { + b2Circle m_circle; + b2Capsule m_capsule; + b2Segment m_segment; + b2Polygon m_polygon; + }; +}; + +static int sampleModifyGeometry = RegisterSample("Shapes", "Modify Geometry", ModifyGeometry::Create); diff --git a/samples/sample.cpp b/samples/sample.cpp index 92169333..be62372c 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -7,9 +7,9 @@ #include "box2d/box2d.h" #include "box2d/callbacks.h" -#include "box2d/joint_util.h" #include "box2d/manifold.h" #include "box2d/math.h" +#include "box2d/math_cpp.h" #include "box2d/timer.h" #include @@ -49,6 +49,34 @@ static void FinishTask(void* taskPtr, void* userContext) } } +static void TestMathCpp() +{ + b2Vec2 a = {1.0f, 2.0f}; + b2Vec2 b = {3.0f, 4.0f}; + + b2Vec2 c = a; + c += b; + c -= b; + c *= 2.0f; + c = -a; + c = c + b; + c = c - a; + c = 2.0f * a; + c = a * 2.0f; + + if (b == a) + { + c = a; + } + + if (b != a) + { + c = b; + } + + c += c; +} + Sample::Sample(const Settings& settings) { b2Vec2 gravity = {0.0f, -10.0f}; @@ -79,6 +107,8 @@ Sample::Sample(const Settings& settings) m_maxProfile = {}; m_totalProfile = {}; + + TestMathCpp(); } Sample::~Sample() diff --git a/samples/settings.h b/samples/settings.h index c0450a0f..67b7a9b3 100644 --- a/samples/settings.h +++ b/samples/settings.h @@ -28,7 +28,7 @@ struct Settings bool drawProfile = false; bool enableWarmStarting = true; bool enableContinuous = true; - bool enableSleep = false; + bool enableSleep = true; bool pause = false; bool singleStep = false; bool restart = false; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 337aa149..e8d8a3a1 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -3,8 +3,6 @@ set(BOX2D_SOURCE_FILES aabb.h allocate.c allocate.h - arena_allocator.c - arena_allocator.h array.c array.h bitset.c @@ -16,6 +14,8 @@ set(BOX2D_SOURCE_FILES body.h broad_phase.c broad_phase.h + constraint_graph.c + constraint_graph.h contact.c contact.h contact_solver.c @@ -26,9 +26,8 @@ set(BOX2D_SOURCE_FILES distance_joint.c dynamic_tree.c geometry.c - graph.c - graph.h hull.c + implementation.c island.c island.h joint.c @@ -44,7 +43,10 @@ set(BOX2D_SOURCE_FILES revolute_joint.c shape.c shape.h - solver_data.h + solver.c + solver.h + stack_allocator.c + stack_allocator.h table.c table.h timer.c @@ -70,10 +72,10 @@ set(BOX2D_API_FILES ../include/box2d/hull.h ../include/box2d/id.h ../include/box2d/joint_types.h - ../include/box2d/joint_util.h ../include/box2d/manifold.h ../include/box2d/math.h ../include/box2d/math_cpp.h + ../include/box2d/math_types.h ../include/box2d/timer.h ../include/box2d/types.h ) diff --git a/src/aabb.c b/src/aabb.c index 4ae9533e..c53c7aa4 100644 --- a/src/aabb.c +++ b/src/aabb.c @@ -4,6 +4,7 @@ #include "aabb.h" #include "box2d/constants.h" +#include "box2d/geometry.h" #include "box2d/math.h" #include @@ -17,10 +18,10 @@ bool b2AABB_IsValid(b2AABB a) } // From Real-time Collision Detection, p179. -b2RayCastOutput b2AABB_RayCast(b2AABB a, b2Vec2 p1, b2Vec2 p2) +b2CastOutput b2AABB_RayCast(b2AABB a, b2Vec2 p1, b2Vec2 p2) { // Radius not handled - b2RayCastOutput output = {0}; + b2CastOutput output = {0}; float tmin = -FLT_MAX; float tmax = FLT_MAX; diff --git a/src/aabb.h b/src/aabb.h index 859bbcb4..d43e7cd9 100644 --- a/src/aabb.h +++ b/src/aabb.h @@ -5,10 +5,10 @@ #include "box2d/constants.h" #include "box2d/math.h" -#include "box2d/types.h" +#include "box2d/geometry.h" // Ray cast an AABB -b2RayCastOutput b2AABB_RayCast(b2AABB a, b2Vec2 p1, b2Vec2 p2); +b2CastOutput b2AABB_RayCast(b2AABB a, b2Vec2 p1, b2Vec2 p2); // Get the perimeter length static inline float b2Perimeter(b2AABB a) diff --git a/src/body.c b/src/body.c index acf9570c..b35f72d8 100644 --- a/src/body.c +++ b/src/body.c @@ -7,9 +7,9 @@ #include "allocate.h" #include "array.h" #include "block_allocator.h" +#include "constraint_graph.h" #include "contact.h" #include "core.h" -#include "graph.h" #include "island.h" #include "joint.h" #include "shape.h" @@ -25,7 +25,7 @@ b2Body* b2GetBody(b2World* world, b2BodyId id) { B2_ASSERT(1 <= id.index && id.index <= world->bodyPool.capacity); b2Body* body = world->bodies + (id.index - 1); - B2_ASSERT(b2ObjectValid(&body->object)); + B2_ASSERT(b2IsValidObject(&body->object)); B2_ASSERT(id.revision == body->object.revision); return body; } @@ -219,7 +219,7 @@ b2BodyId b2CreateBody(b2WorldId worldId, const b2BodyDef* def) B2_ASSERT(b2IsValid(def->angularVelocity)); B2_ASSERT(b2IsValid(def->linearDamping) && def->linearDamping >= 0.0f); B2_ASSERT(b2IsValid(def->angularDamping) && def->angularDamping >= 0.0f); - B2_ASSERT(b2IsValid(def->gravityScale) && def->gravityScale >= 0.0f); + B2_ASSERT(b2IsValid(def->gravityScale)); body->type = def->type; body->origin = def->position; @@ -341,6 +341,33 @@ void b2DestroyBody(b2BodyId bodyId) b2DestroyBodyInternal(world, body); } +void b2DestroyBodyAndJoints(b2BodyId bodyId) +{ + b2World* world = b2GetWorldFromIndexLocked(bodyId.world); + if (world == NULL) + { + return; + } + + b2Body* body = b2GetBody(world, bodyId); + + // Destroy the attached joints + int32_t edgeKey = body->jointList; + while (edgeKey != B2_NULL_INDEX) + { + int32_t jointIndex = edgeKey >> 1; + int32_t edgeIndex = edgeKey & 1; + + b2Joint* joint = world->joints + jointIndex; + edgeKey = joint->edges[edgeIndex].nextKey; + + // Careful because this modifies the list being traversed + b2DestroyJointInternal(world, joint); + } + + b2DestroyBodyInternal(world, body); +} + int32_t b2Body_GetContactCapacity(b2BodyId bodyId) { b2World* world = b2GetWorldFromIndexLocked(bodyId.world); @@ -563,6 +590,7 @@ static b2ShapeId b2CreateShape(b2BodyId bodyId, const b2ShapeDef* def, const voi // Add to shape linked list shape->nextShapeIndex = body->shapeList; body->shapeList = shape->object.index; + body->shapeCount += 1; if (shape->density > 0.0f) { @@ -637,6 +665,8 @@ static void b2DestroyShapeInternal(b2World* world, b2Shape* shape) return; } + body->shapeCount -= 1; + const float density = shape->density; // Destroy any contacts associated with the shape @@ -1125,6 +1155,13 @@ void b2Body_SetType(b2BodyId bodyId, b2BodyType type) b2UpdateBodyMassData(world, body); } +void b2Body_SetUserData(b2BodyId bodyId, void* userData) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + body->userData = userData; +} + void* b2Body_GetUserData(b2BodyId bodyId) { b2World* world = b2GetWorldFromIndex(bodyId.world); @@ -1193,6 +1230,19 @@ b2MassData b2Body_GetMassData(b2BodyId bodyId) return massData; } +void b2Body_ResetMassData(b2BodyId bodyId) +{ + b2World* world = b2GetWorldFromIndexLocked(bodyId.world); + if (world == NULL) + { + return; + } + + b2Body* body = b2GetBody(world, bodyId); + + b2UpdateBodyMassData(world, body); +} + void b2Body_SetLinearDamping(b2BodyId bodyId, float linearDamping) { B2_ASSERT(b2IsValid(linearDamping) && linearDamping >= 0.0f); @@ -1237,7 +1287,7 @@ float b2Body_GetAngularDamping(b2BodyId bodyId) void b2Body_SetGravityScale(b2BodyId bodyId, float gravityScale) { - B2_ASSERT(b2IsValid(gravityScale) && gravityScale >= 0.0f); + B2_ASSERT(b2IsValid(gravityScale)); b2World* world = b2GetWorldFromIndexLocked(bodyId.world); if (world == NULL) @@ -1287,6 +1337,30 @@ bool b2Body_IsEnabled(b2BodyId bodyId) return body->isEnabled; } +bool b2Body_IsSleepEnabled(b2BodyId bodyId) +{ + b2World* world = b2GetWorldFromIndex(bodyId.world); + b2Body* body = b2GetBody(world, bodyId); + return body->enableSleep; +} + +void b2Body_EnableSleep(b2BodyId bodyId, bool enableSleep) +{ + b2World* world = b2GetWorldFromIndexLocked(bodyId.world); + if (world == NULL) + { + return; + } + + b2Body* body = b2GetBody(world, bodyId); + body->enableSleep = enableSleep; + + if (enableSleep == false) + { + b2WakeBody(world, body); + } +} + void b2Body_Disable(b2BodyId bodyId) { b2World* world = b2GetWorldFromIndexLocked(bodyId.world); diff --git a/src/body.h b/src/body.h index 6a3fb9cb..69e7e5af 100644 --- a/src/body.h +++ b/src/body.h @@ -78,6 +78,8 @@ typedef struct b2Body float torque; int32_t shapeList; + int32_t shapeCount; + int32_t chainList; // This is a key: [jointIndex:31, edgeIndex:1] @@ -122,6 +124,7 @@ typedef struct b2Body b2Body* b2GetBody(b2World* world, b2BodyId id); bool b2ShouldBodiesCollide(b2World* world, b2Body* bodyA, b2Body* bodyB); bool b2IsBodyAwake(b2World* world, b2Body* body); +void b2WakeBody(b2World* world, b2Body* body); void b2UpdateBodyMassData(b2World* world, b2Body* body); static inline b2Transform b2MakeTransform(const b2Body* body) diff --git a/src/broad_phase.c b/src/broad_phase.c index a3e2bbfb..630b8e5b 100644 --- a/src/broad_phase.c +++ b/src/broad_phase.c @@ -7,7 +7,7 @@ #include "aabb.h" #include "allocate.h" -#include "arena_allocator.h" +#include "stack_allocator.h" #include "array.h" #include "body.h" #include "contact.h" @@ -229,13 +229,13 @@ static bool b2PairQueryCallback(int32_t proxyId, int32_t shapeIndex, void* conte b2Body* bodyB = world->bodies + bodyIndexB; // Does a joint override collision? Is at least one body dynamic? - // TODO_ERIN this could be a hash set + // #todo this could be a hash set if (b2ShouldBodiesCollide(world, bodyA, bodyB) == false) { return true; } - // TODO_ERIN per thread to eliminate atomic? + // #todo per thread to eliminate atomic? int pairIndex = atomic_fetch_add(&bp->movePairIndex, 1); b2MovePair* pair; diff --git a/src/broad_phase.h b/src/broad_phase.h index d2ce7dd4..55293903 100644 --- a/src/broad_phase.h +++ b/src/broad_phase.h @@ -7,6 +7,7 @@ #include "table.h" #include "box2d/dynamic_tree.h" +#include "box2d/types.h" typedef struct b2Shape b2Shape; typedef struct b2MovePair b2MovePair; @@ -40,6 +41,7 @@ typedef struct b2BroadPhase int32_t movePairCapacity; _Atomic int movePairIndex; + // Tracks shape pairs that have a b2Contact b2HashSet pairSet; } b2BroadPhase; diff --git a/src/constraint_graph.c b/src/constraint_graph.c new file mode 100644 index 00000000..c36e0407 --- /dev/null +++ b/src/constraint_graph.c @@ -0,0 +1,401 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "constraint_graph.h" + +#include "array.h" +#include "bitset.inl" +#include "body.h" +#include "contact.h" +#include "joint.h" +#include "world.h" + +#include + +// Solver using graph coloring. Islands are only used for sleep. +// High-Performance Physical Simulations on Next-Generation Architecture with Many Cores +// http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf + +// Kinematic bodies have to be treated like dynamic bodies in graph coloring. Unlike static bodies, we cannot use a dummy solver +// body for kinematic bodies. We cannot access a kinematic body from multiple threads efficiently because the SIMD solver body +// scatter would write to the same kinematic body from multiple threads. Even if these writes don't modify the body, they will +// cause horrible cache stalls. To make this feasible I would need a way to block these writes. + +// This is used for debugging by making all constraints be assigned to overflow. +#define B2_FORCE_OVERFLOW 0 + +void b2CreateGraph(b2ConstraintGraph* graph, int32_t bodyCapacity, int32_t contactCapacity, int32_t jointCapacity) +{ + *graph = (b2ConstraintGraph){0}; + + bodyCapacity = B2_MAX(bodyCapacity, 8); + contactCapacity = B2_MAX(contactCapacity, 8); + jointCapacity = B2_MAX(jointCapacity, 8); + + for (int32_t i = 0; i < b2_graphColorCount; ++i) + { + b2GraphColor* color = graph->colors + i; + color->bodySet = b2CreateBitSet(bodyCapacity); + b2SetBitCountAndClear(&color->bodySet, bodyCapacity); + + color->contactArray = b2CreateArray(sizeof(int32_t), contactCapacity); + color->jointArray = b2CreateArray(sizeof(int32_t), jointCapacity); + color->contactConstraints = NULL; + } + + graph->overflow.contactArray = b2CreateArray(sizeof(int32_t), contactCapacity); + graph->overflow.jointArray = b2CreateArray(sizeof(int32_t), jointCapacity); + graph->overflow.contactConstraints = NULL; +} + +void b2DestroyGraph(b2ConstraintGraph* graph) +{ + for (int32_t i = 0; i < b2_graphColorCount; ++i) + { + b2GraphColor* color = graph->colors + i; + b2DestroyBitSet(&color->bodySet); + b2DestroyArray(color->contactArray, sizeof(int32_t)); + b2DestroyArray(color->jointArray, sizeof(int32_t)); + } + + b2DestroyArray(graph->overflow.contactArray, sizeof(int32_t)); + b2DestroyArray(graph->overflow.jointArray, sizeof(int32_t)); +} + +void b2AddContactToGraph(b2World* world, b2Contact* contact) +{ + B2_ASSERT(contact->colorIndex == B2_NULL_INDEX); + B2_ASSERT(contact->colorSubIndex == B2_NULL_INDEX); + + b2ConstraintGraph* graph = &world->graph; + +#if B2_FORCE_OVERFLOW == 0 + int32_t bodyIndexA = contact->edges[0].bodyIndex; + int32_t bodyIndexB = contact->edges[1].bodyIndex; + + b2BodyType typeA = world->bodies[bodyIndexA].type; + b2BodyType typeB = world->bodies[bodyIndexB].type; + B2_ASSERT(typeA != b2_staticBody || typeB != b2_staticBody); + + if (typeA != b2_staticBody && typeB != b2_staticBody) + { + for (int32_t i = 0; i < b2_graphColorCount; ++i) + { + b2GraphColor* color = graph->colors + i; + if (b2GetBit(&color->bodySet, bodyIndexA) || b2GetBit(&color->bodySet, bodyIndexB)) + { + continue; + } + + b2SetBitGrow(&color->bodySet, bodyIndexA); + b2SetBitGrow(&color->bodySet, bodyIndexB); + + contact->colorSubIndex = b2Array(color->contactArray).count; + b2Array_Push(color->contactArray, contact->object.index); + contact->colorIndex = i; + break; + } + } + else if (typeA != b2_staticBody) + { + // Static contacts never in color 0 + for (int32_t i = 1; i < b2_graphColorCount; ++i) + { + b2GraphColor* color = graph->colors + i; + if (b2GetBit(&color->bodySet, bodyIndexA)) + { + continue; + } + + b2SetBitGrow(&color->bodySet, bodyIndexA); + + contact->colorSubIndex = b2Array(color->contactArray).count; + b2Array_Push(color->contactArray, contact->object.index); + contact->colorIndex = i; + break; + } + } + else if (typeB != b2_staticBody) + { + // Static contacts never in color 0 + for (int32_t i = 1; i < b2_graphColorCount; ++i) + { + b2GraphColor* color = graph->colors + i; + if (b2GetBit(&color->bodySet, bodyIndexB)) + { + continue; + } + + b2SetBitGrow(&color->bodySet, bodyIndexB); + + contact->colorSubIndex = b2Array(color->contactArray).count; + b2Array_Push(color->contactArray, contact->object.index); + contact->colorIndex = i; + break; + } + } +#endif + + // Overflow + if (contact->colorIndex == B2_NULL_INDEX) + { + contact->colorSubIndex = b2Array(graph->overflow.contactArray).count; + b2Array_Push(graph->overflow.contactArray, contact->object.index); + contact->colorIndex = b2_overflowIndex; + } +} + +void b2RemoveContactFromGraph(b2World* world, b2Contact* contact) +{ + B2_ASSERT(contact->colorIndex != B2_NULL_INDEX); + B2_ASSERT(contact->colorSubIndex != B2_NULL_INDEX); + + b2ConstraintGraph* graph = &world->graph; + + // Overflow + if (contact->colorIndex == b2_overflowIndex) + { + int32_t colorSubIndex = contact->colorSubIndex; + b2Array_RemoveSwap(graph->overflow.contactArray, colorSubIndex); + if (colorSubIndex < b2Array(graph->overflow.contactArray).count) + { + // Fix index on swapped contact + int32_t swappedIndex = graph->overflow.contactArray[colorSubIndex]; + B2_ASSERT(world->contacts[swappedIndex].colorIndex == b2_overflowIndex); + world->contacts[swappedIndex].colorSubIndex = colorSubIndex; + } + + contact->colorIndex = B2_NULL_INDEX; + contact->colorSubIndex = B2_NULL_INDEX; + + return; + } + + B2_ASSERT(0 <= contact->colorIndex && contact->colorIndex < b2_graphColorCount); + int32_t bodyIndexA = contact->edges[0].bodyIndex; + int32_t bodyIndexB = contact->edges[1].bodyIndex; + + b2BodyType typeA = world->bodies[bodyIndexA].type; + b2BodyType typeB = world->bodies[bodyIndexB].type; + B2_ASSERT(typeA != b2_staticBody || typeB != b2_staticBody); + + if (typeA != b2_staticBody && typeB != b2_staticBody) + { + b2GraphColor* color = graph->colors + contact->colorIndex; + B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA) && b2GetBit(&color->bodySet, bodyIndexB)); + + int32_t colorSubIndex = contact->colorSubIndex; + b2Array_RemoveSwap(color->contactArray, colorSubIndex); + if (colorSubIndex < b2Array(color->contactArray).count) + { + // Fix index on swapped contact + int32_t swappedIndex = color->contactArray[colorSubIndex]; + world->contacts[swappedIndex].colorSubIndex = colorSubIndex; + } + + b2ClearBit(&color->bodySet, bodyIndexA); + b2ClearBit(&color->bodySet, bodyIndexB); + } + else if (typeA != b2_staticBody) + { + b2GraphColor* color = graph->colors + contact->colorIndex; + B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA)); + + int32_t colorSubIndex = contact->colorSubIndex; + b2Array_RemoveSwap(color->contactArray, colorSubIndex); + if (colorSubIndex < b2Array(color->contactArray).count) + { + // Fix index on swapped contact + int32_t swappedIndex = color->contactArray[colorSubIndex]; + world->contacts[swappedIndex].colorSubIndex = colorSubIndex; + } + + b2ClearBit(&color->bodySet, bodyIndexA); + } + else if (typeB != b2_staticBody) + { + b2GraphColor* color = graph->colors + contact->colorIndex; + B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexB)); + + int32_t colorSubIndex = contact->colorSubIndex; + b2Array_RemoveSwap(color->contactArray, colorSubIndex); + if (colorSubIndex < b2Array(color->contactArray).count) + { + // Fix index on swapped contact + int32_t swappedIndex = color->contactArray[colorSubIndex]; + world->contacts[swappedIndex].colorSubIndex = colorSubIndex; + } + + b2ClearBit(&color->bodySet, bodyIndexB); + } + + contact->colorIndex = B2_NULL_INDEX; + contact->colorSubIndex = B2_NULL_INDEX; +} + +void b2AddJointToGraph(b2World* world, b2Joint* joint) +{ + B2_ASSERT(joint->colorIndex == B2_NULL_INDEX); + B2_ASSERT(joint->colorSubIndex == B2_NULL_INDEX); + + b2ConstraintGraph* graph = &world->graph; + +#if B2_FORCE_OVERFLOW == 0 + int32_t bodyIndexA = joint->edges[0].bodyIndex; + int32_t bodyIndexB = joint->edges[1].bodyIndex; + + b2BodyType typeA = world->bodies[bodyIndexA].type; + b2BodyType typeB = world->bodies[bodyIndexB].type; + + if (typeA == b2_dynamicBody && typeB == b2_dynamicBody) + { + for (int32_t i = 0; i < b2_graphColorCount; ++i) + { + b2GraphColor* color = graph->colors + i; + if (b2GetBit(&color->bodySet, bodyIndexA) || b2GetBit(&color->bodySet, bodyIndexB)) + { + continue; + } + + b2SetBitGrow(&color->bodySet, bodyIndexA); + b2SetBitGrow(&color->bodySet, bodyIndexB); + + joint->colorSubIndex = b2Array(color->jointArray).count; + b2Array_Push(color->jointArray, joint->object.index); + joint->colorIndex = i; + break; + } + } + else if (typeA == b2_dynamicBody) + { + for (int32_t i = 0; i < b2_graphColorCount; ++i) + { + b2GraphColor* color = graph->colors + i; + if (b2GetBit(&color->bodySet, bodyIndexA)) + { + continue; + } + + b2SetBitGrow(&color->bodySet, bodyIndexA); + + joint->colorSubIndex = b2Array(color->jointArray).count; + b2Array_Push(color->jointArray, joint->object.index); + joint->colorIndex = i; + break; + } + } + else if (typeB == b2_dynamicBody) + { + for (int32_t i = 0; i < b2_graphColorCount; ++i) + { + b2GraphColor* color = graph->colors + i; + if (b2GetBit(&color->bodySet, bodyIndexB)) + { + continue; + } + + b2SetBitGrow(&color->bodySet, bodyIndexB); + + joint->colorSubIndex = b2Array(color->jointArray).count; + b2Array_Push(color->jointArray, joint->object.index); + joint->colorIndex = i; + break; + } + } +#endif + + // Overflow + if (joint->colorIndex == B2_NULL_INDEX) + { + joint->colorSubIndex = b2Array(graph->overflow.jointArray).count; + b2Array_Push(graph->overflow.jointArray, joint->object.index); + joint->colorIndex = b2_overflowIndex; + } +} + +void b2RemoveJointFromGraph(b2World* world, b2Joint* joint) +{ + B2_ASSERT(joint->colorIndex != B2_NULL_INDEX); + B2_ASSERT(joint->colorSubIndex != B2_NULL_INDEX); + + b2ConstraintGraph* graph = &world->graph; + + // Overflow + if (joint->colorIndex == b2_overflowIndex) + { + int32_t colorSubIndex = joint->colorSubIndex; + b2Array_RemoveSwap(graph->overflow.jointArray, colorSubIndex); + if (colorSubIndex < b2Array(graph->overflow.jointArray).count) + { + // Fix index on swapped joint + int32_t swappedIndex = graph->overflow.jointArray[colorSubIndex]; + B2_ASSERT(world->joints[swappedIndex].colorIndex == b2_overflowIndex); + world->joints[swappedIndex].colorSubIndex = colorSubIndex; + } + + joint->colorIndex = B2_NULL_INDEX; + joint->colorSubIndex = B2_NULL_INDEX; + + return; + } + + B2_ASSERT(0 <= joint->colorIndex && joint->colorIndex < b2_graphColorCount); + int32_t bodyIndexA = joint->edges[0].bodyIndex; + int32_t bodyIndexB = joint->edges[1].bodyIndex; + + b2BodyType typeA = world->bodies[bodyIndexA].type; + b2BodyType typeB = world->bodies[bodyIndexB].type; + + if (typeA == b2_dynamicBody && typeB == b2_dynamicBody) + { + b2GraphColor* color = graph->colors + joint->colorIndex; + B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA) && b2GetBit(&color->bodySet, bodyIndexB)); + + int32_t colorSubIndex = joint->colorSubIndex; + b2Array_RemoveSwap(color->jointArray, colorSubIndex); + if (colorSubIndex < b2Array(color->jointArray).count) + { + // Fix index on swapped joint + int32_t swappedIndex = color->jointArray[colorSubIndex]; + world->joints[swappedIndex].colorSubIndex = colorSubIndex; + } + + b2ClearBit(&color->bodySet, bodyIndexA); + b2ClearBit(&color->bodySet, bodyIndexB); + } + else if (typeA == b2_dynamicBody) + { + b2GraphColor* color = graph->colors + joint->colorIndex; + B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA)); + + int32_t colorSubIndex = joint->colorSubIndex; + b2Array_RemoveSwap(color->jointArray, colorSubIndex); + if (colorSubIndex < b2Array(color->jointArray).count) + { + // Fix index on swapped joint + int32_t swappedIndex = color->jointArray[colorSubIndex]; + world->joints[swappedIndex].colorSubIndex = colorSubIndex; + } + + b2ClearBit(&color->bodySet, bodyIndexA); + } + else if (typeB == b2_dynamicBody) + { + b2GraphColor* color = graph->colors + joint->colorIndex; + B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexB)); + + int32_t colorSubIndex = joint->colorSubIndex; + b2Array_RemoveSwap(color->jointArray, colorSubIndex); + if (colorSubIndex < b2Array(color->jointArray).count) + { + // Fix index on swapped joint + int32_t swappedIndex = color->jointArray[colorSubIndex]; + world->joints[swappedIndex].colorSubIndex = colorSubIndex; + } + + b2ClearBit(&color->bodySet, bodyIndexB); + } + + joint->colorIndex = B2_NULL_INDEX; + joint->colorSubIndex = B2_NULL_INDEX; +} diff --git a/src/graph.h b/src/constraint_graph.h similarity index 96% rename from src/graph.h rename to src/constraint_graph.h index 2f65d5c7..f8b3464d 100644 --- a/src/graph.h +++ b/src/constraint_graph.h @@ -56,5 +56,3 @@ void b2RemoveContactFromGraph(b2World* world, b2Contact* contact); void b2AddJointToGraph(b2World* world, b2Joint* joint); void b2RemoveJointFromGraph(b2World* world, b2Joint* joint); - -void b2Solve(b2World* world, b2StepContext* stepContext); diff --git a/src/contact.c b/src/contact.c index 48004603..f4de7fee 100644 --- a/src/contact.c +++ b/src/contact.c @@ -382,6 +382,9 @@ void b2DestroyContact(b2World* world, b2Contact* contact) world->contactAwakeIndexArray[contactIndex] = B2_NULL_INDEX; } + b2WakeBody(world, bodyA); + b2WakeBody(world, bodyB); + b2FreeObject(&world->contactPool, &contact->object); } diff --git a/src/contact_solver.c b/src/contact_solver.c index 89eb730b..da09250c 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -5,14 +5,14 @@ #include "array.h" #include "body.h" +#include "constraint_graph.h" #include "contact.h" #include "core.h" -#include "graph.h" #include "world.h" #include "x86/avx2.h" #include "x86/fma.h" -// Soft constraints with constraint error sub-stepping. Includes a relax stage to help remove excess energy. +// Soft contact constraints with sub-stepping support // http://mmacklin.com/smallsteps.pdf // https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf diff --git a/src/contact_solver.h b/src/contact_solver.h index 2b63671b..d93fb811 100644 --- a/src/contact_solver.h +++ b/src/contact_solver.h @@ -3,12 +3,11 @@ #pragma once -#include "solver_data.h" +#include "solver.h" -// todo this could be hidden in contact_solver.c, then graph.c just needs to know the sizeof(b2ContactConstraintSIMD) -#include "x86/avx.h" +#include "box2d/math_types.h" -#include "box2d/math.h" +#include "x86/avx.h" typedef struct b2Contact b2Contact; diff --git a/src/core.h b/src/core.h index 5ba96fe2..589ff24b 100644 --- a/src/core.h +++ b/src/core.h @@ -15,6 +15,8 @@ #define B2_DEBUG 1 #endif +#define B2_VALIDATE B2_DEBUG + // Define platform #if defined(_WIN64) #define B2_PLATFORM_WINDOWS @@ -83,5 +85,3 @@ extern b2AssertFcn* b2AssertHandler; #else #define B2_ASSERT(...) ((void)0) #endif - -#define B2_VALIDATE B2_DEBUG diff --git a/src/distance.c b/src/distance.c index 9cf8e38b..5734cd30 100644 --- a/src/distance.c +++ b/src/distance.c @@ -502,7 +502,7 @@ int32_t b2_gjkIters; int32_t b2_gjkMaxIters; #endif -b2DistanceOutput b2ShapeDistance(b2DistanceCache* B2_RESTRICT cache, const b2DistanceInput* B2_RESTRICT input) +b2DistanceOutput b2ShapeDistance(b2DistanceCache* cache, const b2DistanceInput* input) { #if B2_GJK_DEBUG ++b2_gjkCalls; @@ -659,9 +659,9 @@ b2DistanceOutput b2ShapeDistance(b2DistanceCache* B2_RESTRICT cache, const b2Dis // Algorithm by Gino van den Bergen. // "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010 // TODO_ERIN this is failing when used to raycast a box -b2RayCastOutput b2ShapeCast(const b2ShapeCastPairInput* input) +b2CastOutput b2ShapeCast(const b2ShapeCastPairInput* input) { - b2RayCastOutput output = {0}; + b2CastOutput output = {0}; const b2DistanceProxy* proxyA = &input->proxyA; const b2DistanceProxy* proxyB = &input->proxyB; diff --git a/src/distance_joint.c b/src/distance_joint.c index c01bbcf7..bde253e1 100644 --- a/src/distance_joint.c +++ b/src/distance_joint.c @@ -6,12 +6,13 @@ #include "body.h" #include "core.h" #include "joint.h" -#include "solver_data.h" +#include "solver.h" #include "world.h" // needed for dll export #include "box2d/box2d.h" #include "box2d/debug_draw.h" +#include "box2d/joint_types.h" #include @@ -39,8 +40,8 @@ void b2PrepareDistanceJoint(b2Joint* base, b2StepContext* context) b2Body* bodyA = context->bodies + indexA; b2Body* bodyB = context->bodies + indexB; - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); float mA = bodyA->invMass; float iA = bodyA->invI; @@ -278,23 +279,52 @@ float b2DistanceJoint_GetConstraintForce(b2JointId jointId, float inverseTimeSte return (joint->impulse + joint->lowerImpulse - joint->upperImpulse) * inverseTimeStep; } -void b2DistanceJoint_SetLength(b2JointId jointId, float length, float minLength, float maxLength) +void b2DistanceJoint_SetLength(b2JointId jointId, float length) { b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); b2DistanceJoint* joint = &base->distanceJoint; joint->length = B2_CLAMP(length, b2_linearSlop, b2_huge); + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; +} + +float b2DistanceJoint_GetLength(b2JointId jointId) +{ + b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->length; +} + +void b2DistanceJoint_SetLengthRange(b2JointId jointId, float minLength, float maxLength) +{ + b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); + b2DistanceJoint* joint = &base->distanceJoint; minLength = B2_CLAMP(minLength, b2_linearSlop, b2_huge); maxLength = B2_CLAMP(maxLength, b2_linearSlop, b2_huge); joint->minLength = B2_MIN(minLength, maxLength); joint->maxLength = B2_MAX(minLength, maxLength); - joint->impulse = 0.0f; joint->lowerImpulse = 0.0f; joint->upperImpulse = 0.0f; } +float b2DistanceJoint_GetMinLength(b2JointId jointId) +{ + b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->minLength; +} + +float b2DistanceJoint_GetMaxLength(b2JointId jointId) +{ + b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->maxLength; +} + float b2DistanceJoint_GetCurrentLength(b2JointId jointId) { b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); @@ -311,8 +341,8 @@ float b2DistanceJoint_GetCurrentLength(b2JointId jointId) b2Body* bodyA = world->bodies + indexA; b2Body* bodyB = world->bodies + indexB; - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); b2Vec2 pA = b2TransformPoint(b2MakeTransform(bodyA), base->localOriginAnchorA); b2Vec2 pB = b2TransformPoint(b2MakeTransform(bodyB), base->localOriginAnchorB); @@ -329,6 +359,20 @@ void b2DistanceJoint_SetTuning(b2JointId jointId, float hertz, float dampingRati joint->dampingRatio = dampingRatio; } +float b2DistanceJoint_GetHertz(b2JointId jointId) +{ + b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->hertz; +} + +float b2DistanceJoint_GetDampingRatio(b2JointId jointId) +{ + b2Joint* base = b2GetJointCheckType(jointId, b2_distanceJoint); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->dampingRatio; +} + #if 0 void b2DistanceJoint::Dump() { diff --git a/src/geometry.c b/src/geometry.c index 5b07700d..d72d2d30 100644 --- a/src/geometry.c +++ b/src/geometry.c @@ -472,13 +472,13 @@ bool b2PointInPolygon(b2Vec2 point, const b2Polygon* shape) // Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019 // http://www.codercorner.com/blog/?p=321 -b2RayCastOutput b2RayCastCircle(const b2RayCastInput* input, const b2Circle* shape) +b2CastOutput b2RayCastCircle(const b2RayCastInput* input, const b2Circle* shape) { B2_ASSERT(b2IsValidRay(input)); b2Vec2 p = shape->point; - b2RayCastOutput output = {0}; + b2CastOutput output = {0}; // Shift ray so circle center is the origin b2Vec2 s = b2Sub(input->origin, p); @@ -529,11 +529,11 @@ b2RayCastOutput b2RayCastCircle(const b2RayCastInput* input, const b2Circle* sha return output; } -b2RayCastOutput b2RayCastCapsule(const b2RayCastInput* input, const b2Capsule* shape) +b2CastOutput b2RayCastCapsule(const b2RayCastInput* input, const b2Capsule* shape) { B2_ASSERT(b2IsValidRay(input)); - b2RayCastOutput output = {0}; + b2CastOutput output = {0}; b2Vec2 v1 = shape->point1; b2Vec2 v2 = shape->point2; @@ -663,7 +663,7 @@ b2RayCastOutput b2RayCastCapsule(const b2RayCastInput* input, const b2Capsule* s } // Ray vs line segment -b2RayCastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* shape, bool oneSided) +b2CastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* shape, bool oneSided) { if (oneSided) { @@ -671,7 +671,7 @@ b2RayCastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* s float offset = b2Cross(b2Sub(input->origin, shape->point1), b2Sub(shape->point2, shape->point1)); if (offset < 0.0f) { - b2RayCastOutput output = {0}; + b2CastOutput output = {0}; return output; } } @@ -684,7 +684,7 @@ b2RayCastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* s b2Vec2 v2 = shape->point2; b2Vec2 e = b2Sub(v2, v1); - b2RayCastOutput output = {0}; + b2CastOutput output = {0}; float length; b2Vec2 eUnit = b2GetLengthAndNormalize(&length, e); @@ -744,7 +744,7 @@ b2RayCastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* s return output; } -b2RayCastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* shape) +b2CastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* shape) { B2_ASSERT(b2IsValidRay(input)); @@ -758,7 +758,7 @@ b2RayCastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* s int32_t index = -1; - b2RayCastOutput output = {0}; + b2CastOutput output = {0}; for (int32_t i = 0; i < shape->count; ++i) { @@ -830,7 +830,7 @@ b2RayCastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* s return b2ShapeCast(&castInput); } -b2RayCastOutput b2ShapeCastCircle(const b2ShapeCastInput* input, const b2Circle* shape) +b2CastOutput b2ShapeCastCircle(const b2ShapeCastInput* input, const b2Circle* shape) { b2ShapeCastPairInput pairInput; pairInput.proxyA = b2MakeProxy(&shape->point, 1, shape->radius); @@ -840,11 +840,11 @@ b2RayCastOutput b2ShapeCastCircle(const b2ShapeCastInput* input, const b2Circle* pairInput.translationB = input->translation; pairInput.maxFraction = input->maxFraction; - b2RayCastOutput output = b2ShapeCast(&pairInput); + b2CastOutput output = b2ShapeCast(&pairInput); return output; } -b2RayCastOutput b2ShapeCastCapsule(const b2ShapeCastInput* input, const b2Capsule* shape) +b2CastOutput b2ShapeCastCapsule(const b2ShapeCastInput* input, const b2Capsule* shape) { b2ShapeCastPairInput pairInput; pairInput.proxyA = b2MakeProxy(&shape->point1, 2, shape->radius); @@ -854,11 +854,11 @@ b2RayCastOutput b2ShapeCastCapsule(const b2ShapeCastInput* input, const b2Capsul pairInput.translationB = input->translation; pairInput.maxFraction = input->maxFraction; - b2RayCastOutput output = b2ShapeCast(&pairInput); + b2CastOutput output = b2ShapeCast(&pairInput); return output; } -b2RayCastOutput b2ShapeCastSegment(const b2ShapeCastInput* input, const b2Segment* shape) +b2CastOutput b2ShapeCastSegment(const b2ShapeCastInput* input, const b2Segment* shape) { b2ShapeCastPairInput pairInput; pairInput.proxyA = b2MakeProxy(&shape->point1, 2, 0.0f); @@ -868,11 +868,11 @@ b2RayCastOutput b2ShapeCastSegment(const b2ShapeCastInput* input, const b2Segmen pairInput.translationB = input->translation; pairInput.maxFraction = input->maxFraction; - b2RayCastOutput output = b2ShapeCast(&pairInput); + b2CastOutput output = b2ShapeCast(&pairInput); return output; } -b2RayCastOutput b2ShapeCastPolygon(const b2ShapeCastInput* input, const b2Polygon* shape) +b2CastOutput b2ShapeCastPolygon(const b2ShapeCastInput* input, const b2Polygon* shape) { b2ShapeCastPairInput pairInput; pairInput.proxyA = b2MakeProxy(shape->vertices, shape->count, shape->radius); @@ -882,6 +882,6 @@ b2RayCastOutput b2ShapeCastPolygon(const b2ShapeCastInput* input, const b2Polygo pairInput.translationB = input->translation; pairInput.maxFraction = input->maxFraction; - b2RayCastOutput output = b2ShapeCast(&pairInput); + b2CastOutput output = b2ShapeCast(&pairInput); return output; } diff --git a/src/implementation.c b/src/implementation.c new file mode 100644 index 00000000..a7101341 --- /dev/null +++ b/src/implementation.c @@ -0,0 +1,8 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#define BOX2D_IMPLEMENTATION + +#include "box2d/color.h" +#include "box2d/dynamic_tree.h" +#include "box2d/math.h" diff --git a/src/island.c b/src/island.c index b796fd92..55e1ad57 100644 --- a/src/island.c +++ b/src/island.c @@ -4,14 +4,14 @@ #include "island.h" #include "aabb.h" -#include "arena_allocator.h" +#include "stack_allocator.h" #include "array.h" #include "body.h" #include "contact.h" #include "core.h" #include "joint.h" #include "shape.h" -#include "solver_data.h" +#include "solver.h" #include "world.h" #include "box2d/callbacks.h" diff --git a/src/joint.c b/src/joint.c index 620f7842..cb62d359 100644 --- a/src/joint.c +++ b/src/joint.c @@ -7,7 +7,7 @@ #include "contact.h" #include "core.h" #include "shape.h" -#include "solver_data.h" +#include "solver.h" #include "world.h" // needed for dll export @@ -15,7 +15,60 @@ #include "box2d/color.h" #include "box2d/debug_draw.h" #include "box2d/joint_types.h" -#include "box2d/joint_util.h" + +b2DistanceJointDef b2DefaultDistanceJointDef() +{ + b2DistanceJointDef def = {0}; + def.length = 1.0f; + def.maxLength = b2_huge; + return def; +} + +b2MotorJointDef b2DefaultMotorJointDef() +{ + b2MotorJointDef def = {0}; + def.maxForce = 1.0f; + def.maxTorque = 1.0f; + def.correctionFactor = 0.3f; + return def; +} + +b2MouseJointDef b2DefaultMouseJointDef() +{ + b2MouseJointDef def = {0}; + def.hertz = 4.0f; + def.dampingRatio = 1.0f; + return def; +} + +b2PrismaticJointDef b2DefaultPrismaticJointDef() +{ + b2PrismaticJointDef def = {0}; + def.localAxisA = (b2Vec2){1.0f, 0.0f}; + return def; +} + +b2RevoluteJointDef b2DefaultRevoluteJointDef() +{ + b2RevoluteJointDef def = {0}; + def.drawSize = 0.25f; + return def; +} + +b2WeldJointDef b2DefaultWeldJointDef() +{ + b2WeldJointDef def = {0}; + return def; +} + +b2WheelJointDef b2DefaultWheelJointDef() +{ + b2WheelJointDef def = {0}; + def.localAxisA.y = 1.0f; + def.hertz = 1.0f; + def.dampingRatio = 0.7f; + return def; +} // Get joint from id with validation b2Joint* b2GetJointCheckType(b2JointId id, b2JointType type) @@ -42,77 +95,11 @@ b2Joint* b2GetJoint(b2World* world, b2JointId jointId) { B2_ASSERT(1 <= jointId.index && jointId.index <= world->jointPool.capacity); b2Joint* joint = world->joints + (jointId.index - 1); - B2_ASSERT(b2ObjectValid(&joint->object)); + B2_ASSERT(b2IsValidObject(&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) -{ - B2_ASSERT(bodyIdA.world == bodyIdB.world); - - b2World* world = b2GetWorldFromIndex(bodyIdA.world); - B2_ASSERT(0 <= bodyIdA.index && bodyIdA.index < world->bodyPool.capacity); - B2_ASSERT(0 <= bodyIdB.index && bodyIdB.index < world->bodyPool.capacity); - - b2Body* bodyA = world->bodies + bodyIdA.index; - b2Body* bodyB = world->bodies + bodyIdB.index; - - float massA = bodyA->mass; - float massB = bodyB->mass; - float mass; - if (massA > 0.0f && massB > 0.0f) - { - mass = massA * massB / (massA + massB); - } - else if (massA > 0.0f) - { - mass = massA; - } - else - { - mass = massB; - } - - float omega = 2.0f * b2_pi * frequencyHertz; - *stiffness = mass * omega * omega; - *damping = 2.0f * mass * dampingRatio * omega; -} - -void b2AngularStiffness(float* stiffness, float* damping, float frequencyHertz, float dampingRatio, b2BodyId bodyIdA, - b2BodyId bodyIdB) -{ - B2_ASSERT(bodyIdA.world == bodyIdB.world); - - b2World* world = b2GetWorldFromIndex(bodyIdA.world); - B2_ASSERT(0 <= bodyIdA.index && bodyIdA.index < world->bodyPool.capacity); - B2_ASSERT(0 <= bodyIdB.index && bodyIdB.index < world->bodyPool.capacity); - - b2Body* bodyA = world->bodies + bodyIdA.index; - b2Body* bodyB = world->bodies + bodyIdB.index; - - float IA = bodyA->I; - float IB = bodyB->I; - float I; - if (IA > 0.0f && IB > 0.0f) - { - I = IA * IB / (IA + IB); - } - else if (IA > 0.0f) - { - I = IA; - } - else - { - I = IB; - } - - float omega = 2.0f * b2_pi * frequencyHertz; - *stiffness = I * omega * omega; - *damping = 2.0f * I * dampingRatio * omega; -} - static b2Joint* b2CreateJoint(b2World* world, b2Body* bodyA, b2Body* bodyB) { b2Joint* joint = (b2Joint*)b2AllocObject(&world->jointPool); @@ -222,8 +209,8 @@ b2JointId b2CreateDistanceJoint(b2WorldId worldId, const b2DistanceJointDef* def b2Body* bodyA = world->bodies + (def->bodyIdA.index - 1); b2Body* bodyB = world->bodies + (def->bodyIdB.index - 1); - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); b2Joint* joint = b2CreateJoint(world, bodyA, bodyB); @@ -231,6 +218,7 @@ b2JointId b2CreateDistanceJoint(b2WorldId worldId, const b2DistanceJointDef* def joint->localOriginAnchorA = def->localAnchorA; joint->localOriginAnchorB = def->localAnchorB; joint->collideConnected = def->collideConnected; + joint->userData = def->userData; b2DistanceJoint empty = {0}; joint->distanceJoint = empty; @@ -269,15 +257,16 @@ b2JointId b2CreateMotorJoint(b2WorldId worldId, const b2MotorJointDef* def) b2Body* bodyA = world->bodies + (def->bodyIdA.index - 1); b2Body* bodyB = world->bodies + (def->bodyIdB.index - 1); - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); b2Joint* joint = b2CreateJoint(world, bodyA, bodyB); joint->type = b2_motorJoint; joint->localOriginAnchorA = (b2Vec2){0.0f, 0.0f}; joint->localOriginAnchorB = (b2Vec2){0.0f, 0.0f}; - joint->collideConnected = true; + joint->collideConnected = def->collideConnected; + joint->userData = def->userData; joint->motorJoint = (b2MotorJoint){0}; joint->motorJoint.linearOffset = def->linearOffset; @@ -286,6 +275,12 @@ b2JointId b2CreateMotorJoint(b2WorldId worldId, const b2MotorJointDef* def) joint->motorJoint.maxTorque = def->maxTorque; joint->motorJoint.correctionFactor = B2_CLAMP(def->correctionFactor, 0.0f, 1.0f); + // If the joint prevents collisions, then destroy all contacts between attached bodies + if (def->collideConnected == false) + { + b2DestroyContactsBetweenBodies(world, bodyA, bodyB); + } + b2JointId jointId = {joint->object.index + 1, world->poolIndex, joint->object.revision}; return jointId; } @@ -306,8 +301,8 @@ b2JointId b2CreateMouseJoint(b2WorldId worldId, const b2MouseJointDef* def) b2Body* bodyA = world->bodies + (def->bodyIdA.index - 1); b2Body* bodyB = world->bodies + (def->bodyIdB.index - 1); - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); b2Joint* joint = b2CreateJoint(world, bodyA, bodyB); @@ -315,6 +310,7 @@ b2JointId b2CreateMouseJoint(b2WorldId worldId, const b2MouseJointDef* def) joint->localOriginAnchorA = b2InvTransformPoint(b2MakeTransform(bodyA), def->target); joint->localOriginAnchorB = b2InvTransformPoint(b2MakeTransform(bodyB), def->target); joint->collideConnected = true; + joint->userData = def->userData; b2MouseJoint empty = {0}; joint->mouseJoint = empty; @@ -342,8 +338,8 @@ b2JointId b2CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDef* def b2Body* bodyA = world->bodies + (def->bodyIdA.index - 1); b2Body* bodyB = world->bodies + (def->bodyIdB.index - 1); - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); b2Joint* joint = b2CreateJoint(world, bodyA, bodyB); @@ -351,6 +347,7 @@ b2JointId b2CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDef* def joint->localOriginAnchorA = def->localAnchorA; joint->localOriginAnchorB = def->localAnchorB; joint->collideConnected = def->collideConnected; + joint->userData = def->userData; joint->drawSize = def->drawSize; b2RevoluteJoint empty = {0}; @@ -395,8 +392,8 @@ b2JointId b2CreatePrismaticJoint(b2WorldId worldId, const b2PrismaticJointDef* d b2Body* bodyA = world->bodies + (def->bodyIdA.index - 1); b2Body* bodyB = world->bodies + (def->bodyIdB.index - 1); - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); b2Joint* joint = b2CreateJoint(world, bodyA, bodyB); @@ -404,6 +401,7 @@ b2JointId b2CreatePrismaticJoint(b2WorldId worldId, const b2PrismaticJointDef* d joint->localOriginAnchorA = def->localAnchorA; joint->localOriginAnchorB = def->localAnchorB; joint->collideConnected = def->collideConnected; + joint->userData = def->userData; b2PrismaticJoint empty = {0}; joint->prismaticJoint = empty; @@ -448,8 +446,8 @@ b2JointId b2CreateWeldJoint(b2WorldId worldId, const b2WeldJointDef* def) b2Body* bodyA = world->bodies + (def->bodyIdA.index - 1); b2Body* bodyB = world->bodies + (def->bodyIdB.index - 1); - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); b2Joint* joint = b2CreateJoint(world, bodyA, bodyB); @@ -457,6 +455,7 @@ b2JointId b2CreateWeldJoint(b2WorldId worldId, const b2WeldJointDef* def) joint->localOriginAnchorA = def->localAnchorA; joint->localOriginAnchorB = def->localAnchorB; joint->collideConnected = def->collideConnected; + joint->userData = def->userData; b2WeldJoint empty = {0}; joint->weldJoint = empty; @@ -494,8 +493,8 @@ b2JointId b2CreateWheelJoint(b2WorldId worldId, const b2WheelJointDef* def) b2Body* bodyA = world->bodies + (def->bodyIdA.index - 1); b2Body* bodyB = world->bodies + (def->bodyIdB.index - 1); - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); b2Joint* joint = b2CreateJoint(world, bodyA, bodyB); @@ -503,6 +502,7 @@ b2JointId b2CreateWheelJoint(b2WorldId worldId, const b2WheelJointDef* def) joint->localOriginAnchorA = def->localAnchorA; joint->localOriginAnchorB = def->localAnchorB; joint->collideConnected = def->collideConnected; + joint->userData = def->userData; // todo test this joint->wheelJoint = (b2WheelJoint){0}; @@ -532,6 +532,69 @@ b2JointId b2CreateWheelJoint(b2WorldId worldId, const b2WheelJointDef* def) return jointId; } +void b2DestroyJointInternal(b2World* world, b2Joint* joint) +{ + b2JointEdge* edgeA = joint->edges + 0; + b2JointEdge* edgeB = joint->edges + 1; + + b2Body* bodyA = world->bodies + edgeA->bodyIndex; + b2Body* bodyB = world->bodies + edgeB->bodyIndex; + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); + + // Remove from body A + if (edgeA->prevKey != B2_NULL_INDEX) + { + b2Joint* prevJoint = world->joints + (edgeA->prevKey >> 1); + b2JointEdge* prevEdge = prevJoint->edges + (edgeA->prevKey & 1); + prevEdge->nextKey = edgeA->nextKey; + } + + if (edgeA->nextKey != B2_NULL_INDEX) + { + b2Joint* nextJoint = world->joints + (edgeA->nextKey >> 1); + b2JointEdge* nextEdge = nextJoint->edges + (edgeA->nextKey & 1); + nextEdge->prevKey = edgeA->prevKey; + } + + int32_t edgeKeyA = (joint->object.index << 1) | 0; + if (bodyA->jointList == edgeKeyA) + { + bodyA->jointList = edgeA->nextKey; + } + + bodyA->jointCount -= 1; + + // Remove from body B + if (edgeB->prevKey != B2_NULL_INDEX) + { + b2Joint* prevJoint = world->joints + (edgeB->prevKey >> 1); + b2JointEdge* prevEdge = prevJoint->edges + (edgeB->prevKey & 1); + prevEdge->nextKey = edgeB->nextKey; + } + + if (edgeB->nextKey != B2_NULL_INDEX) + { + b2Joint* nextJoint = world->joints + (edgeB->nextKey >> 1); + b2JointEdge* nextEdge = nextJoint->edges + (edgeB->nextKey & 1); + nextEdge->prevKey = edgeB->prevKey; + } + + int32_t edgeKeyB = (joint->object.index << 1) | 1; + if (bodyB->jointList == edgeKeyB) + { + bodyB->jointList = edgeB->nextKey; + } + + bodyB->jointCount -= 1; + + b2UnlinkJoint(world, joint); + + b2RemoveJointFromGraph(world, joint); + + b2FreeObject(&world->jointPool, &joint->object); +} + void b2DestroyJoint(b2JointId jointId) { b2World* world = b2GetWorldFromIndex(jointId.world); @@ -549,8 +612,8 @@ void b2DestroyJoint(b2JointId jointId) b2Body* bodyA = world->bodies + edgeA->bodyIndex; b2Body* bodyB = world->bodies + edgeB->bodyIndex; - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); // Remove from body A if (edgeA->prevKey != B2_NULL_INDEX) @@ -619,7 +682,7 @@ b2BodyId b2Joint_GetBodyA(b2JointId jointId) int32_t bodyIndex = joint->edges[0].bodyIndex; b2Body* body = world->bodies + bodyIndex; - B2_ASSERT(b2ObjectValid(&body->object)); + B2_ASSERT(b2IsValidObject(&body->object)); b2BodyId bodyId = {bodyIndex + 1, jointId.world, body->object.revision}; return bodyId; } @@ -631,11 +694,115 @@ b2BodyId b2Joint_GetBodyB(b2JointId jointId) int32_t bodyIndex = joint->edges[1].bodyIndex; b2Body* body = world->bodies + bodyIndex; - B2_ASSERT(b2ObjectValid(&body->object)); + B2_ASSERT(b2IsValidObject(&body->object)); b2BodyId bodyId = {bodyIndex + 1, jointId.world, body->object.revision}; return bodyId; } +b2Vec2 b2Joint_GetLocalAnchorA(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + return joint->localOriginAnchorA; +} + +b2Vec2 b2Joint_GetLocalAnchorB(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + return joint->localOriginAnchorB; +} + +void b2Joint_SetCollideConnected(b2JointId jointId, bool shouldCollide) +{ + b2World* world = b2GetWorldFromIndexLocked(jointId.world); + if (world == NULL) + { + return; + } + + b2Joint* joint = b2GetJoint(world, jointId); + if (joint->collideConnected == shouldCollide) + { + return; + } + + joint->collideConnected = shouldCollide; + + int32_t bodyIndexA = joint->edges[0].bodyIndex; + int32_t bodyIndexB = joint->edges[1].bodyIndex; + + b2Body* bodyA = world->bodies + bodyIndexA; + b2Body* bodyB = world->bodies + bodyIndexA; + + if (shouldCollide) + { + // need to tell the broadphase to look for new pairs for one of the + // two bodies. Pick the one with the fewest shapes. + int shapeCountA = bodyA->shapeCount; + int shapeCountB = bodyB->shapeCount; + + int shapeIndex = bodyA->shapeCount < bodyB->shapeCount ? bodyA->shapeList : bodyB->shapeList; + while (shapeIndex != B2_NULL_INDEX) + { + b2Shape* shape = world->shapes + shapeIndex; + + if (shape->proxyKey != B2_NULL_INDEX) + { + b2BufferMove(&world->broadPhase, shape->proxyKey); + } + + shapeIndex = shape->nextShapeIndex; + } + } + else + { + // no collision between bodies, destroy any contact between them + int contactCountA = bodyA->contactCount; + int contactCountB = bodyB->contactCount; + + int32_t contactKey = contactCountA < contactCountB ? bodyA->contactList : bodyB->contactList; + while (contactKey != B2_NULL_INDEX) + { + int32_t contactIndex = contactKey >> 1; + int32_t edgeIndex = contactKey & 1; + + b2Contact* contact = world->contacts + contactIndex; + contactKey = contact->edges[edgeIndex].nextKey; + + b2Shape* shapeA = world->shapes + contact->shapeIndexA; + b2Shape* shapeB = world->shapes + contact->shapeIndexB; + + if ((bodyIndexA == shapeA->bodyIndex && bodyIndexB == shapeB->bodyIndex) || + (bodyIndexA == shapeB->bodyIndex && bodyIndexB == shapeA->bodyIndex)) + { + b2DestroyContact(world, contact); + } + } + } +} + +bool b2Joint_GetCollideConnected(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + return joint->collideConnected; +} + +void b2Joint_SetUserData(b2JointId jointId, void* userData) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + joint->userData = userData; +} + +void* b2Joint_GetUserData(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + return joint->userData; +} + extern void b2PrepareDistanceJoint(b2Joint* base, b2StepContext* context); extern void b2PrepareMotorJoint(b2Joint* base, b2StepContext* context); extern void b2PrepareMouseJoint(b2Joint* base, b2StepContext* context); @@ -788,7 +955,7 @@ void b2PrepareOverflowJoints(b2StepContext* context) B2_ASSERT(0 <= index && index < world->jointPool.capacity); b2Joint* joint = joints + index; - B2_ASSERT(b2ObjectValid(&joint->object) == true); + B2_ASSERT(b2IsValidObject(&joint->object) == true); b2PrepareJoint(joint, context); } @@ -812,7 +979,7 @@ void b2WarmStartOverflowJoints(b2StepContext* context) B2_ASSERT(0 <= index && index < world->jointPool.capacity); b2Joint* joint = joints + index; - B2_ASSERT(b2ObjectValid(&joint->object) == true); + B2_ASSERT(b2IsValidObject(&joint->object) == true); b2WarmStartJoint(joint, context); } @@ -836,7 +1003,7 @@ void b2SolveOverflowJoints(b2StepContext* context, bool useBias) B2_ASSERT(0 <= index && index < world->jointPool.capacity); b2Joint* joint = joints + index; - B2_ASSERT(b2ObjectValid(&joint->object) == true); + B2_ASSERT(b2IsValidObject(&joint->object) == true); b2SolveJoint(joint, context, useBias); } @@ -957,7 +1124,7 @@ void b2DrawJoint(b2DebugDraw* draw, b2World* world, b2Joint* joint) if (joint->colorIndex != B2_NULL_INDEX) { b2Vec2 p = b2Lerp(pA, pB, 0.5f); - draw->DrawPoint(p, 5.0f, b2MakeColor(colors[joint->colorIndex], 1.0f), draw->context); + draw->DrawPoint(p, 5.0f, b2MakeColor(colors[joint->colorIndex]), draw->context); } } } diff --git a/src/joint.h b/src/joint.h index fda2ffbd..d3a0c067 100644 --- a/src/joint.h +++ b/src/joint.h @@ -3,7 +3,7 @@ #pragma once #include "pool.h" -#include "solver_data.h" +#include "solver.h" #include "box2d/joint_types.h" @@ -220,15 +220,16 @@ typedef struct b2Joint b2WheelJoint wheelJoint; }; + void* userData; + float drawSize; bool isMarked; bool collideConnected; } b2Joint; b2Joint* b2GetJoint(b2World* world, b2JointId jointId); - -// todo remove this b2Joint* b2GetJointCheckType(b2JointId id, b2JointType type); +void b2DestroyJointInternal(b2World* world, b2Joint* joint); void b2PrepareJoint(b2Joint* joint, b2StepContext* context); void b2WarmStartJoint(b2Joint* joint, b2StepContext* context); diff --git a/src/math.c b/src/math.c index 3e7c14db..791777b9 100644 --- a/src/math.c +++ b/src/math.c @@ -38,6 +38,21 @@ bool b2Vec2_IsValid(b2Vec2 v) return true; } +bool b2Rot_IsValid(b2Rot q) +{ + if (isnan(q.s) || isnan(q.c)) + { + return false; + } + + if (isinf(q.s) || isinf(q.c)) + { + return false; + } + + return b2IsNormalized(q); +} + b2Vec2 b2Normalize(b2Vec2 v) { float length = b2Length(v); diff --git a/src/motor_joint.c b/src/motor_joint.c index e668acec..76a2bc09 100644 --- a/src/motor_joint.c +++ b/src/motor_joint.c @@ -4,7 +4,7 @@ #include "body.h" #include "core.h" #include "joint.h" -#include "solver_data.h" +#include "solver.h" #include "world.h" // needed for dll export @@ -180,79 +180,64 @@ void b2SolveMotorJoint(b2Joint* base, const b2StepContext* context, bool useBias void b2MotorJoint_SetLinearOffset(b2JointId jointId, b2Vec2 linearOffset) { - b2World* world = b2GetWorldFromIndex(jointId.world); - B2_ASSERT(world->locked == false); - if (world->locked) - { - return; - } - - b2Joint* joint = b2GetJoint(world, jointId); - B2_ASSERT(joint->type == b2_motorJoint); - + b2Joint* joint = b2GetJointCheckType(jointId, b2_motorJoint); joint->motorJoint.linearOffset = linearOffset; } -void b2MotorJoint_SetAngularOffset(b2JointId jointId, float angularOffset) +b2Vec2 b2MotorJoint_GetLinearOffset(b2JointId jointId) { - b2World* world = b2GetWorldFromIndex(jointId.world); - B2_ASSERT(world->locked == false); - if (world->locked) - { - return; - } - - b2Joint* joint = b2GetJoint(world, jointId); - B2_ASSERT(joint->type == b2_motorJoint); + b2Joint* joint = b2GetJointCheckType(jointId, b2_motorJoint); + return joint->motorJoint.linearOffset; +} +void b2MotorJoint_SetAngularOffset(b2JointId jointId, float angularOffset) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_motorJoint); joint->motorJoint.angularOffset = angularOffset; } -void b2MotorJoint_SetMaxForce(b2JointId jointId, float maxForce) +float b2MotorJoint_GetAngularOffset(b2JointId jointId) { - b2World* world = b2GetWorldFromIndex(jointId.world); - B2_ASSERT(world->locked == false); - if (world->locked) - { - return; - } - - b2Joint* joint = b2GetJoint(world, jointId); - B2_ASSERT(joint->type == b2_motorJoint); + b2Joint* joint = b2GetJointCheckType(jointId, b2_motorJoint); + return joint->motorJoint.angularOffset; +} +void b2MotorJoint_SetMaxForce(b2JointId jointId, float maxForce) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_motorJoint); joint->motorJoint.maxForce = B2_MAX(0.0f, maxForce); } -void b2MotorJoint_SetMaxTorque(b2JointId jointId, float maxTorque) +float b2MotorJoint_GetMaxForce(b2JointId jointId) { - b2World* world = b2GetWorldFromIndex(jointId.world); - B2_ASSERT(world->locked == false); - if (world->locked) - { - return; - } - - b2Joint* joint = b2GetJoint(world, jointId); - B2_ASSERT(joint->type == b2_motorJoint); + b2Joint* joint = b2GetJointCheckType(jointId, b2_motorJoint); + return joint->motorJoint.maxForce; +} +void b2MotorJoint_SetMaxTorque(b2JointId jointId, float maxTorque) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_motorJoint); joint->motorJoint.maxTorque = B2_MAX(0.0f, maxTorque); } -void b2MotorJoint_SetCorrectionFactor(b2JointId jointId, float correctionFactor) +float b2MotorJoint_GetMaxTorque(b2JointId jointId) { - b2World* world = b2GetWorldFromIndex(jointId.world); - B2_ASSERT(world->locked == false); - if (world->locked) - { - return; - } - - b2Joint* joint = b2GetJoint(world, jointId); - B2_ASSERT(joint->type == b2_motorJoint); + b2Joint* joint = b2GetJointCheckType(jointId, b2_motorJoint); + return joint->motorJoint.maxTorque; +} +void b2MotorJoint_SetCorrectionFactor(b2JointId jointId, float correctionFactor) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_motorJoint); joint->motorJoint.correctionFactor = B2_CLAMP(correctionFactor, 0.0f, 1.0f); } +float b2MotorJoint_GetCorrectionFactor(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_motorJoint); + return joint->motorJoint.correctionFactor; +} + b2Vec2 b2MotorJoint_GetConstraintForce(b2JointId jointId) { b2World* world = b2GetWorldFromIndex(jointId.world); diff --git a/src/mouse_joint.c b/src/mouse_joint.c index dfe742f1..4d701054 100644 --- a/src/mouse_joint.c +++ b/src/mouse_joint.c @@ -4,7 +4,7 @@ #include "body.h" #include "core.h" #include "joint.h" -#include "solver_data.h" +#include "solver.h" #include "world.h" // needed for dll export @@ -16,6 +16,31 @@ void b2MouseJoint_SetTarget(b2JointId jointId, b2Vec2 target) base->mouseJoint.targetA = target; } +b2Vec2 b2MouseJoint_GetTarget(b2JointId jointId) +{ + b2Joint* base = b2GetJointCheckType(jointId, b2_mouseJoint); + return base->mouseJoint.targetA; +} + +void b2MouseJoint_SetTuning(b2JointId jointId, float hertz, float dampingRatio) +{ + b2Joint* base = b2GetJointCheckType(jointId, b2_mouseJoint); + base->mouseJoint.hertz = hertz; + base->mouseJoint.dampingRatio = dampingRatio; +} + +float b2MouseJoint_GetHertz(b2JointId jointId) +{ + b2Joint* base = b2GetJointCheckType(jointId, b2_mouseJoint); + return base->mouseJoint.hertz; +} + +float b2MouseJoint_GetDampingRatio(b2JointId jointId) +{ + b2Joint* base = b2GetJointCheckType(jointId, b2_mouseJoint); + return base->mouseJoint.dampingRatio; +} + void b2PrepareMouseJoint(b2Joint* base, b2StepContext* context) { B2_ASSERT(base->type == b2_mouseJoint); diff --git a/src/pool.h b/src/pool.h index fe7cbdae..720fd4de 100644 --- a/src/pool.h +++ b/src/pool.h @@ -35,7 +35,7 @@ void b2FreeObject(b2Pool* pool, b2Object* object); void b2GrowPool(b2Pool* pool, int32_t capacity); -static inline bool b2ObjectValid(const b2Object* object) +static inline bool b2IsValidObject(const b2Object* object) { // this means the object is not on the free list return object->index == object->next; diff --git a/src/prismatic_joint.c b/src/prismatic_joint.c index 1df6e722..461bb4aa 100644 --- a/src/prismatic_joint.c +++ b/src/prismatic_joint.c @@ -4,7 +4,7 @@ #include "body.h" #include "core.h" #include "joint.h" -#include "solver_data.h" +#include "solver.h" #include "world.h" // needed for dll export @@ -13,6 +13,94 @@ #include +void b2PrismaticJoint_EnableLimit(b2JointId jointId, bool enableLimit) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); + joint->prismaticJoint.enableLimit = enableLimit; +} + +bool b2PrismaticJoint_IsLimitEnabled(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); + return joint->prismaticJoint.enableLimit; +} + +void b2PrismaticJoint_EnableMotor(b2JointId jointId, bool enableMotor) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); + joint->prismaticJoint.enableMotor = enableMotor; +} + +bool b2PrismaticJoint_IsMotorEnabled(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); + return joint->prismaticJoint.enableMotor; +} + +void b2PrismaticJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); + joint->prismaticJoint.motorSpeed = motorSpeed; +} + +float b2PrismaticJoint_GetMotorSpeed(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); + return joint->prismaticJoint.motorSpeed; +} + +float b2PrismaticJoint_GetMotorForce(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* base = b2GetJoint(world, jointId); + B2_ASSERT(base->type == b2_prismaticJoint); + return world->inv_h * base->prismaticJoint.motorImpulse; +} + +void b2PrismaticJoint_SetMaxMotorForce(b2JointId jointId, float force) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); + joint->prismaticJoint.maxMotorForce = force; +} + +float b2PrismaticJoint_GetMaxMotorForce(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); + return joint->prismaticJoint.maxMotorForce; +} + +b2Vec2 b2PrismaticJoint_GetConstraintForce(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* base = b2GetJoint(world, jointId); + B2_ASSERT(base->type == b2_prismaticJoint); + + int32_t indexA = base->edges[0].bodyIndex; + b2Body* bodyA = world->bodies + indexA; + B2_ASSERT(b2IsValidObject(&bodyA->object)); + + b2PrismaticJoint* joint = &base->prismaticJoint; + + b2Vec2 axisA = b2RotateVector(bodyA->rotation, joint->localAxisA); + b2Vec2 perpA = b2LeftPerp(axisA); + + float inv_h = world->inv_h; + float perpForce = inv_h * joint->impulse.x; + float axialForce = inv_h * (joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse); + + b2Vec2 force = b2Add(b2MulSV(perpForce, perpA), b2MulSV(axialForce, axisA)); + return force; +} + +float b2PrismaticJoint_GetConstraintTorque(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_prismaticJoint); + + return world->inv_h * joint->prismaticJoint.impulse.y; +} + // Linear constraint (point-to-line) // d = p2 - p1 = x2 + r2 - x1 - r1 // C = dot(perp, d) @@ -66,8 +154,8 @@ void b2PreparePrismaticJoint(b2Joint* base, b2StepContext* context) int32_t indexB = base->edges[1].bodyIndex; b2Body* bodyA = context->bodies + indexA; b2Body* bodyB = context->bodies + indexB; - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); float mA = bodyA->invMass; float iA = bodyA->invI; @@ -194,7 +282,7 @@ void b2SolvePrismaticJoint(b2Joint* base, b2StepContext* context, bool useBias) float Cdot = b2Dot(axisA, b2Sub(vB, vA)) + a2 * wB - a1 * wA; float impulse = joint->axialMass * (joint->motorSpeed - Cdot); float oldImpulse = joint->motorImpulse; - float maxImpulse = context->dt * joint->maxMotorForce; + float maxImpulse = context->h * joint->maxMotorForce; joint->motorImpulse = B2_CLAMP(joint->motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = joint->motorImpulse - oldImpulse; @@ -349,70 +437,6 @@ void b2SolvePrismaticJoint(b2Joint* base, b2StepContext* context, bool useBias) stateB->angularVelocity = wB; } -void b2PrismaticJoint_EnableLimit(b2JointId jointId, bool enableLimit) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); - joint->prismaticJoint.enableLimit = enableLimit; -} - -void b2PrismaticJoint_EnableMotor(b2JointId jointId, bool enableMotor) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); - joint->prismaticJoint.enableMotor = enableMotor; -} - -void b2PrismaticJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); - joint->prismaticJoint.motorSpeed = motorSpeed; -} - -float b2PrismaticJoint_GetMotorForce(b2JointId jointId) -{ - b2World* world = b2GetWorldFromIndex(jointId.world); - b2Joint* base = b2GetJoint(world, jointId); - B2_ASSERT(base->type == b2_prismaticJoint); - return world->inv_h * base->prismaticJoint.motorImpulse; -} - -void b2PrismaticJoint_SetMaxMotorForce(b2JointId jointId, float force) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_prismaticJoint); - joint->prismaticJoint.maxMotorForce = force; -} - -b2Vec2 b2PrismaticJoint_GetConstraintForce(b2JointId jointId) -{ - b2World* world = b2GetWorldFromIndex(jointId.world); - b2Joint* base = b2GetJoint(world, jointId); - B2_ASSERT(base->type == b2_prismaticJoint); - - int32_t indexA = base->edges[0].bodyIndex; - b2Body* bodyA = world->bodies + indexA; - B2_ASSERT(b2ObjectValid(&bodyA->object)); - - b2PrismaticJoint* joint = &base->prismaticJoint; - - b2Vec2 axisA = b2RotateVector(bodyA->rotation, joint->localAxisA); - b2Vec2 perpA = b2LeftPerp(axisA); - - float inv_h = world->inv_h; - float perpForce = inv_h * joint->impulse.x; - float axialForce = inv_h * (joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse); - - b2Vec2 force = b2Add(b2MulSV(perpForce, perpA), b2MulSV(axialForce, axisA)); - return force; -} - -float b2PrismaticJoint_GetConstraintTorque(b2JointId jointId) -{ - b2World* world = b2GetWorldFromIndex(jointId.world); - b2Joint* joint = b2GetJoint(world, jointId); - B2_ASSERT(joint->type == b2_prismaticJoint); - - return world->inv_h * joint->prismaticJoint.impulse.y; -} - #if 0 void b2PrismaticJoint::Dump() { diff --git a/src/revolute_joint.c b/src/revolute_joint.c index 5ce61ebf..75b02d00 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -6,7 +6,7 @@ #include "body.h" #include "core.h" #include "joint.h" -#include "solver_data.h" +#include "solver.h" #include "world.h" // needed for dll export @@ -15,6 +15,84 @@ #include +void b2RevoluteJoint_EnableLimit(b2JointId jointId, bool enableLimit) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); + joint->revoluteJoint.enableLimit = enableLimit; +} + +bool b2RevoluteJoint_IsLimitEnabled(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); + return joint->revoluteJoint.enableLimit; +} + +void b2RevoluteJoint_EnableMotor(b2JointId jointId, bool enableMotor) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); + joint->revoluteJoint.enableMotor = enableMotor; +} + +bool b2RevoluteJoint_IsMotorEnabled(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); + return joint->revoluteJoint.enableMotor; +} + +void b2RevoluteJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); + joint->revoluteJoint.motorSpeed = motorSpeed; +} + +float b2RevoluteJoint_GetMotorSpeed(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); + return joint->revoluteJoint.motorSpeed; +} + +float b2RevoluteJoint_GetMotorTorque(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_revoluteJoint); + + return world->inv_h * joint->revoluteJoint.motorImpulse; +} + +void b2RevoluteJoint_SetMaxMotorTorque(b2JointId jointId, float torque) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); + joint->revoluteJoint.maxMotorTorque = torque; +} + +float b2RevoluteJoint_GetMaxMotorTorque(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); + return joint->revoluteJoint.maxMotorTorque; +} + +b2Vec2 b2RevoluteJoint_GetConstraintForce(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_revoluteJoint); + + b2Vec2 force = b2MulSV(world->inv_h, joint->revoluteJoint.linearImpulse); + return force; +} + +float b2RevoluteJoint_GetConstraintTorque(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_revoluteJoint); + + const b2RevoluteJoint* revolute = &joint->revoluteJoint; + float torque = world->inv_h * (revolute->motorImpulse + revolute->lowerImpulse - revolute->upperImpulse); + return torque; +} + // Point-to-point constraint // C = p2 - p1 // Cdot = v2 - v1 @@ -52,8 +130,8 @@ void b2PrepareRevoluteJoint(b2Joint* base, b2StepContext* context) int32_t indexB = base->edges[1].bodyIndex; b2Body* bodyA = context->bodies + indexA; b2Body* bodyB = context->bodies + indexB; - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); float mA = bodyA->invMass; float iA = bodyA->invI; @@ -272,60 +350,6 @@ void b2SolveRevoluteJoint(b2Joint* base, b2StepContext* context, bool useBias) stateB->angularVelocity = wB; } -void b2RevoluteJoint_EnableLimit(b2JointId jointId, bool enableLimit) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); - joint->revoluteJoint.enableLimit = enableLimit; -} - -void b2RevoluteJoint_EnableMotor(b2JointId jointId, bool enableMotor) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); - joint->revoluteJoint.enableMotor = enableMotor; -} - -void b2RevoluteJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); - joint->revoluteJoint.motorSpeed = motorSpeed; -} - -float b2RevoluteJoint_GetMotorTorque(b2JointId jointId) -{ - b2World* world = b2GetWorldFromIndex(jointId.world); - b2Joint* joint = b2GetJoint(world, jointId); - B2_ASSERT(joint->type == b2_revoluteJoint); - - return world->inv_h * joint->revoluteJoint.motorImpulse; -} - -void b2RevoluteJoint_SetMaxMotorTorque(b2JointId jointId, float torque) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); - joint->revoluteJoint.maxMotorTorque = torque; -} - -b2Vec2 b2RevoluteJoint_GetConstraintForce(b2JointId jointId) -{ - b2World* world = b2GetWorldFromIndex(jointId.world); - b2Joint* joint = b2GetJoint(world, jointId); - B2_ASSERT(joint->type == b2_revoluteJoint); - - b2Vec2 force = b2MulSV(world->inv_h, joint->revoluteJoint.linearImpulse); - return force; -} - -float b2RevoluteJoint_GetConstraintTorque(b2JointId jointId) -{ - b2World* world = b2GetWorldFromIndex(jointId.world); - b2Joint* joint = b2GetJoint(world, jointId); - B2_ASSERT(joint->type == b2_revoluteJoint); - - const b2RevoluteJoint* revolute = &joint->revoluteJoint; - float torque = world->inv_h * (revolute->motorImpulse + revolute->lowerImpulse - revolute->upperImpulse); - return torque; -} - #if 0 void b2RevoluteJoint::Dump() { diff --git a/src/shape.c b/src/shape.c index f991f987..23d22b3c 100644 --- a/src/shape.c +++ b/src/shape.c @@ -16,7 +16,7 @@ b2Shape* b2GetShape(b2World* world, b2ShapeId shapeId) { B2_ASSERT(1 <= shapeId.index && shapeId.index <= world->shapePool.capacity); b2Shape* shape = world->shapes + (shapeId.index - 1); - B2_ASSERT(b2ObjectValid(&shape->object)); + B2_ASSERT(b2IsValidObject(&shape->object)); B2_ASSERT(shape->object.revision == shapeId.revision); return shape; } @@ -25,7 +25,7 @@ static b2ChainShape* b2GetChainShape(b2World* world, b2ChainId chainId) { B2_ASSERT(1 <= chainId.index && chainId.index <= world->chainPool.capacity); b2ChainShape* chain = world->chains + (chainId.index - 1); - B2_ASSERT(b2ObjectValid(&chain->object)); + B2_ASSERT(b2IsValidObject(&chain->object)); B2_ASSERT(chain->object.revision == chainId.revision); return chain; } @@ -138,13 +138,13 @@ b2ShapeExtent b2ComputeShapeExtent(const b2Shape* shape) return extent; } -b2RayCastOutput b2RayCastShape(const b2RayCastInput* input, const b2Shape* shape, b2Transform xf) +b2CastOutput b2RayCastShape(const b2RayCastInput* input, const b2Shape* shape, b2Transform xf) { b2RayCastInput localInput = *input; localInput.origin = b2InvTransformPoint(xf, input->origin); localInput.translation = b2InvRotateVector(xf.q, input->translation); - b2RayCastOutput output = {0}; + b2CastOutput output = {0}; switch (shape->type) { case b2_capsuleShape: @@ -171,7 +171,7 @@ b2RayCastOutput b2RayCastShape(const b2RayCastInput* input, const b2Shape* shape return output; } -b2RayCastOutput b2ShapeCastShape(const b2ShapeCastInput* input, const b2Shape* shape, b2Transform xf) +b2CastOutput b2ShapeCastShape(const b2ShapeCastInput* input, const b2Shape* shape, b2Transform xf) { b2ShapeCastInput localInput = *input; @@ -182,7 +182,7 @@ b2RayCastOutput b2ShapeCastShape(const b2ShapeCastInput* input, const b2Shape* s localInput.translation = b2InvRotateVector(xf.q, input->translation); - b2RayCastOutput output = {0}; + b2CastOutput output = {0}; switch (shape->type) { case b2_capsuleShape: @@ -265,12 +265,19 @@ b2BodyId b2Shape_GetBody(b2ShapeId shapeId) b2Shape* shape = b2GetShape(world, shapeId); b2Body* body = world->bodies + shape->bodyIndex; - B2_ASSERT(b2ObjectValid(&body->object)); + B2_ASSERT(b2IsValidObject(&body->object)); b2BodyId bodyId = {body->object.index + 1, shapeId.world, body->object.revision}; return bodyId; } +void b2Shape_SetUserData(b2ShapeId shapeId, void* userData) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); + shape->userData = userData; +} + void* b2Shape_GetUserData(b2ShapeId shapeId) { b2World* world = b2GetWorldFromIndex(shapeId.world); @@ -291,7 +298,7 @@ bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point) b2Shape* shape = b2GetShape(world, shapeId); b2Body* body = world->bodies + shape->bodyIndex; - B2_ASSERT(b2ObjectValid(&body->object)); + B2_ASSERT(b2IsValidObject(&body->object)); b2Vec2 localPoint = b2InvTransformPoint(b2MakeTransform(body), point); @@ -311,6 +318,58 @@ bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point) } } +b2CastOutput b2Shape_RayCast(b2ShapeId shapeId, b2Vec2 origin, b2Vec2 translation) +{ + b2World* world = b2GetWorldFromIndex(shapeId.world); + b2Shape* shape = b2GetShape(world, shapeId); + + b2Body* body = world->bodies + shape->bodyIndex; + B2_ASSERT(b2IsValidObject(&body->object)); + b2Transform transform = b2MakeTransform(body); + + // input in local coordinates + b2RayCastInput input = {0}; + input.maxFraction = 1.0f; + input.origin = b2InvTransformPoint(transform, origin); + input.translation = b2InvRotateVector(transform.q, translation); + + b2CastOutput output = {0}; + switch (shape->type) + { + case b2_capsuleShape: + output = b2RayCastCapsule(&input, &shape->capsule); + break; + + case b2_circleShape: + output = b2RayCastCircle(&input, &shape->circle); + break; + + case b2_segmentShape: + output = b2RayCastSegment(&input, &shape->segment, false); + break; + + case b2_polygonShape: + output = b2RayCastPolygon(&input, &shape->polygon); + break; + + case b2_smoothSegmentShape: + output = b2RayCastSegment(&input, &shape->smoothSegment.segment, true); + break; + + default: + B2_ASSERT(false); + return output; + } + + if (output.hit) + { + // convert to world coordinates + output.normal = b2RotateVector(transform.q, output.normal); + output.point = b2TransformPoint(transform, output.point); + } + return output; +} + void b2Shape_SetDensity(b2ShapeId shapeId, float density) { B2_ASSERT(b2IsValid(density) && density >= 0.0f); @@ -329,11 +388,6 @@ void b2Shape_SetDensity(b2ShapeId shapeId, float density) } shape->density = density; - - b2Body* body = world->bodies + shape->bodyIndex; - B2_ASSERT(b2ObjectValid(&body->object)); - - b2UpdateBodyMassData(world, body); } float b2Shape_GetDensity(b2ShapeId shapeId) @@ -394,15 +448,55 @@ b2Filter b2Shape_GetFilter(b2ShapeId shapeId) return shape->filter; } +static void b2ResetContactsAndProxy(b2World* world, b2Shape* shape) +{ + int32_t shapeIndex = shape->object.index; + + b2Body* body = world->bodies + shape->bodyIndex; + B2_ASSERT(b2IsValidObject(&body->object)); + + // Destroy any contacts associated with the shape + int32_t contactKey = body->contactList; + while (contactKey != B2_NULL_INDEX) + { + int32_t contactIndex = contactKey >> 1; + int32_t edgeIndex = contactKey & 1; + + b2Contact* contact = world->contacts + contactIndex; + contactKey = contact->edges[edgeIndex].nextKey; + + if (contact->shapeIndexA == shapeIndex || contact->shapeIndexB == shapeIndex) + { + b2DestroyContact(world, contact); + } + } + + if (body->isEnabled) + { + // Must recreate proxy due to changed filter bits that exist in the dynamic tree + b2DestroyShapeProxy(shape, &world->broadPhase); + b2CreateShapeProxy(shape, &world->broadPhase, body->type, b2MakeTransform(body)); + } + else + { + B2_ASSERT(shape->proxyKey == B2_NULL_INDEX); + } +} + void b2Shape_SetFilter(b2ShapeId shapeId, b2Filter filter) { - b2World* world = b2GetWorldFromIndex(shapeId.world); + b2World* world = b2GetWorldFromIndexLocked(shapeId.world); + if (world == NULL) + { + return; + } + b2Shape* shape = b2GetShape(world, shapeId); shape->filter = filter; int32_t shapeIndex = shape->object.index; b2Body* body = world->bodies + shape->bodyIndex; - B2_ASSERT(b2ObjectValid(&body->object)); + B2_ASSERT(b2IsValidObject(&body->object)); // Destroy any contacts associated with the shape int32_t contactKey = body->contactList; @@ -422,6 +516,7 @@ void b2Shape_SetFilter(b2ShapeId shapeId, b2Filter filter) if (body->isEnabled) { + // Must recreate proxy due to changed filter bits that exist in the dynamic tree b2DestroyShapeProxy(shape, &world->broadPhase); b2CreateShapeProxy(shape, &world->broadPhase, body->type, b2MakeTransform(body)); } @@ -438,44 +533,100 @@ b2ShapeType b2Shape_GetType(b2ShapeId shapeId) return shape->type; } -const b2Circle* b2Shape_GetCircle(b2ShapeId shapeId) +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; + return shape->circle; } -const b2Segment* b2Shape_GetSegment(b2ShapeId shapeId) +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; + return shape->segment; } -const b2SmoothSegment* b2Shape_GetSmoothSegment(b2ShapeId shapeId) +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; + return shape->smoothSegment; } -const b2Capsule* b2Shape_GetCapsule(b2ShapeId shapeId) +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; + return shape->capsule; } -const b2Polygon* b2Shape_GetPolygon(b2ShapeId shapeId) +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; + return shape->polygon; +} + +void b2Shape_SetCircle(b2ShapeId shapeId, b2Circle circle) +{ + b2World* world = b2GetWorldFromIndexLocked(shapeId.world); + if (world == NULL) + { + return; + } + + b2Shape* shape = b2GetShape(world, shapeId); + shape->circle = circle; + shape->type = b2_circleShape; + b2ResetContactsAndProxy(world, shape); +} + +void b2Shape_SetCapsule(b2ShapeId shapeId, b2Capsule capsule) +{ + b2World* world = b2GetWorldFromIndexLocked(shapeId.world); + if (world == NULL) + { + return; + } + + b2Shape* shape = b2GetShape(world, shapeId); + shape->capsule = capsule; + shape->type = b2_capsuleShape; + b2ResetContactsAndProxy(world, shape); +} + +void b2Shape_SetSegment(b2ShapeId shapeId, b2Segment segment) +{ + b2World* world = b2GetWorldFromIndexLocked(shapeId.world); + if (world == NULL) + { + return; + } + + b2Shape* shape = b2GetShape(world, shapeId); + shape->segment = segment; + shape->type = b2_segmentShape; + b2ResetContactsAndProxy(world, shape); +} + +void b2Shape_SetPolygon(b2ShapeId shapeId, b2Polygon polygon) +{ + b2World* world = b2GetWorldFromIndexLocked(shapeId.world); + if (world == NULL) + { + return; + } + + b2Shape* shape = b2GetShape(world, shapeId); + shape->polygon = polygon; + shape->type = b2_polygonShape; + b2ResetContactsAndProxy(world, shape); } b2ChainId b2Shape_GetParentChain(b2ShapeId shapeId) @@ -489,7 +640,7 @@ b2ChainId b2Shape_GetParentChain(b2ShapeId shapeId) { B2_ASSERT(0 <= chainIndex && chainIndex < world->chainPool.capacity); b2ChainShape* chain = world->chains + chainIndex; - B2_ASSERT(b2ObjectValid(&chain->object)); + B2_ASSERT(b2IsValidObject(&chain->object)); b2ChainId chainId = {chainIndex, shapeId.world, chain->object.revision}; return chainId; } @@ -550,6 +701,7 @@ int32_t b2Shape_GetContactCapacity(b2ShapeId shapeId) b2Shape* shape = b2GetShape(world, shapeId); b2Body* body = world->bodies + shape->bodyIndex; + B2_ASSERT(b2IsValidObject(&body->object)); // Conservative and fast return body->contactCount; diff --git a/src/shape.h b/src/shape.h index 2dbf3903..9fd2ef00 100644 --- a/src/shape.h +++ b/src/shape.h @@ -74,7 +74,7 @@ b2Vec2 b2GetShapeCentroid(const b2Shape* shape); 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); +b2CastOutput b2RayCastShape(const b2RayCastInput* input, const b2Shape* shape, b2Transform xf); +b2CastOutput b2ShapeCastShape(const b2ShapeCastInput* input, const b2Shape* shape, b2Transform xf); b2Shape* b2GetShape(b2World* world, b2ShapeId shapeId); diff --git a/src/graph.c b/src/solver.c similarity index 81% rename from src/graph.c rename to src/solver.c index ea4adeb8..edeed7ce 100644 --- a/src/graph.c +++ b/src/solver.c @@ -1,11 +1,11 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT -#include "graph.h" +#include "solver.h" #include "aabb.h" #include "allocate.h" -#include "arena_allocator.h" +#include "stack_allocator.h" #include "array.h" #include "bitset.inl" #include "body.h" @@ -14,7 +14,7 @@ #include "core.h" #include "joint.h" #include "shape.h" -#include "solver_data.h" +#include "solver.h" #include "world.h" // for mm_pause @@ -25,18 +25,6 @@ #include #include -// Solver using graph coloring. Islands are only used for sleep. -// High-Performance Physical Simulations on Next-Generation Architecture with Many Cores -// http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf - -// Kinematic bodies have to be treated like dynamic bodies in graph coloring. Unlike static bodies, we cannot use a dummy solver -// body for kinematic bodies. We cannot access a kinematic body from multiple threads efficiently because the SIMD solver body -// scatter would write to the same kinematic body from multiple threads. Even if these writes don't modify the body, they will -// cause horrible cache stalls. To make this feasible I would need a way to block these writes. - -// This is used for debugging by making all constraints be assigned to overflow. -#define B2_FORCE_OVERFLOW 0 - typedef struct b2WorkerContext { b2StepContext* context; @@ -44,382 +32,6 @@ typedef struct b2WorkerContext void* userTask; } b2WorkerContext; -void b2CreateGraph(b2ConstraintGraph* graph, int32_t bodyCapacity, int32_t contactCapacity, int32_t jointCapacity) -{ - *graph = (b2ConstraintGraph){0}; - - bodyCapacity = B2_MAX(bodyCapacity, 8); - contactCapacity = B2_MAX(contactCapacity, 8); - jointCapacity = B2_MAX(jointCapacity, 8); - - for (int32_t i = 0; i < b2_graphColorCount; ++i) - { - b2GraphColor* color = graph->colors + i; - color->bodySet = b2CreateBitSet(bodyCapacity); - b2SetBitCountAndClear(&color->bodySet, bodyCapacity); - - color->contactArray = b2CreateArray(sizeof(int32_t), contactCapacity); - color->jointArray = b2CreateArray(sizeof(int32_t), jointCapacity); - color->contactConstraints = NULL; - } - - graph->overflow.contactArray = b2CreateArray(sizeof(int32_t), contactCapacity); - graph->overflow.jointArray = b2CreateArray(sizeof(int32_t), jointCapacity); - graph->overflow.contactConstraints = NULL; -} - -void b2DestroyGraph(b2ConstraintGraph* graph) -{ - for (int32_t i = 0; i < b2_graphColorCount; ++i) - { - b2GraphColor* color = graph->colors + i; - b2DestroyBitSet(&color->bodySet); - b2DestroyArray(color->contactArray, sizeof(int32_t)); - b2DestroyArray(color->jointArray, sizeof(int32_t)); - } - - b2DestroyArray(graph->overflow.contactArray, sizeof(int32_t)); - b2DestroyArray(graph->overflow.jointArray, sizeof(int32_t)); -} - -void b2AddContactToGraph(b2World* world, b2Contact* contact) -{ - B2_ASSERT(contact->colorIndex == B2_NULL_INDEX); - B2_ASSERT(contact->colorSubIndex == B2_NULL_INDEX); - - b2ConstraintGraph* graph = &world->graph; - -#if B2_FORCE_OVERFLOW == 0 - int32_t bodyIndexA = contact->edges[0].bodyIndex; - int32_t bodyIndexB = contact->edges[1].bodyIndex; - - b2BodyType typeA = world->bodies[bodyIndexA].type; - b2BodyType typeB = world->bodies[bodyIndexB].type; - B2_ASSERT(typeA != b2_staticBody || typeB != b2_staticBody); - - if (typeA != b2_staticBody && typeB != b2_staticBody) - { - for (int32_t i = 0; i < b2_graphColorCount; ++i) - { - b2GraphColor* color = graph->colors + i; - if (b2GetBit(&color->bodySet, bodyIndexA) || b2GetBit(&color->bodySet, bodyIndexB)) - { - continue; - } - - b2SetBitGrow(&color->bodySet, bodyIndexA); - b2SetBitGrow(&color->bodySet, bodyIndexB); - - contact->colorSubIndex = b2Array(color->contactArray).count; - b2Array_Push(color->contactArray, contact->object.index); - contact->colorIndex = i; - break; - } - } - else if (typeA != b2_staticBody) - { - // Static contacts never in color 0 - for (int32_t i = 1; i < b2_graphColorCount; ++i) - { - b2GraphColor* color = graph->colors + i; - if (b2GetBit(&color->bodySet, bodyIndexA)) - { - continue; - } - - b2SetBitGrow(&color->bodySet, bodyIndexA); - - contact->colorSubIndex = b2Array(color->contactArray).count; - b2Array_Push(color->contactArray, contact->object.index); - contact->colorIndex = i; - break; - } - } - else if (typeB != b2_staticBody) - { - // Static contacts never in color 0 - for (int32_t i = 1; i < b2_graphColorCount; ++i) - { - b2GraphColor* color = graph->colors + i; - if (b2GetBit(&color->bodySet, bodyIndexB)) - { - continue; - } - - b2SetBitGrow(&color->bodySet, bodyIndexB); - - contact->colorSubIndex = b2Array(color->contactArray).count; - b2Array_Push(color->contactArray, contact->object.index); - contact->colorIndex = i; - break; - } - } -#endif - - // Overflow - if (contact->colorIndex == B2_NULL_INDEX) - { - contact->colorSubIndex = b2Array(graph->overflow.contactArray).count; - b2Array_Push(graph->overflow.contactArray, contact->object.index); - contact->colorIndex = b2_overflowIndex; - } -} - -void b2RemoveContactFromGraph(b2World* world, b2Contact* contact) -{ - B2_ASSERT(contact->colorIndex != B2_NULL_INDEX); - B2_ASSERT(contact->colorSubIndex != B2_NULL_INDEX); - - b2ConstraintGraph* graph = &world->graph; - - // Overflow - if (contact->colorIndex == b2_overflowIndex) - { - int32_t colorSubIndex = contact->colorSubIndex; - b2Array_RemoveSwap(graph->overflow.contactArray, colorSubIndex); - if (colorSubIndex < b2Array(graph->overflow.contactArray).count) - { - // Fix index on swapped contact - int32_t swappedIndex = graph->overflow.contactArray[colorSubIndex]; - B2_ASSERT(world->contacts[swappedIndex].colorIndex == b2_overflowIndex); - world->contacts[swappedIndex].colorSubIndex = colorSubIndex; - } - - contact->colorIndex = B2_NULL_INDEX; - contact->colorSubIndex = B2_NULL_INDEX; - - return; - } - - B2_ASSERT(0 <= contact->colorIndex && contact->colorIndex < b2_graphColorCount); - int32_t bodyIndexA = contact->edges[0].bodyIndex; - int32_t bodyIndexB = contact->edges[1].bodyIndex; - - b2BodyType typeA = world->bodies[bodyIndexA].type; - b2BodyType typeB = world->bodies[bodyIndexB].type; - B2_ASSERT(typeA != b2_staticBody || typeB != b2_staticBody); - - if (typeA != b2_staticBody && typeB != b2_staticBody) - { - b2GraphColor* color = graph->colors + contact->colorIndex; - B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA) && b2GetBit(&color->bodySet, bodyIndexB)); - - int32_t colorSubIndex = contact->colorSubIndex; - b2Array_RemoveSwap(color->contactArray, colorSubIndex); - if (colorSubIndex < b2Array(color->contactArray).count) - { - // Fix index on swapped contact - int32_t swappedIndex = color->contactArray[colorSubIndex]; - world->contacts[swappedIndex].colorSubIndex = colorSubIndex; - } - - b2ClearBit(&color->bodySet, bodyIndexA); - b2ClearBit(&color->bodySet, bodyIndexB); - } - else if (typeA != b2_staticBody) - { - b2GraphColor* color = graph->colors + contact->colorIndex; - B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA)); - - int32_t colorSubIndex = contact->colorSubIndex; - b2Array_RemoveSwap(color->contactArray, colorSubIndex); - if (colorSubIndex < b2Array(color->contactArray).count) - { - // Fix index on swapped contact - int32_t swappedIndex = color->contactArray[colorSubIndex]; - world->contacts[swappedIndex].colorSubIndex = colorSubIndex; - } - - b2ClearBit(&color->bodySet, bodyIndexA); - } - else if (typeB != b2_staticBody) - { - b2GraphColor* color = graph->colors + contact->colorIndex; - B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexB)); - - int32_t colorSubIndex = contact->colorSubIndex; - b2Array_RemoveSwap(color->contactArray, colorSubIndex); - if (colorSubIndex < b2Array(color->contactArray).count) - { - // Fix index on swapped contact - int32_t swappedIndex = color->contactArray[colorSubIndex]; - world->contacts[swappedIndex].colorSubIndex = colorSubIndex; - } - - b2ClearBit(&color->bodySet, bodyIndexB); - } - - contact->colorIndex = B2_NULL_INDEX; - contact->colorSubIndex = B2_NULL_INDEX; -} - -void b2AddJointToGraph(b2World* world, b2Joint* joint) -{ - B2_ASSERT(joint->colorIndex == B2_NULL_INDEX); - B2_ASSERT(joint->colorSubIndex == B2_NULL_INDEX); - - b2ConstraintGraph* graph = &world->graph; - -#if B2_FORCE_OVERFLOW == 0 - int32_t bodyIndexA = joint->edges[0].bodyIndex; - int32_t bodyIndexB = joint->edges[1].bodyIndex; - - b2BodyType typeA = world->bodies[bodyIndexA].type; - b2BodyType typeB = world->bodies[bodyIndexB].type; - - if (typeA == b2_dynamicBody && typeB == b2_dynamicBody) - { - for (int32_t i = 0; i < b2_graphColorCount; ++i) - { - b2GraphColor* color = graph->colors + i; - if (b2GetBit(&color->bodySet, bodyIndexA) || b2GetBit(&color->bodySet, bodyIndexB)) - { - continue; - } - - b2SetBitGrow(&color->bodySet, bodyIndexA); - b2SetBitGrow(&color->bodySet, bodyIndexB); - - joint->colorSubIndex = b2Array(color->jointArray).count; - b2Array_Push(color->jointArray, joint->object.index); - joint->colorIndex = i; - break; - } - } - else if (typeA == b2_dynamicBody) - { - for (int32_t i = 0; i < b2_graphColorCount; ++i) - { - b2GraphColor* color = graph->colors + i; - if (b2GetBit(&color->bodySet, bodyIndexA)) - { - continue; - } - - b2SetBitGrow(&color->bodySet, bodyIndexA); - - joint->colorSubIndex = b2Array(color->jointArray).count; - b2Array_Push(color->jointArray, joint->object.index); - joint->colorIndex = i; - break; - } - } - else if (typeB == b2_dynamicBody) - { - for (int32_t i = 0; i < b2_graphColorCount; ++i) - { - b2GraphColor* color = graph->colors + i; - if (b2GetBit(&color->bodySet, bodyIndexB)) - { - continue; - } - - b2SetBitGrow(&color->bodySet, bodyIndexB); - - joint->colorSubIndex = b2Array(color->jointArray).count; - b2Array_Push(color->jointArray, joint->object.index); - joint->colorIndex = i; - break; - } - } -#endif - - // Overflow - if (joint->colorIndex == B2_NULL_INDEX) - { - joint->colorSubIndex = b2Array(graph->overflow.jointArray).count; - b2Array_Push(graph->overflow.jointArray, joint->object.index); - joint->colorIndex = b2_overflowIndex; - } -} - -void b2RemoveJointFromGraph(b2World* world, b2Joint* joint) -{ - B2_ASSERT(joint->colorIndex != B2_NULL_INDEX); - B2_ASSERT(joint->colorSubIndex != B2_NULL_INDEX); - - b2ConstraintGraph* graph = &world->graph; - - // Overflow - if (joint->colorIndex == b2_overflowIndex) - { - int32_t colorSubIndex = joint->colorSubIndex; - b2Array_RemoveSwap(graph->overflow.jointArray, colorSubIndex); - if (colorSubIndex < b2Array(graph->overflow.jointArray).count) - { - // Fix index on swapped joint - int32_t swappedIndex = graph->overflow.jointArray[colorSubIndex]; - B2_ASSERT(world->joints[swappedIndex].colorIndex == b2_overflowIndex); - world->joints[swappedIndex].colorSubIndex = colorSubIndex; - } - - joint->colorIndex = B2_NULL_INDEX; - joint->colorSubIndex = B2_NULL_INDEX; - - return; - } - - B2_ASSERT(0 <= joint->colorIndex && joint->colorIndex < b2_graphColorCount); - int32_t bodyIndexA = joint->edges[0].bodyIndex; - int32_t bodyIndexB = joint->edges[1].bodyIndex; - - b2BodyType typeA = world->bodies[bodyIndexA].type; - b2BodyType typeB = world->bodies[bodyIndexB].type; - - if (typeA == b2_dynamicBody && typeB == b2_dynamicBody) - { - b2GraphColor* color = graph->colors + joint->colorIndex; - B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA) && b2GetBit(&color->bodySet, bodyIndexB)); - - int32_t colorSubIndex = joint->colorSubIndex; - b2Array_RemoveSwap(color->jointArray, colorSubIndex); - if (colorSubIndex < b2Array(color->jointArray).count) - { - // Fix index on swapped joint - int32_t swappedIndex = color->jointArray[colorSubIndex]; - world->joints[swappedIndex].colorSubIndex = colorSubIndex; - } - - b2ClearBit(&color->bodySet, bodyIndexA); - b2ClearBit(&color->bodySet, bodyIndexB); - } - else if (typeA == b2_dynamicBody) - { - b2GraphColor* color = graph->colors + joint->colorIndex; - B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA)); - - int32_t colorSubIndex = joint->colorSubIndex; - b2Array_RemoveSwap(color->jointArray, colorSubIndex); - if (colorSubIndex < b2Array(color->jointArray).count) - { - // Fix index on swapped joint - int32_t swappedIndex = color->jointArray[colorSubIndex]; - world->joints[swappedIndex].colorSubIndex = colorSubIndex; - } - - b2ClearBit(&color->bodySet, bodyIndexA); - } - else if (typeB == b2_dynamicBody) - { - b2GraphColor* color = graph->colors + joint->colorIndex; - B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexB)); - - int32_t colorSubIndex = joint->colorSubIndex; - b2Array_RemoveSwap(color->jointArray, colorSubIndex); - if (colorSubIndex < b2Array(color->jointArray).count) - { - // Fix index on swapped joint - int32_t swappedIndex = color->jointArray[colorSubIndex]; - world->joints[swappedIndex].colorSubIndex = colorSubIndex; - } - - b2ClearBit(&color->bodySet, bodyIndexB); - } - - joint->colorIndex = B2_NULL_INDEX; - joint->colorSubIndex = B2_NULL_INDEX; -} - // Integrate velocities and apply damping. static void b2IntegrateVelocitiesTask(int32_t startIndex, int32_t endIndex, b2StepContext* context) { @@ -483,7 +95,7 @@ static void b2PrepareJointsTask(int32_t startIndex, int32_t endIndex, b2StepCont B2_ASSERT(0 <= index && index < world->jointPool.capacity); b2Joint* joint = joints + index; - B2_ASSERT(b2ObjectValid(&joint->object) == true); + B2_ASSERT(b2IsValidObject(&joint->object) == true); b2PrepareJoint(joint, context); } @@ -505,7 +117,7 @@ static void b2WarmStartJointsTask(int32_t startIndex, int32_t endIndex, b2StepCo B2_ASSERT(0 <= index && index < world->jointPool.capacity); b2Joint* joint = joints + index; - B2_ASSERT(b2ObjectValid(&joint->object) == true); + B2_ASSERT(b2IsValidObject(&joint->object) == true); b2WarmStartJoint(joint, context); } @@ -527,7 +139,7 @@ static void b2SolveJointsTask(int32_t startIndex, int32_t endIndex, b2StepContex B2_ASSERT(0 <= index && index < world->jointPool.capacity); b2Joint* joint = joints + index; - B2_ASSERT(b2ObjectValid(&joint->object) == true); + B2_ASSERT(b2IsValidObject(&joint->object) == true); b2SolveJoint(joint, context, useBias); } @@ -588,7 +200,7 @@ static void b2FinalizeBodiesTask(int32_t startIndex, int32_t endIndex, uint32_t int32_t bodyIndex = solverToBodyMap[i]; b2Body* body = bodies + bodyIndex; - B2_ASSERT(b2ObjectValid(&body->object)); + B2_ASSERT(b2IsValidObject(&body->object)); b2Vec2 v = state->linearVelocity; float w = state->angularVelocity; @@ -1169,7 +781,7 @@ static bool b2SolveConstraintGraph(b2World* world, b2StepContext* context) while (bodyIndex != B2_NULL_INDEX) { b2Body* body = bodies + bodyIndex; - B2_ASSERT(b2ObjectValid(&body->object)); + B2_ASSERT(b2IsValidObject(&body->object)); B2_ASSERT(body->object.index == bodyIndex); awakeBodies[index] = body; @@ -1792,7 +1404,7 @@ static bool b2SolveConstraintGraph(b2World* world, b2StepContext* context) uint32_t ctz = b2CTZ(word); uint32_t islandIndex = 64 * k + ctz; - B2_ASSERT(b2ObjectValid(&islands[islandIndex].object)); + B2_ASSERT(b2IsValidObject(&islands[islandIndex].object)); b2Array_Push(world->awakeIslandArray, islandIndex); @@ -1810,7 +1422,7 @@ static bool b2SolveConstraintGraph(b2World* world, b2StepContext* context) for (int32_t i = 0; i < world->islandPool.capacity; ++i) { b2Island* island = world->islands + i; - if (b2ObjectValid(&island->object) == false) + if (b2IsValidObject(&island->object) == false) { continue; } @@ -1953,7 +1565,7 @@ static bool b2ContinuousQueryCallback(int32_t proxyId, int32_t shapeIndex, void* static void b2SolveContinuous(b2World* world, int32_t bodyIndex) { b2Body* fastBody = world->bodies + bodyIndex; - B2_ASSERT(b2ObjectValid(&fastBody->object)); + B2_ASSERT(b2IsValidObject(&fastBody->object)); B2_ASSERT(fastBody->type == b2_dynamicBody && fastBody->isFast); b2Shape* shapes = world->shapes; @@ -2149,7 +1761,7 @@ void b2Solve(b2World* world, b2StepContext* context) uint32_t shapeIndex = 64 * k + ctz; b2Shape* shape = shapes + shapeIndex; - B2_ASSERT(b2ObjectValid(&shape->object)); + B2_ASSERT(b2IsValidObject(&shape->object)); if (shape->isFast == false) { b2BroadPhase_EnlargeProxy(broadPhase, shape->proxyKey, shape->fatAABB); diff --git a/src/solver_data.h b/src/solver.h similarity index 97% rename from src/solver_data.h rename to src/solver.h index eb91bfa7..975af96e 100644 --- a/src/solver_data.h +++ b/src/solver.h @@ -11,6 +11,7 @@ typedef struct b2BodyState b2BodyState; typedef struct b2BodyParam b2BodyParam; +typedef struct b2World b2World; typedef struct b2Softness { @@ -121,7 +122,6 @@ typedef struct b2StepContext bool enableWarmStarting; } b2StepContext; - static inline b2Softness b2MakeSoft(float hertz, float zeta, float h) { if (hertz == 0.0f) @@ -135,3 +135,5 @@ static inline b2Softness b2MakeSoft(float hertz, float zeta, float h) float a3 = 1.0f / (1.0f + a2); return (b2Softness){omega / a1, a2 * a3, a3}; } + +void b2Solve(b2World* world, b2StepContext* stepContext); diff --git a/src/arena_allocator.c b/src/stack_allocator.c similarity index 99% rename from src/arena_allocator.c rename to src/stack_allocator.c index e449aea9..f41a99e7 100644 --- a/src/arena_allocator.c +++ b/src/stack_allocator.c @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT -#include "arena_allocator.h" +#include "stack_allocator.h" #include "allocate.h" #include "array.h" diff --git a/src/arena_allocator.h b/src/stack_allocator.h similarity index 100% rename from src/arena_allocator.h rename to src/stack_allocator.h diff --git a/src/types.c b/src/types.c index 373b89f4..227e9f14 100644 --- a/src/types.c +++ b/src/types.c @@ -4,3 +4,62 @@ #include "box2d/types.h" #include + +b2WorldDef b2DefaultWorldDef() +{ + b2WorldDef def = {0}; + def.gravity.x = 0.0f; + def.gravity.y = -10.0f; + def.restitutionThreshold = 1.0f * b2_lengthUnitsPerMeter; + def.contactPushoutVelocity = 3.0f * b2_lengthUnitsPerMeter; + def.contactHertz = 30.0; + def.contactDampingRatio = 10.0f; + def.jointHertz = 60.0; + def.jointDampingRatio = 2.0f; + def.enableSleep = true; + def.enableContinous = true; + def.stackAllocatorCapacity = 1024 * 1024; + return def; +} + +b2BodyDef b2DefaultBodyDef() +{ + b2BodyDef def = {0}; + def.type = b2_staticBody; + def.gravityScale = 1.0f; + def.enableSleep = true; + def.isAwake = true; + def.isEnabled = true; + return def; +} + +b2Filter b2DefaultFilter() +{ + b2Filter filter = {0x00000001, 0xFFFFFFFF, 0}; + return filter; +} + +b2QueryFilter b2DefaultQueryFilter() +{ + b2QueryFilter filter = {0x00000001, 0xFFFFFFFF}; + return filter; +} + +b2ShapeDef b2DefaultShapeDef() +{ + b2ShapeDef def = {0}; + def.friction = 0.6f; + def.density = 1.0f; + def.filter = b2DefaultFilter(); + def.enableSensorEvents = true; + def.enableContactEvents = true; + return def; +} + +b2ChainDef b2DefaultChainDef() +{ + b2ChainDef def = {0}; + def.friction = 0.6f; + def.filter = b2DefaultFilter(); + return def; +} diff --git a/src/weld_joint.c b/src/weld_joint.c index ebb6bb58..645ab8df 100644 --- a/src/weld_joint.c +++ b/src/weld_joint.c @@ -4,12 +4,64 @@ #include "body.h" #include "core.h" #include "joint.h" -#include "solver_data.h" +#include "solver.h" #include "world.h" // needed for dll export #include "box2d/box2d.h" +void b2WeldJoint_SetLinearHertz(b2JointId jointId, float hertz) +{ + B2_ASSERT(b2IsValid(hertz) && hertz >= 0.0f); + b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); + joint->weldJoint.linearHertz = hertz; +} + +float b2WeldJoint_GetLinearHertz(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); + return joint->weldJoint.linearHertz; +} + +void b2WeldJoint_SetLinearDampingRatio(b2JointId jointId, float dampingRatio) +{ + B2_ASSERT(b2IsValid(dampingRatio) && dampingRatio >= 0.0f); + b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); + joint->weldJoint.linearDampingRatio = dampingRatio; +} + +float b2WeldJoint_GetLinearDampingRatio(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); + return joint->weldJoint.linearDampingRatio; +} + +void b2WeldJoint_SetAngularHertz(b2JointId jointId, float hertz) +{ + B2_ASSERT(b2IsValid(hertz) && hertz >= 0.0f); + b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); + joint->weldJoint.angularHertz = hertz; +} + +float b2WeldJoint_GetAngularHertz(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); + return joint->weldJoint.angularHertz; +} + +void b2WeldJoint_SetAngularDampingRatio(b2JointId jointId, float dampingRatio) +{ + B2_ASSERT(b2IsValid(dampingRatio) && dampingRatio >= 0.0f); + b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); + joint->weldJoint.angularDampingRatio = dampingRatio; +} + +float b2WeldJoint_GetAngularDampingRatio(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); + return joint->weldJoint.angularDampingRatio; +} + // Point-to-point constraint // C = p2 - p1 // Cdot = v2 - v1 @@ -32,8 +84,8 @@ void b2PrepareWeldJoint(b2Joint* base, b2StepContext* context) int32_t indexB = base->edges[1].bodyIndex; b2Body* bodyA = context->bodies + indexA; b2Body* bodyB = context->bodies + indexB; - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); float mA = bodyA->invMass; float iA = bodyA->invI; @@ -217,32 +269,3 @@ void b2DumpWeldJoint() b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } #endif - -void b2WeldJoint_SetLinearHertz(b2JointId jointId, float hertz) -{ - B2_ASSERT(b2IsValid(hertz) && hertz >= 0.0f); - - b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); - joint->weldJoint.linearHertz = hertz; -} - -void b2WeldJoint_SetLinearDampingRatio(b2JointId jointId, float dampingRatio) -{ - B2_ASSERT(b2IsValid(dampingRatio) && dampingRatio >= 0.0f); - b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); - joint->weldJoint.linearDampingRatio = dampingRatio; -} - -void b2WeldJoint_SetAngularHertz(b2JointId jointId, float hertz) -{ - B2_ASSERT(b2IsValid(hertz) && hertz >= 0.0f); - b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); - joint->weldJoint.angularHertz = hertz; -} - -void b2WeldJoint_SetAngularDampingRatio(b2JointId jointId, float dampingRatio) -{ - B2_ASSERT(b2IsValid(dampingRatio) && dampingRatio >= 0.0f); - b2Joint* joint = b2GetJointCheckType(jointId, b2_weldJoint); - joint->weldJoint.angularDampingRatio = dampingRatio; -} diff --git a/src/wheel_joint.c b/src/wheel_joint.c index 1b117ff5..9fff7fb3 100644 --- a/src/wheel_joint.c +++ b/src/wheel_joint.c @@ -4,7 +4,7 @@ #include "body.h" #include "core.h" #include "joint.h" -#include "solver_data.h" +#include "solver.h" #include "world.h" // needed for dll export @@ -13,6 +13,128 @@ #include +void b2WheelJoint_SetSpringHertz(b2JointId jointId, float hertz) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + joint->wheelJoint.hertz = hertz; +} + +float b2WheelJoint_GetSpringHertz(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + return joint->wheelJoint.hertz; +} + +void b2WheelJoint_SetSpringDampingRatio(b2JointId jointId, float dampingRatio) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + joint->wheelJoint.dampingRatio = dampingRatio; +} + +float b2WheelJoint_GetSpringDampingRatio(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + return joint->wheelJoint.dampingRatio; +} + +void b2WheelJoint_EnableLimit(b2JointId jointId, bool enableLimit) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + + if (joint->wheelJoint.enableLimit != enableLimit) + { + joint->wheelJoint.lowerImpulse = 0.0f; + joint->wheelJoint.upperImpulse = 0.0f; + } + + joint->wheelJoint.enableLimit = enableLimit; +} + +bool b2WheelJoint_IsLimitEnabled(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + return joint->wheelJoint.enableLimit; +} + +void b2WheelJoint_EnableMotor(b2JointId jointId, bool enableMotor) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + + if (joint->wheelJoint.enableMotor != enableMotor) + { + joint->wheelJoint.motorImpulse = 0.0f; + } + + joint->wheelJoint.enableMotor = enableMotor; +} + +bool b2WheelJoint_IsMotorEnabled(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + return joint->wheelJoint.enableMotor; +} + +void b2WheelJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + joint->wheelJoint.motorSpeed = motorSpeed; +} + +float b2WheelJoint_GetMotorSpeed(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + return joint->wheelJoint.motorSpeed; +} + +float b2WheelJoint_GetMotorTorque(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_wheelJoint); + + return world->inv_h * joint->wheelJoint.motorImpulse; +} + +void b2WheelJoint_SetMaxMotorTorque(b2JointId jointId, float torque) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + joint->wheelJoint.maxMotorTorque = torque; +} + +float b2WheelJoint_GetMaxMotorTorque(b2JointId jointId) +{ + b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); + return joint->wheelJoint.maxMotorTorque; +} + +b2Vec2 b2WheelJoint_GetConstraintForce(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* base = b2GetJoint(world, jointId); + B2_ASSERT(base->type == b2_wheelJoint); + + b2WheelJoint* joint = &base->wheelJoint; + + // This is a frame behind + b2Vec2 axisA = joint->axisA; + b2Vec2 perpA = b2LeftPerp(axisA); + + float perpForce = world->inv_h * joint->perpImpulse; + float axialForce = world->inv_h * (joint->springImpulse + joint->lowerImpulse - joint->upperImpulse); + + b2Vec2 force = b2Add(b2MulSV(perpForce, perpA), b2MulSV(axialForce, axisA)); + return force; +} + +float b2WheelJoint_GetConstraintTorque(b2JointId jointId) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_wheelJoint); + + return world->inv_h * joint->wheelJoint.motorImpulse; +} + // Linear constraint (point-to-line) // d = pB - pA = xB + rB - xA - rA // C = dot(ay, d) @@ -37,8 +159,8 @@ void b2PrepareWheelJoint(b2Joint* base, b2StepContext* context) int32_t indexB = base->edges[1].bodyIndex; b2Body* bodyA = context->bodies + indexA; b2Body* bodyB = context->bodies + indexB; - B2_ASSERT(b2ObjectValid(&bodyA->object)); - B2_ASSERT(b2ObjectValid(&bodyB->object)); + B2_ASSERT(b2IsValidObject(&bodyA->object)); + B2_ASSERT(b2IsValidObject(&bodyB->object)); float mA = bodyA->invMass; float iA = bodyA->invI; @@ -329,93 +451,6 @@ void b2SolveWheelJoint(b2Joint* base, b2StepContext* context, bool useBias) stateB->angularVelocity = wB; } -void b2WheelJoint_SetSpringHertz(b2JointId jointId, float hertz) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); - joint->wheelJoint.hertz = hertz; -} - -void b2WheelJoint_SetSpringDampingRatio(b2JointId jointId, float dampingRatio) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); - joint->wheelJoint.dampingRatio = dampingRatio; -} - -void b2WheelJoint_EnableLimit(b2JointId jointId, bool enableLimit) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); - - if (joint->wheelJoint.enableLimit != enableLimit) - { - joint->wheelJoint.lowerImpulse = 0.0f; - joint->wheelJoint.upperImpulse = 0.0f; - } - - joint->wheelJoint.enableLimit = enableLimit; - -} - -void b2WheelJoint_EnableMotor(b2JointId jointId, bool enableMotor) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); - - if (joint->wheelJoint.enableMotor != enableMotor) - { - joint->wheelJoint.motorImpulse = 0.0f; - } - - joint->wheelJoint.enableMotor = enableMotor; -} - -void b2WheelJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); - joint->wheelJoint.motorSpeed = motorSpeed; -} - -float b2WheelJoint_GetMotorTorque(b2JointId jointId) -{ - b2World* world = b2GetWorldFromIndex(jointId.world); - b2Joint* joint = b2GetJoint(world, jointId); - B2_ASSERT(joint->type == b2_wheelJoint); - - return world->inv_h * joint->wheelJoint.motorImpulse; -} - -void b2WheelJoint_SetMaxMotorTorque(b2JointId jointId, float torque) -{ - b2Joint* joint = b2GetJointCheckType(jointId, b2_wheelJoint); - joint->wheelJoint.maxMotorTorque = torque; -} - -b2Vec2 b2WheelJoint_GetConstraintForce(b2JointId jointId) -{ - b2World* world = b2GetWorldFromIndex(jointId.world); - b2Joint* base = b2GetJoint(world, jointId); - B2_ASSERT(base->type == b2_wheelJoint); - - b2WheelJoint* joint = &base->wheelJoint; - - // This is a frame behind - b2Vec2 axisA = joint->axisA; - b2Vec2 perpA = b2LeftPerp(axisA); - - float perpForce = world->inv_h * joint->perpImpulse; - float axialForce = world->inv_h * (joint->springImpulse + joint->lowerImpulse - joint->upperImpulse); - - b2Vec2 force = b2Add(b2MulSV(perpForce, perpA), b2MulSV(axialForce, axisA)); - return force; -} - -float b2WheelJoint_GetConstraintTorque(b2JointId jointId) -{ - b2World* world = b2GetWorldFromIndex(jointId.world); - b2Joint* joint = b2GetJoint(world, jointId); - B2_ASSERT(joint->type == b2_wheelJoint); - - return world->inv_h * joint->wheelJoint.motorImpulse; -} - #if 0 void b2WheelJoint_Dump() { diff --git a/src/world.c b/src/world.c index d922b32b..20cd0575 100644 --- a/src/world.c +++ b/src/world.c @@ -7,24 +7,25 @@ #include "aabb.h" #include "allocate.h" -#include "arena_allocator.h" +#include "stack_allocator.h" #include "array.h" #include "bitset.h" #include "bitset.inl" #include "block_allocator.h" #include "body.h" #include "broad_phase.h" +#include "constraint_graph.h" #include "contact.h" #include "core.h" -#include "graph.h" #include "island.h" #include "joint.h" #include "pool.h" #include "shape.h" -#include "solver_data.h" +#include "solver.h" // needed for dll export #include "box2d/box2d.h" +#include "box2d/color.h" #include "box2d/constants.h" #include "box2d/debug_draw.h" #include "box2d/distance.h" @@ -220,7 +221,7 @@ void b2DestroyWorld(b2WorldId id) for (int32_t i = 0; i < chainCapacity; ++i) { b2ChainShape* chain = world->chains + i; - if (b2ObjectValid(&chain->object)) + if (b2IsValidObject(&chain->object)) { b2Free(chain->shapeIndices, chain->count * sizeof(int32_t)); } @@ -650,7 +651,7 @@ static void b2DrawShape(b2DebugDraw* draw, b2Shape* shape, b2Transform xf, b2Col b2Vec2 p2 = b2TransformPoint(xf, segment->point2); draw->DrawSegment(p1, p2, color, draw->context); draw->DrawPoint(p2, 4.0f, color, draw->context); - draw->DrawSegment(p1, b2Lerp(p1, p2, 0.1f), b2MakeColor(b2_colorPaleGreen4, 1.0f), draw->context); + draw->DrawSegment(p1, b2Lerp(p1, p2, 0.1f), b2MakeColor(b2_colorPaleGreen4), draw->context); } break; @@ -695,39 +696,39 @@ void b2World_Draw(b2WorldId worldId, b2DebugDraw* draw) if (b->type == b2_dynamicBody && b->mass == 0.0f) { // Bad body - color = b2MakeColor(b2_colorRed, 0.5f); + color = b2MakeColor(b2_colorRed); } else if (b->isEnabled == false) { - color = b2MakeColor(b2_colorSlateGray2, 0.5f); + color = b2MakeColor(b2_colorSlateGray2); } else if (shape->isSensor) { - color = b2MakeColor(b2_colorWheat, 1.0f); + color = b2MakeColor(b2_colorWheat); } else if (b->isSpeedCapped) { - color = b2MakeColor(b2_colorYellow, 1.0f); + color = b2MakeColor(b2_colorYellow); } else if (b->isFast) { - color = b2MakeColor(b2_colorSalmon, 1.0f); + color = b2MakeColor(b2_colorSalmon); } else if (b->type == b2_staticBody) { - color = b2MakeColor(b2_colorPaleGreen, 1.0f); + color = b2MakeColor(b2_colorPaleGreen); } else if (b->type == b2_kinematicBody) { - color = (b2Color){0.5f, 0.5f, 0.9f, 1.0f}; + color = (b2Color){0.5f, 0.5f, 0.9f}; } else if (isAwake) { - color = b2MakeColor(b2_colorPink3, 1.0f); + color = b2MakeColor(b2_colorPink3); } else { - color = b2MakeColor(b2_colorGray, 1.0f); + color = b2MakeColor(b2_colorGray); } b2DrawShape(draw, shape, xf, color); @@ -794,7 +795,7 @@ void b2World_Draw(b2WorldId worldId, b2DebugDraw* draw) for (int32_t i = 0; i < bodyCapacity; ++i) { b2Body* body = bodies + i; - if (b2ObjectValid(&body->object) == false) + if (b2IsValidObject(&body->object) == false) { continue; } @@ -849,7 +850,7 @@ void b2World_Draw(b2WorldId worldId, b2DebugDraw* draw) { // graph color float pointSize = colorIndex == b2_graphColorCount ? 7.5f : 5.0f; - draw->DrawPoint(point->point, pointSize, b2MakeColor(colors[colorIndex], 1.0f), draw->context); + draw->DrawPoint(point->point, pointSize, b2MakeColor(colors[colorIndex]), draw->context); // g_draw.DrawString(point->position, "%d", point->color); } else if (point->separation > b2_linearSlop) @@ -955,7 +956,7 @@ bool b2Body_IsValid(b2BodyId id) } b2Body* body = world->bodies + (id.index - 1); - if (b2ObjectValid(&body->object) == false) + if (b2IsValidObject(&body->object) == false) { return false; } @@ -978,7 +979,7 @@ bool b2Shape_IsValid(b2ShapeId id) } b2Shape* shape = world->shapes + (id.index - 1); - if (b2ObjectValid(&shape->object) == false) + if (b2IsValidObject(&shape->object) == false) { return false; } @@ -1001,7 +1002,7 @@ bool b2Chain_IsValid(b2ChainId id) } b2ChainShape* chain = world->chains + (id.index - 1); - if (b2ObjectValid(&chain->object) == false) + if (b2IsValidObject(&chain->object) == false) { return false; } @@ -1024,7 +1025,7 @@ bool b2Joint_IsValid(b2JointId id) } b2Joint* joint = world->joints + (id.index - 1); - if (b2ObjectValid(&joint->object) == false) + if (b2IsValidObject(&joint->object) == false) { return false; } @@ -1185,6 +1186,8 @@ void b2World_QueryAABB(b2WorldId worldId, b2QueryResultFcn* fcn, b2AABB aabb, b2 return; } + B2_ASSERT(b2AABB_IsValid(aabb)); + WorldQueryContext worldContext = {world, fcn, filter, context}; for (int32_t i = 0; i < b2_bodyTypeCount; ++i) @@ -1253,6 +1256,9 @@ void b2World_OverlapCircle(b2WorldId worldId, b2QueryResultFcn* fcn, const b2Cir return; } + B2_ASSERT(b2Vec2_IsValid(transform.p)); + B2_ASSERT(b2Rot_IsValid(transform.q)); + b2AABB aabb = b2ComputeCircleAABB(circle, transform); WorldOverlapContext worldContext = { world, fcn, filter, b2MakeProxy(&circle->point, 1, circle->radius), transform, context, @@ -1274,6 +1280,9 @@ void b2World_OverlapCapsule(b2WorldId worldId, b2QueryResultFcn* fcn, const b2Ca return; } + B2_ASSERT(b2Vec2_IsValid(transform.p)); + B2_ASSERT(b2Rot_IsValid(transform.q)); + b2AABB aabb = b2ComputeCapsuleAABB(capsule, transform); WorldOverlapContext worldContext = { world, fcn, filter, b2MakeProxy(&capsule->point1, 2, capsule->radius), transform, context, @@ -1295,6 +1304,9 @@ void b2World_OverlapPolygon(b2WorldId worldId, b2QueryResultFcn* fcn, const b2Po return; } + B2_ASSERT(b2Vec2_IsValid(transform.p)); + B2_ASSERT(b2Rot_IsValid(transform.q)); + b2AABB aabb = b2ComputePolygonAABB(polygon, transform); WorldOverlapContext worldContext = { world, fcn, filter, b2MakeProxy(polygon->vertices, polygon->count, polygon->radius), transform, context, @@ -1337,10 +1349,10 @@ static float RayCastCallback(const b2RayCastInput* input, int32_t proxyId, int32 B2_ASSERT(0 <= bodyIndex && bodyIndex < world->bodyPool.capacity); b2Body* body = world->bodies + bodyIndex; - B2_ASSERT(b2ObjectValid(&body->object)); + B2_ASSERT(b2IsValidObject(&body->object)); b2Transform transform = b2MakeTransform(body); - b2RayCastOutput output = b2RayCastShape(input, shape, transform); + b2CastOutput output = b2RayCastShape(input, shape, transform); if (output.hit) { @@ -1363,9 +1375,10 @@ void b2World_RayCast(b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2Que return; } - b2RayCastInput input = {origin, translation, 1.0f}; + B2_ASSERT(b2Vec2_IsValid(origin)); + B2_ASSERT(b2Vec2_IsValid(translation)); - // todo validate input + b2RayCastInput input = {origin, translation, 1.0f}; WorldRayCastContext worldContext = {world, fcn, filter, 1.0f, context}; @@ -1405,6 +1418,9 @@ b2RayResult b2World_RayCastClosest(b2WorldId worldId, b2Vec2 origin, b2Vec2 tran return result; } + B2_ASSERT(b2Vec2_IsValid(origin)); + B2_ASSERT(b2Vec2_IsValid(translation)); + b2RayCastInput input = {origin, translation, 1.0f}; WorldRayCastContext worldContext = {world, b2RayCastClosestFcn, filter, 1.0f, &result}; @@ -1445,10 +1461,10 @@ static float ShapeCastCallback(const b2ShapeCastInput* input, int32_t proxyId, i B2_ASSERT(0 <= bodyIndex && bodyIndex < world->bodyPool.capacity); b2Body* body = world->bodies + bodyIndex; - B2_ASSERT(b2ObjectValid(&body->object)); + B2_ASSERT(b2IsValidObject(&body->object)); b2Transform transform = b2MakeTransform(body); - b2RayCastOutput output = b2ShapeCastShape(input, shape, transform); + b2CastOutput output = b2ShapeCastShape(input, shape, transform); if (output.hit) { @@ -1471,6 +1487,10 @@ void b2World_CircleCast(b2WorldId worldId, const b2Circle* circle, b2Transform o return; } + B2_ASSERT(b2Vec2_IsValid(originTransform.p)); + B2_ASSERT(b2Rot_IsValid(originTransform.q)); + B2_ASSERT(b2Vec2_IsValid(translation)); + b2ShapeCastInput input; input.points[0] = b2TransformPoint(originTransform, circle->point); input.count = 1; @@ -1503,6 +1523,10 @@ void b2World_CapsuleCast(b2WorldId worldId, const b2Capsule* capsule, b2Transfor return; } + B2_ASSERT(b2Vec2_IsValid(originTransform.p)); + B2_ASSERT(b2Rot_IsValid(originTransform.q)); + B2_ASSERT(b2Vec2_IsValid(translation)); + b2ShapeCastInput input; input.points[0] = b2TransformPoint(originTransform, capsule->point1); input.points[1] = b2TransformPoint(originTransform, capsule->point2); @@ -1536,6 +1560,10 @@ void b2World_PolygonCast(b2WorldId worldId, const b2Polygon* polygon, b2Transfor return; } + B2_ASSERT(b2Vec2_IsValid(originTransform.p)); + B2_ASSERT(b2Rot_IsValid(originTransform.q)); + B2_ASSERT(b2Vec2_IsValid(translation)); + b2ShapeCastInput input; for (int i = 0; i < polygon->count; ++i) { diff --git a/src/world.h b/src/world.h index c3b5f1f9..d4013ac9 100644 --- a/src/world.h +++ b/src/world.h @@ -5,8 +5,8 @@ #include "bitset.h" #include "broad_phase.h" +#include "constraint_graph.h" #include "island.h" -#include "graph.h" #include "pool.h" #include "box2d/callbacks.h" diff --git a/test/test_collision.c b/test/test_collision.c index 13be7c91..5cf13a02 100644 --- a/test/test_collision.c +++ b/test/test_collision.c @@ -24,7 +24,7 @@ static int AABBTest(void) b2Vec2 p1 = (b2Vec2){-2.0f, 0.0f}; b2Vec2 p2 = (b2Vec2){2.0f, 0.0f}; - b2RayCastOutput output = b2AABB_RayCast(a, p1, p2); + b2CastOutput output = b2AABB_RayCast(a, p1, p2); ENSURE(output.hit == true); ENSURE(0.1f < output.fraction && output.fraction < 0.9f); diff --git a/test/test_distance.c b/test/test_distance.c index def1200b..261c9df7 100644 --- a/test/test_distance.c +++ b/test/test_distance.c @@ -77,7 +77,7 @@ static int ShapeCastTest(void) input.translationB = (b2Vec2){-2.0f, 0.0f}; input.maxFraction = 1.0f; - b2RayCastOutput output = b2ShapeCast(&input); + b2CastOutput output = b2ShapeCast(&input); ENSURE(output.hit); ENSURE_SMALL(output.fraction - 0.5f, b2_linearSlop); diff --git a/test/test_shape.c b/test/test_shape.c index 24d3cc53..f779970f 100644 --- a/test/test_shape.c +++ b/test/test_shape.c @@ -130,7 +130,7 @@ static int RayCastShapeTest(void) b2RayCastInput input = {{-4.0f, 0.0f}, {8.0f, 0.0f}, 1.0f}; { - b2RayCastOutput output = b2RayCastCircle(&input, &circle); + b2CastOutput output = b2RayCastCircle(&input, &circle); ENSURE(output.hit); ENSURE_SMALL(output.normal.x + 1.0f, FLT_EPSILON); ENSURE_SMALL(output.normal.y, FLT_EPSILON); @@ -138,7 +138,7 @@ static int RayCastShapeTest(void) } { - b2RayCastOutput output = b2RayCastPolygon(&input, &box); + b2CastOutput output = b2RayCastPolygon(&input, &box); ENSURE(output.hit); ENSURE_SMALL(output.normal.x + 1.0f, FLT_EPSILON); ENSURE_SMALL(output.normal.y, FLT_EPSILON); @@ -146,7 +146,7 @@ static int RayCastShapeTest(void) } { - b2RayCastOutput output = b2RayCastSegment(&input, &segment, true); + b2CastOutput output = b2RayCastSegment(&input, &segment, true); ENSURE(output.hit); ENSURE_SMALL(output.normal.x + 1.0f, FLT_EPSILON); ENSURE_SMALL(output.normal.y, FLT_EPSILON);