From 519ded32531e1574caba32700481f4b5294dc383 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Fri, 22 Dec 2023 23:34:44 -0800 Subject: [PATCH] Migrate remaining joint types (#84) faster prismatic joint added wheel joint added motor joint decided to skip pulley and gear joints for now the friction joint can be implemented with the motor joint added several samples, include ragdoll sample --- .clang-format | 1 + include/box2d/box2d.h | 214 ++- include/box2d/event_types.h | 6 +- include/box2d/joint_types.h | 121 +- include/box2d/types.h | 1 + samples/CMakeLists.txt | 2 + samples/collection/benchmark.cpp | 256 ++-- samples/collection/human.cpp | 380 +++++ samples/collection/human.h | 42 + samples/collection/sample_events.cpp | 17 +- samples/collection/sample_joints.cpp | 1424 +++++++++++++----- samples/collection/sample_shapes.cpp | 185 ++- samples/draw.cpp | 2 +- src/CMakeLists.txt | 6 +- src/{stack_allocator.c => arena_allocator.c} | 2 +- src/{stack_allocator.h => arena_allocator.h} | 0 src/body.c | 4 +- src/broad_phase.c | 28 +- src/distance_joint.c | 6 +- src/geometry.c | 5 + src/graph.c | 45 +- src/island.c | 10 +- src/joint.c | 195 ++- src/joint.h | 121 +- src/motor_joint.c | 310 ++++ src/mouse_joint.c | 6 +- src/prismatic_joint.c | 211 ++- src/revolute_joint.c | 171 ++- src/shape.c | 2 +- src/weld_joint.c | 47 +- src/wheel_joint.c | 562 +++++++ src/world.c | 7 +- 32 files changed, 3534 insertions(+), 855 deletions(-) create mode 100644 samples/collection/human.cpp create mode 100644 samples/collection/human.h rename src/{stack_allocator.c => arena_allocator.c} (99%) rename src/{stack_allocator.h => arena_allocator.h} (100%) create mode 100644 src/motor_joint.c create mode 100644 src/wheel_joint.c diff --git a/.clang-format b/.clang-format index 12b4a08e..73b59dd4 100644 --- a/.clang-format +++ b/.clang-format @@ -1,5 +1,6 @@ # Reference: https://clang.llvm.org/docs/ClangFormatStyleOptions.html # /c/Program Files/Microsoft Visual Studio/2022/Community/VC/Tools/Llvm/bin +# https://clang-format-configurator.site/ # default values: # https://github.com/llvm/llvm-project/blob/main/clang/lib/Format/Format.cpp --- diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index f554cbbb..eca508fa 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -18,6 +18,10 @@ typedef struct b2Polygon b2Polygon; typedef struct b2DebugDraw b2DebugDraw; typedef struct b2Segment b2Segment; +/////////////////////////////////////////////////////////////////////////////// +/// World API +/////////////////////////////////////////////////////////////////////////////// + /// Create a world for rigid body simulation. This contains all the bodies, shapes, and constraints. BOX2D_API b2WorldId b2CreateWorld(const b2WorldDef* def); @@ -42,10 +46,87 @@ BOX2D_API b2BodyId b2World_CreateBody(b2WorldId worldId, const b2BodyDef* def); /// @warning This function is locked during callbacks. BOX2D_API void b2World_DestroyBody(b2BodyId bodyId); +/// Create a joint +BOX2D_API b2JointId b2World_CreateDistanceJoint(b2WorldId worldId, const b2DistanceJointDef* def); +BOX2D_API b2JointId b2World_CreateMotorJoint(b2WorldId worldId, const b2MotorJointDef* def); +BOX2D_API b2JointId b2World_CreateMouseJoint(b2WorldId worldId, const b2MouseJointDef* def); +BOX2D_API b2JointId b2World_CreatePrismaticJoint(b2WorldId worldId, const b2PrismaticJointDef* def); +BOX2D_API b2JointId b2World_CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDef* def); +BOX2D_API b2JointId b2World_CreateWeldJoint(b2WorldId worldId, const b2WeldJointDef* def); +BOX2D_API b2JointId b2World_CreateWheelJoint(b2WorldId worldId, const b2WheelJointDef* def); + +/// Destroy a joint +BOX2D_API void b2World_DestroyJoint(b2JointId jointId); + /// Get sensor events for the current time step. Do not store a reference to this data. BOX2D_API b2SensorEvents b2World_GetSensorEvents(b2WorldId worldId); BOX2D_API b2ContactEvents b2World_GetContactEvents(b2WorldId worldId); +/// Query the world for all shapes that potentially overlap the provided AABB. +BOX2D_API void b2World_QueryAABB(b2WorldId worldId, b2QueryResultFcn* fcn, b2AABB aabb, b2QueryFilter filter, void* context); + +/// Query the world for all shapes that overlap the provided circle. +BOX2D_API void b2World_OverlapCircle(b2WorldId worldId, b2QueryResultFcn* fcn, const b2Circle* circle, b2Transform transform, + b2QueryFilter filter, void* context); + +/// Query the world for all shapes that overlap the provided capsule. +BOX2D_API void b2World_OverlapCapsule(b2WorldId worldId, b2QueryResultFcn* fcn, const b2Capsule* capsule, b2Transform transform, + b2QueryFilter filter, void* context); + +/// Query the world for all shapes that overlap the provided polygon. +BOX2D_API void b2World_OverlapPolygon(b2WorldId worldId, b2QueryResultFcn* fcn, const b2Polygon* polygon, b2Transform transform, + b2QueryFilter filter, void* context); + +/// Ray-cast the world for all shapes in the path of the ray. Your callback +/// controls whether you get the closest point, any point, or n-points. +/// The ray-cast ignores shapes that contain the starting point. +/// @param callback a user implemented callback class. +/// @param point1 the ray starting point +/// @param point2 the ray ending point +BOX2D_API void b2World_RayCast(b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter, b2RayResultFcn* fcn, + void* context); + +// Ray-cast closest hit. Convenience function. This is less general than b2World_RayCast and does not allow for custom filtering. +BOX2D_API b2RayResult b2World_RayCastClosest(b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter); + +BOX2D_API void b2World_CircleCast(b2WorldId worldId, const b2Circle* circle, b2Transform originTransform, b2Vec2 translation, + b2QueryFilter filter, b2RayResultFcn* fcn, void* context); + +BOX2D_API void b2World_CapsuleCast(b2WorldId worldId, const b2Capsule* capsule, b2Transform originTransform, b2Vec2 translation, + b2QueryFilter filter, b2RayResultFcn* fcn, void* context); + +BOX2D_API void b2World_PolygonCast(b2WorldId worldId, const b2Polygon* polygon, b2Transform originTransform, b2Vec2 translation, + b2QueryFilter filter, b2RayResultFcn* fcn, void* context); + +/// Enable/disable sleep. Advanced feature for testing. +BOX2D_API void b2World_EnableSleeping(b2WorldId worldId, bool flag); + +/// Enable/disable constraint warm starting. Advanced feature for testing. +BOX2D_API void b2World_EnableWarmStarting(b2WorldId worldId, bool flag); + +/// Enable/disable continuous collision. Advanced feature for testing. +BOX2D_API void b2World_EnableContinuous(b2WorldId worldId, bool flag); + +/// Adjust the restitution threshold. Advanced feature for testing. +BOX2D_API void b2World_SetRestitutionThreshold(b2WorldId worldId, float value); + +/// 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) +/// - push velocity is the maximum contact constraint push out velocity (meters per second) +/// Advanced feature +BOX2D_API void b2World_SetContactTuning(b2WorldId worldId, float hertz, float dampingRatio, float pushVelocity); + +/// Get the current profile +BOX2D_API struct b2Profile b2World_GetProfile(b2WorldId worldId); + +/// Get counters and sizes +BOX2D_API struct b2Statistics b2World_GetStatistics(b2WorldId worldId); + +/////////////////////////////////////////////////////////////////////////////// +/// Body API +/////////////////////////////////////////////////////////////////////////////// + BOX2D_API b2BodyType b2Body_GetType(b2BodyId bodyId); BOX2D_API void b2Body_SetType(b2BodyId bodyId, b2BodyType type); @@ -137,8 +218,10 @@ BOX2D_API void b2Body_Disable(b2BodyId bodyId); /// Enable a body by adding it to the simulation BOX2D_API void b2Body_Enable(b2BodyId bodyId); -/// Create a shape and attach it to a body. Contacts are not created until the next time step. +/// Create a shape and attach it to a body. The shape defintion and geometry are fully cloned. +/// Contacts are not created until the next time step. /// @warning This function is locked during callbacks. +/// @return the shape id for accessing the shape BOX2D_API b2ShapeId b2Body_CreateCircle(b2BodyId bodyId, const b2ShapeDef* def, const b2Circle* circle); BOX2D_API b2ShapeId b2Body_CreateSegment(b2BodyId bodyId, const b2ShapeDef* def, const b2Segment* segment); BOX2D_API b2ShapeId b2Body_CreateCapsule(b2BodyId bodyId, const b2ShapeDef* def, const b2Capsule* capsule); @@ -152,6 +235,16 @@ BOX2D_API void b2Body_DestroyChain(b2ChainId chainId); BOX2D_API b2ShapeId b2Body_GetFirstShape(b2BodyId bodyId); BOX2D_API b2ShapeId b2Body_GetNextShape(b2ShapeId shapeId); +/// Get the maximum capacity required for retrieving all the touching contacts on a body +BOX2D_API int32_t b2Body_GetContactCapacity(b2BodyId bodyId); + +/// Get the touching contact data for a body +BOX2D_API int32_t b2Body_GetContactData(b2BodyId bodyId, b2ContactData* contactData, int32_t capacity); + +/////////////////////////////////////////////////////////////////////////////// +/// Shape API +/////////////////////////////////////////////////////////////////////////////// + BOX2D_API b2BodyId b2Shape_GetBody(b2ShapeId shapeId); BOX2D_API void* b2Shape_GetUserData(b2ShapeId shapeId); BOX2D_API bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point); @@ -168,30 +261,17 @@ BOX2D_API const b2Polygon* b2Shape_GetPolygon(b2ShapeId shapeId); BOX2D_API void b2Chain_SetFriction(b2ChainId chainId, float friction); BOX2D_API void b2Chain_SetRestitution(b2ChainId chainId, float restitution); -/// Contacts - -/// Get the number of touching contacts on a body -BOX2D_API int32_t b2Body_GetContactCount(b2BodyId bodyId); - -/// Get the touching contact data for a body -BOX2D_API int32_t b2Body_GetContactData(b2BodyId bodyId, b2ContactData* contactData, int32_t capacity); - -/// Get the number of touching contacts on a shape. For efficiency, this may be larger than the actual number. -BOX2D_API int32_t b2Shape_GetContactCount(b2ShapeId shapeId); +/// Get the maximum capacity required for retrieving all the touching contacts on a shape +BOX2D_API int32_t b2Shape_GetContactCapacity(b2ShapeId shapeId); /// Get the touching contact data for a shape. The provided shapeId will be either shapeIdA or shapeIdB on the contact data. BOX2D_API int32_t b2Shape_GetContactData(b2ShapeId shapeId, b2ContactData* contactData, int32_t capacity); -/// Create a joint -BOX2D_API b2JointId b2World_CreateDistanceJoint(b2WorldId worldId, const b2DistanceJointDef* def); -BOX2D_API b2JointId b2World_CreateMouseJoint(b2WorldId worldId, const b2MouseJointDef* def); -BOX2D_API b2JointId b2World_CreatePrismaticJoint(b2WorldId worldId, const b2PrismaticJointDef* def); -BOX2D_API b2JointId b2World_CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDef* def); -BOX2D_API b2JointId b2World_CreateWeldJoint(b2WorldId worldId, const b2WeldJointDef* def); - -/// Destroy a joint -BOX2D_API void b2World_DestroyJoint(b2JointId jointId); +/////////////////////////////////////////////////////////////////////////////// +/// Joint API +/////////////////////////////////////////////////////////////////////////////// +/// Generic joint access BOX2D_API b2BodyId b2Joint_GetBodyA(b2JointId jointId); BOX2D_API b2BodyId b2Joint_GetBodyB(b2JointId jointId); @@ -201,75 +281,43 @@ BOX2D_API void b2DistanceJoint_SetLength(b2JointId jointId, float length, float BOX2D_API float b2DistanceJoint_GetCurrentLength(b2JointId jointId); BOX2D_API void b2DistanceJoint_SetTuning(b2JointId jointId, float hertz, float dampingRatio); +/// Motor joint access +BOX2D_API void b2MotorJoint_SetLinearOffset(b2JointId jointId, b2Vec2 linearOffset); +BOX2D_API void b2MotorJoint_SetAngularOffset(b2JointId jointId, float angularOffset); +BOX2D_API void b2MotorJoint_SetMaxForce(b2JointId jointId, float maxForce); +BOX2D_API void b2MotorJoint_SetMaxTorque(b2JointId jointId, float maxTorque); +BOX2D_API void b2MotorJoint_SetCorrectionFactor(b2JointId jointId, float correctionFactor); +BOX2D_API b2Vec2 b2MotorJoint_GetConstraintForce(b2JointId jointId, float inverseTimeStep); +BOX2D_API float b2MotorJoint_GetConstraintTorque(b2JointId jointId, float inverseTimeStep); + /// Mouse joint access BOX2D_API void b2MouseJoint_SetTarget(b2JointId jointId, b2Vec2 target); +// Prismatic joint access +BOX2D_API void b2PrismaticJoint_EnableLimit(b2JointId jointId, bool enableLimit); +BOX2D_API void b2PrismaticJoint_EnableMotor(b2JointId jointId, bool enableMotor); +BOX2D_API void b2PrismaticJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed); +BOX2D_API float b2PrismaticJoint_GetMotorForce(b2JointId jointId, float inverseTimeStep); +BOX2D_API void b2PrismaticJoint_SetMaxMotorForce(b2JointId jointId, float force); +BOX2D_API b2Vec2 b2PrismaticJoint_GetConstraintForce(b2JointId jointId, float inverseTimeStep); +BOX2D_API float b2PrismaticJoint_GetConstraintTorque(b2JointId jointId, float inverseTimeStep); + // Revolute joint access BOX2D_API void b2RevoluteJoint_EnableLimit(b2JointId jointId, bool enableLimit); BOX2D_API void b2RevoluteJoint_EnableMotor(b2JointId jointId, bool enableMotor); BOX2D_API void b2RevoluteJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed); BOX2D_API float b2RevoluteJoint_GetMotorTorque(b2JointId jointId, float inverseTimeStep); BOX2D_API void b2RevoluteJoint_SetMaxMotorTorque(b2JointId jointId, float torque); -BOX2D_API b2Vec2 b2RevoluteJoint_GetConstraintForce(b2JointId jointId); - -/// Query the world for all shapes that potentially overlap the provided AABB. -BOX2D_API void b2World_QueryAABB(b2WorldId worldId, b2QueryResultFcn* fcn, b2AABB aabb, b2QueryFilter filter, void* context); - -/// Query the world for all shapes that overlap the provided circle. -BOX2D_API void b2World_OverlapCircle(b2WorldId worldId, b2QueryResultFcn* fcn, const b2Circle* circle, b2Transform transform, - b2QueryFilter filter, void* context); - -/// Query the world for all shapes that overlap the provided capsule. -BOX2D_API void b2World_OverlapCapsule(b2WorldId worldId, b2QueryResultFcn* fcn, const b2Capsule* capsule, b2Transform transform, - b2QueryFilter filter, void* context); - -/// Query the world for all shapes that overlap the provided polygon. -BOX2D_API void b2World_OverlapPolygon(b2WorldId worldId, b2QueryResultFcn* fcn, const b2Polygon* polygon, b2Transform transform, - b2QueryFilter filter, void* context); - -/// Ray-cast the world for all shapes in the path of the ray. Your callback -/// controls whether you get the closest point, any point, or n-points. -/// The ray-cast ignores shapes that contain the starting point. -/// @param callback a user implemented callback class. -/// @param point1 the ray starting point -/// @param point2 the ray ending point -BOX2D_API void b2World_RayCast(b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter, - b2RayResultFcn* fcn, void* context); - -// Ray-cast closest hit. Convenience function. This is less general than b2World_RayCast and does not allow for custom filtering. -BOX2D_API b2RayResult b2World_RayCastClosest(b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter); - -BOX2D_API void b2World_CircleCast(b2WorldId worldId, const b2Circle* circle, b2Transform originTransform, b2Vec2 translation, - b2QueryFilter filter, b2RayResultFcn* fcn, void* context); - -BOX2D_API void b2World_CapsuleCast(b2WorldId worldId, const b2Capsule* capsule, b2Transform originTransform, b2Vec2 translation, - b2QueryFilter filter, b2RayResultFcn* fcn, void* context); - -BOX2D_API void b2World_PolygonCast(b2WorldId worldId, const b2Polygon* polygon, b2Transform originTransform, b2Vec2 translation, - b2QueryFilter filter, b2RayResultFcn* fcn, void* context); - -/// Advanced API for testing and special cases - -/// Enable/disable sleep. -BOX2D_API void b2World_EnableSleeping(b2WorldId worldId, bool flag); - -/// Enable/disable contact warm starting. Improves stacking stability. -BOX2D_API void b2World_EnableWarmStarting(b2WorldId worldId, bool flag); - -/// Enable/disable continuous collision. -BOX2D_API void b2World_EnableContinuous(b2WorldId worldId, bool flag); - -/// Adjust the restitution threshold -BOX2D_API void b2World_SetRestitutionThreshold(b2WorldId worldId, float value); - -/// 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) -/// - push velocity is the maximum contact constraint push out velocity (meters per second) -BOX2D_API void b2World_SetContactTuning(b2WorldId worldId, float hertz, float dampingRatio, float pushVelocity); - -/// Get the current profile -BOX2D_API struct b2Profile b2World_GetProfile(b2WorldId worldId); - -/// Get counters and sizes -BOX2D_API struct b2Statistics b2World_GetStatistics(b2WorldId worldId); +BOX2D_API b2Vec2 b2RevoluteJoint_GetConstraintForce(b2JointId jointId, float inverseTimeStep); +BOX2D_API float b2RevoluteJoint_GetConstraintTorque(b2JointId jointId, float inverseTimeStep); + +// Wheel joint access +BOX2D_API void b2WheelJoint_SetStiffness(b2JointId jointId, float stiffness); +BOX2D_API void b2WheelJoint_SetDamping(b2JointId jointId, float damping); +BOX2D_API void b2WheelJoint_EnableLimit(b2JointId jointId, bool enableLimit); +BOX2D_API void b2WheelJoint_EnableMotor(b2JointId jointId, bool enableMotor); +BOX2D_API void b2WheelJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed); +BOX2D_API float b2WheelJoint_GetMotorTorque(b2JointId jointId, float inverseTimeStep); +BOX2D_API void b2WheelJoint_SetMaxMotorTorque(b2JointId jointId, float torque); +BOX2D_API b2Vec2 b2WheelJoint_GetConstraintForce(b2JointId jointId, float inverseTimeStep); +BOX2D_API float b2WheelJoint_GetConstraintTorque(b2JointId jointId, float inverseTimeStep); diff --git a/include/box2d/event_types.h b/include/box2d/event_types.h index 186195a4..acdf4684 100644 --- a/include/box2d/event_types.h +++ b/include/box2d/event_types.h @@ -35,7 +35,8 @@ typedef struct b2SensorEvents int endCount; } b2SensorEvents; -/// A begin touch event is generated when two shapes begin touching. +/// A begin touch event is generated when two shapes begin touching. By convention the manifold +/// normal points from shape A to shape B. typedef struct b2ContactBeginTouchEvent { b2ShapeId shapeIdA; @@ -61,7 +62,8 @@ typedef struct b2ContactEvents int endCount; } b2ContactEvents; -/// This is the data you can access using a b2ContactId. +/// The contact data for two shapes. By convention the manifold normal points +/// from shape A to shape B. typedef struct b2ContactData { b2ShapeId shapeIdA; diff --git a/include/box2d/joint_types.h b/include/box2d/joint_types.h index 9b6e5899..07fc7510 100644 --- a/include/box2d/joint_types.h +++ b/include/box2d/joint_types.h @@ -59,13 +59,50 @@ static inline b2DistanceJointDef b2DefaultDistanceJointDef(void) return def; } +/// A motor joint is used to control the relative motion +/// between two bodies. A typical usage is to control the movement +/// of a dynamic body with respect to the ground. +typedef struct b2MotorJointDef +{ + /// The first attached body. + b2BodyId bodyIdA; + + /// The second attached body. + b2BodyId bodyIdB; + + /// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters. + b2Vec2 linearOffset; + + /// The bodyB angle minus bodyA angle in radians. + float angularOffset; + + /// The maximum motor force in N. + float maxForce; + + /// The maximum motor torque in N-m. + float maxTorque; + + /// Position correction factor in the range [0,1]. + float correctionFactor; +} b2MotorJointDef; + +static inline b2MotorJointDef b2DefaultMotorJointDef(void) +{ + b2MotorJointDef def = B2_ZERO_INIT; + def.bodyIdA = b2_nullBodyId; + def.bodyIdB = b2_nullBodyId; + def.linearOffset = B2_LITERAL(b2Vec2){0.0f, 0.0f}; + def.angularOffset = 0.0f; + def.maxForce = 1.0f; + def.maxTorque = 1.0f; + def.correctionFactor = 0.3f; + return def; +} + /// A mouse joint is used to make a point on a body track a /// specified world point. This a soft constraint with a maximum /// force. This allows the constraint to stretch without /// applying huge forces. -/// NOTE: this joint is not documented in the manual because it was -/// developed to be used in samples. If you want to learn how to -/// use the mouse joint, look at the samples app. typedef struct b2MouseJointDef { /// The first attached body. @@ -138,7 +175,7 @@ typedef struct b2PrismaticJointDef /// Enable/disable the joint motor. bool enableMotor; - /// The maximum motor torque, usually in N-m. + /// The maximum motor force, usually in N. float maxMotorForce; /// The desired motor speed in radians per second. @@ -215,6 +252,9 @@ typedef struct b2RevoluteJointDef /// Usually in N-m. float maxMotorTorque; + /// Scale the debug draw + float drawSize; + /// Set this flag to true if the attached bodies should collide. bool collideConnected; } b2RevoluteJointDef; @@ -234,6 +274,7 @@ static inline b2RevoluteJointDef b2DefaultRevoluteJointDef(void) def.motorSpeed = 0.0f; def.enableLimit = false; def.enableMotor = false; + def.drawSize = 0.25f; def.collideConnected = false; return def; } @@ -288,3 +329,75 @@ static inline b2WeldJointDef b2DefaultWeldJointDef(void) def.collideConnected = false; return def; } + +/// Wheel joint definition. This requires defining a line of +/// motion using an axis and an anchor point. The definition uses local +/// anchor points and a local axis so that the initial configuration +/// can violate the constraint slightly. The joint translation is zero +/// when the local anchor points coincide in world space. Using local +/// anchors and a local axis helps when saving and loading a game. +typedef struct b2WheelJointDef +{ + /// The first attached body. + b2BodyId bodyIdA; + + /// The second attached body. + b2BodyId bodyIdB; + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The local translation unit axis in bodyA. + b2Vec2 localAxisA; + + /// Enable/disable the joint limit. + bool enableLimit; + + /// The lower translation limit, usually in meters. + float lowerTranslation; + + /// The upper translation limit, usually in meters. + float upperTranslation; + + /// Enable/disable the joint motor. + bool enableMotor; + + /// The maximum motor torque, usually in N-m. + float maxMotorTorque; + + /// The desired motor speed in radians per second. + float motorSpeed; + + /// The linear stiffness in N/m + float stiffness; + + /// The linear damping in N*s/m + float damping; + + /// Set this flag to true if the attached bodies should collide. + bool collideConnected; +} b2WheelJointDef; + +/// Use this to initialize your joint definition +static inline b2WheelJointDef b2DefaultWheelJointDef(void) +{ + b2WheelJointDef def = B2_ZERO_INIT; + def.bodyIdA = b2_nullBodyId; + def.bodyIdB = b2_nullBodyId; + def.localAnchorA = B2_LITERAL(b2Vec2){0.0f, 0.0f}; + def.localAnchorB = B2_LITERAL(b2Vec2){0.0f, 0.0f}; + def.localAxisA = B2_LITERAL(b2Vec2){1.0f, 0.0f}; + def.enableLimit = false; + def.lowerTranslation = 0.0f; + def.upperTranslation = 0.0f; + def.enableMotor = false; + def.maxMotorTorque = 0.0f; + def.motorSpeed = 0.0f; + def.stiffness = 0.0f; + def.damping = 0.0f; + def.collideConnected = false; + return def; +} diff --git a/include/box2d/types.h b/include/box2d/types.h index 66242180..564d56d5 100644 --- a/include/box2d/types.h +++ b/include/box2d/types.h @@ -11,6 +11,7 @@ #include #include +// These macros handle some differences between C and C++ #ifdef __cplusplus #define B2_LITERAL(T) T #define B2_ZERO_INIT \ diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 0f38612f..2b97a169 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -65,6 +65,8 @@ set(BOX2D_SAMPLES settings.cpp collection/benchmark.cpp + collection/human.cpp + collection/human.h collection/sample_joints.cpp collection/sample_robustness.cpp collection/sample_bodies.cpp diff --git a/samples/collection/benchmark.cpp b/samples/collection/benchmark.cpp index 07f10394..18ac0896 100644 --- a/samples/collection/benchmark.cpp +++ b/samples/collection/benchmark.cpp @@ -1,11 +1,13 @@ // SPDX-FileCopyrightText: 2022 Erin Catto // SPDX-License-Identifier: MIT +#include "human.h" #include "sample.h" #include "settings.h" #include "box2d/box2d.h" #include "box2d/geometry.h" +#include "box2d/hull.h" #include #include @@ -19,6 +21,8 @@ class BenchmarkBarrel : public Sample e_circleShape = 0, e_capsuleShape = 1, e_boxShape = 2, + e_compoundShape = 3, + e_humanShape = 4, }; enum @@ -33,27 +37,21 @@ class BenchmarkBarrel : public Sample float groundSize = 25.0f; { - b2BodyDef bd = b2DefaultBodyDef(); - b2BodyId groundId = b2World_CreateBody(m_worldId, &bd); + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2World_CreateBody(m_worldId, &bodyDef); b2Polygon box = b2MakeBox(groundSize, 1.2f); - b2ShapeDef sd = b2DefaultShapeDef(); - b2Body_CreatePolygon(groundId, &sd, &box); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Body_CreatePolygon(groundId, &shapeDef, &box); box = b2MakeOffsetBox(1.2f, 2.0f * groundSize, {-groundSize, 2.0f * groundSize}, 0.0f); - b2Body_CreatePolygon(groundId, &sd, &box); + b2Body_CreatePolygon(groundId, &shapeDef, &box); box = b2MakeOffsetBox(1.2f, 2.0f * groundSize, {groundSize, 2.0f * groundSize}, 0.0f); - b2Body_CreatePolygon(groundId, &sd, &box); + b2Body_CreatePolygon(groundId, &shapeDef, &box); - b2Segment segment = {{-400.0f, -60.0f}, {400.0f, -60.0f}}; - b2Body_CreateSegment(groundId, &sd, &segment); - - segment = {{-800.0f, 100.0f}, {-400.0f, -60.0f}}; - b2Body_CreateSegment(groundId, &sd, &segment); - - segment = {{400.0f, -60.0f}, {800.0f, 100.0f}}; - b2Body_CreateSegment(groundId, &sd, &segment); + box = b2MakeOffsetBox(800.0f, 10.0f, {0.0f, -80.0f}, 0.0f); + b2Body_CreatePolygon(groundId, &shapeDef, &box); } for (int32_t i = 0; i < e_maxRows * e_maxColumns; ++i) @@ -61,7 +59,7 @@ class BenchmarkBarrel : public Sample m_bodies[i] = b2_nullBodyId; } - m_shapeType = e_circleShape; + m_shapeType = e_compoundShape; CreateScene(); } @@ -75,33 +73,90 @@ class BenchmarkBarrel : public Sample b2World_DestroyBody(m_bodies[i]); m_bodies[i] = b2_nullBodyId; } + + m_humans[i].Despawn(); } m_columnCount = g_sampleDebug ? 10 : e_maxColumns; + m_rowCount = g_sampleDebug ? 40 : e_maxRows; + + if (m_shapeType == e_compoundShape) + { + if (g_sampleDebug == false) + { + m_columnCount = 20; + } + } + else if (m_shapeType == e_humanShape) + { + if (g_sampleDebug) + { + m_rowCount = 10; + m_columnCount = 10; + } + else + { + m_columnCount = 15; + m_rowCount = 50; + } + } float rad = 0.5f; - float shift = rad * 2.0f; + float shift = 1.0f; float centerx = shift * m_columnCount / 2.0f; float centery = shift / 2.0f; - b2BodyDef bd = b2DefaultBodyDef(); - bd.type = b2_dynamicBody; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; - b2ShapeDef sd = b2DefaultShapeDef(); - sd.density = 1.0f; - sd.friction = 0.5f; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = 0.5f; - b2Polygon cuboid = b2MakeBox(0.5f, 0.5f); + b2Polygon box = b2MakeBox(0.5f, 0.5f); b2Capsule capsule = {{0.0f, -0.25f}, {0.0f, 0.25f}, rad}; b2Circle circle = {{0.0f, 0.0f}, rad}; - float extray = m_shapeType == e_capsuleShape ? rad : 0.0f; + b2Vec2 vertices[3]; + vertices[0] = {-1.0f, 0.0f}; + vertices[1] = {0.5f, 1.0f}; + vertices[2] = {0.0f, 2.0f}; + b2Hull hull = b2ComputeHull(vertices, 3); + b2Polygon left = b2MakePolygon(&hull, 0.0f); - m_rowCount = g_sampleDebug ? 40 : e_maxRows; + vertices[0] = {1.0f, 0.0f}; + vertices[1] = {-0.5f, 1.0f}; + vertices[2] = {0.0f, 2.0f}; + hull = b2ComputeHull(vertices, 3); + b2Polygon right = b2MakePolygon(&hull, 0.0f); + + //b2Polygon top = b2MakeOffsetBox(0.8f, 0.2f, {0.0f, 0.8f}, 0.0f); + //b2Polygon leftLeg = b2MakeOffsetBox(0.2f, 0.5f, {-0.6f, 0.5f}, 0.0f); + //b2Polygon rightLeg = b2MakeOffsetBox(0.2f, 0.5f, {0.6f, 0.5f}, 0.0f); - int32_t index = 0; float side = -0.05f; + float extray = 0.0f; + if (m_shapeType == e_capsuleShape) + { + extray = 0.5f; + } + else if (m_shapeType == e_compoundShape) + { + extray = 0.25f; + side = 0.25f; + shift = 2.0f; + centerx = shift * m_columnCount / 2.0f - 1.0f; + } + else if (m_shapeType == e_humanShape) + { + extray = 0.5f; + side = 0.55f; + shift = 2.5f; + centerx = shift * m_columnCount / 2.0f; + } + + int32_t index = 0; for (int32_t i = 0; i < m_columnCount; ++i) { @@ -111,22 +166,34 @@ class BenchmarkBarrel : public Sample { float y = j * (shift + extray) + centery + 2.0f; - bd.position = {x + side, y}; + bodyDef.position = {x + side, y}; side = -side; - m_bodies[index] = b2World_CreateBody(m_worldId, &bd); + m_bodies[index] = b2World_CreateBody(m_worldId, &bodyDef); if (m_shapeType == e_circleShape) { - b2Body_CreateCircle(m_bodies[index], &sd, &circle); + b2Body_CreateCircle(m_bodies[index], &shapeDef, &circle); } else if (m_shapeType == e_capsuleShape) { - b2Body_CreateCapsule(m_bodies[index], &sd, &capsule); + b2Body_CreateCapsule(m_bodies[index], &shapeDef, &capsule); } - else + else if (m_shapeType == e_boxShape) + { + b2Body_CreatePolygon(m_bodies[index], &shapeDef, &box); + } + else if (m_shapeType == e_compoundShape) { - b2Body_CreatePolygon(m_bodies[index], &sd, &cuboid); + b2Body_CreatePolygon(m_bodies[index], &shapeDef, &left); + b2Body_CreatePolygon(m_bodies[index], &shapeDef, &right); + //b2Body_CreatePolygon(m_bodies[index], &shapeDef, &top); + //b2Body_CreatePolygon(m_bodies[index], &shapeDef, &leftLeg); + //b2Body_CreatePolygon(m_bodies[index], &shapeDef, &rightLeg); + } + else if (m_shapeType == e_humanShape) + { + m_humans[index].Spawn(m_worldId, bodyDef.position, 3.5f, index + 1); } index += 1; @@ -141,7 +208,7 @@ class BenchmarkBarrel : public Sample ImGui::Begin("Stacks", nullptr, ImGuiWindowFlags_NoResize); bool changed = false; - const char* shapeTypes[] = {"Circle", "Capsule", "Box"}; + const char* shapeTypes[] = {"Circle", "Capsule", "Box", "Compound", "Human"}; int shapeType = int(m_shapeType); changed = changed || ImGui::Combo("Shape", &shapeType, shapeTypes, IM_ARRAYSIZE(shapeTypes)); @@ -163,6 +230,7 @@ class BenchmarkBarrel : public Sample } b2BodyId m_bodies[e_maxRows * e_maxColumns]; + Human m_humans[e_maxRows * e_maxColumns]; int32_t m_columnCount; int32_t m_rowCount; @@ -179,29 +247,29 @@ class BenchmarkTumbler : public Sample { b2BodyId groundId; { - b2BodyDef bd = b2DefaultBodyDef(); - groundId = b2World_CreateBody(m_worldId, &bd); + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2World_CreateBody(m_worldId, &bodyDef); } { - b2BodyDef bd = b2DefaultBodyDef(); - bd.type = b2_dynamicBody; - bd.enableSleep = false; - bd.position = {0.0f, 10.0f}; - b2BodyId bodyId = b2World_CreateBody(m_worldId, &bd); + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.enableSleep = false; + bodyDef.position = {0.0f, 10.0f}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); - b2ShapeDef sd = b2DefaultShapeDef(); - sd.density = 50.0f; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 50.0f; b2Polygon polygon; polygon = b2MakeOffsetBox(0.5f, 10.0f, {10.0f, 0.0f}, 0.0); - b2Body_CreatePolygon(bodyId, &sd, &polygon); + b2Body_CreatePolygon(bodyId, &shapeDef, &polygon); polygon = b2MakeOffsetBox(0.5f, 10.0f, {-10.0f, 0.0f}, 0.0); - b2Body_CreatePolygon(bodyId, &sd, &polygon); + b2Body_CreatePolygon(bodyId, &shapeDef, &polygon); polygon = b2MakeOffsetBox(10.0f, 0.5f, {0.0f, 10.0f}, 0.0); - b2Body_CreatePolygon(bodyId, &sd, &polygon); + b2Body_CreatePolygon(bodyId, &shapeDef, &polygon); polygon = b2MakeOffsetBox(10.0f, 0.5f, {0.0f, -10.0f}, 0.0); - b2Body_CreatePolygon(bodyId, &sd, &polygon); + b2Body_CreatePolygon(bodyId, &shapeDef, &polygon); // m_motorSpeed = 9.0f; m_motorSpeed = 25.0f; @@ -244,16 +312,16 @@ class BenchmarkTumbler : public Sample float a = 0.125f; for (int32_t i = 0; i < 5 && m_count < m_maxCount; ++i) { - b2BodyDef bd = b2DefaultBodyDef(); - bd.type = b2_dynamicBody; - bd.position = {5.0f * a + 2.0f * a * i, 10.0f + 2.0f * a * (m_stepCount % 5)}; - b2BodyId bodyId = b2World_CreateBody(m_worldId, &bd); + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = {5.0f * a + 2.0f * a * i, 10.0f + 2.0f * a * (m_stepCount % 5)}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); - b2ShapeDef sd = b2DefaultShapeDef(); - sd.density = 1.0f; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; b2Polygon polygon = b2MakeBox(0.125f, 0.125f); - b2Body_CreatePolygon(bodyId, &sd, &polygon); + b2Body_CreatePolygon(bodyId, &shapeDef, &polygon); ++m_count; } } @@ -281,8 +349,8 @@ class BenchmarkManyTumblers : public Sample BenchmarkManyTumblers(const Settings& settings) : Sample(settings) { - b2BodyDef bd = b2DefaultBodyDef(); - m_groundId = b2World_CreateBody(m_worldId, &bd); + b2BodyDef bodyDef = b2DefaultBodyDef(); + m_groundId = b2World_CreateBody(m_worldId, &bodyDef); m_rowCount = g_sampleDebug ? 2 : 19; m_columnCount = g_sampleDebug ? 2 : 19; @@ -312,24 +380,24 @@ class BenchmarkManyTumblers : public Sample void CreateTumbler(b2Vec2 position, int index) { - b2BodyDef bd = b2DefaultBodyDef(); - bd.type = b2_dynamicBody; - bd.position = {position.x, position.y}; - b2BodyId bodyId = b2World_CreateBody(m_worldId, &bd); + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = {position.x, position.y}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); m_tumblerIds[index] = bodyId; - b2ShapeDef sd = b2DefaultShapeDef(); - sd.density = 50.0f; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 50.0f; b2Polygon polygon; polygon = b2MakeOffsetBox(0.25f, 2.0f, {2.0f, 0.0f}, 0.0); - b2Body_CreatePolygon(bodyId, &sd, &polygon); + b2Body_CreatePolygon(bodyId, &shapeDef, &polygon); polygon = b2MakeOffsetBox(0.25f, 2.0f, {-2.0f, 0.0f}, 0.0); - b2Body_CreatePolygon(bodyId, &sd, &polygon); + b2Body_CreatePolygon(bodyId, &shapeDef, &polygon); polygon = b2MakeOffsetBox(2.0f, 0.25f, {0.0f, 2.0f}, 0.0); - b2Body_CreatePolygon(bodyId, &sd, &polygon); + b2Body_CreatePolygon(bodyId, &shapeDef, &polygon); polygon = b2MakeOffsetBox(2.0f, 0.25f, {0.0f, -2.0f}, 0.0); - b2Body_CreatePolygon(bodyId, &sd, &polygon); + b2Body_CreatePolygon(bodyId, &shapeDef, &polygon); b2RevoluteJointDef jd = b2DefaultRevoluteJointDef(); jd.bodyIdA = m_groundId; @@ -432,9 +500,9 @@ class BenchmarkManyTumblers : public Sample if (m_bodyIndex < m_bodyCount && (m_stepCount & 0x7) == 0) { - b2ShapeDef sd = b2DefaultShapeDef(); - sd.density = 1.0f; - // sd.restitution = 0.5f; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + // shapeDef.restitution = 0.5f; b2Circle circle = {{0.0f, 0.0f}, 0.125f}; b2Polygon polygon = b2MakeBox(0.125f, 0.125f); @@ -445,22 +513,22 @@ class BenchmarkManyTumblers : public Sample { assert(m_bodyIndex < m_bodyCount); - b2BodyDef bd = b2DefaultBodyDef(); - bd.type = b2_dynamicBody; - bd.position = m_positions[i]; - m_bodyIds[m_bodyIndex] = b2World_CreateBody(m_worldId, &bd); + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = m_positions[i]; + m_bodyIds[m_bodyIndex] = b2World_CreateBody(m_worldId, &bodyDef); // if (j == 0) //{ - // b2Body_CreatePolygon(m_bodyIds[m_bodyIndex], &sd, &polygon); + // b2Body_CreatePolygon(m_bodyIds[m_bodyIndex], &shapeDef, &polygon); // } // else if (j == 1) { - b2Body_CreateCapsule(m_bodyIds[m_bodyIndex], &sd, &capsule); + b2Body_CreateCapsule(m_bodyIds[m_bodyIndex], &shapeDef, &capsule); } // else //{ - // b2Body_CreateCircle(m_bodyIds[m_bodyIndex], &sd, &circle); + // b2Body_CreateCircle(m_bodyIds[m_bodyIndex], &shapeDef, &circle); // } m_bodyIndex += 1; @@ -537,7 +605,7 @@ class BenchmarkPyramid : public Sample shapeDef.density = 1.0f; float h = m_extent - m_round; - b2Polygon cuboid = b2MakeRoundedBox(h, h, m_round); + b2Polygon box = b2MakeRoundedBox(h, h, m_round); float shift = 1.0f * h; @@ -553,7 +621,7 @@ class BenchmarkPyramid : public Sample assert(m_bodyIndex < m_bodyCount); m_bodyIds[m_bodyIndex] = b2World_CreateBody(m_worldId, &bodyDef); - b2Body_CreatePolygon(m_bodyIds[m_bodyIndex], &shapeDef, &cuboid); + b2Body_CreatePolygon(m_bodyIds[m_bodyIndex], &shapeDef, &box); m_bodyIndex += 1; } @@ -718,12 +786,12 @@ class BenchmarkCreateDestroy : public Sample { float groundSize = 100.0f; - b2BodyDef bd = b2DefaultBodyDef(); - b2BodyId groundId = b2World_CreateBody(m_worldId, &bd); + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2World_CreateBody(m_worldId, &bodyDef); b2Polygon box = b2MakeBox(groundSize, 1.0f); - b2ShapeDef sd = b2DefaultShapeDef(); - b2Body_CreatePolygon(groundId, &sd, &box); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Body_CreatePolygon(groundId, &shapeDef, &box); for (int32_t i = 0; i < e_maxBodyCount; ++i) { @@ -752,15 +820,15 @@ class BenchmarkCreateDestroy : public Sample float centerx = shift * count / 2.0f; float centery = shift / 2.0f + 1.0f; - b2BodyDef bd = b2DefaultBodyDef(); - bd.type = b2_dynamicBody; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; - b2ShapeDef sd = b2DefaultShapeDef(); - sd.density = 1.0f; - sd.friction = 0.5f; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = 0.5f; float h = 0.5f; - b2Polygon cuboid = b2MakeRoundedBox(h, h, 0.0f); + b2Polygon box = b2MakeRoundedBox(h, h, 0.0f); int32_t index = 0; @@ -771,11 +839,11 @@ class BenchmarkCreateDestroy : public Sample for (int32_t j = i; j < count; ++j) { float x = 0.5f * i * shift + (j - i) * shift - centerx; - bd.position = {x, y}; + bodyDef.position = {x, y}; assert(index < e_maxBodyCount); - m_bodies[index] = b2World_CreateBody(m_worldId, &bd); - b2Body_CreatePolygon(m_bodies[index], &sd, &cuboid); + m_bodies[index] = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreatePolygon(m_bodies[index], &shapeDef, &box); index += 1; } @@ -843,19 +911,19 @@ class BenchmarkJointGrid : public Sample float fk = (float)k; float fi = (float)i; - b2BodyDef bd = b2DefaultBodyDef(); + b2BodyDef bodyDef = b2DefaultBodyDef(); if (k >= numk / 2 - 3 && k <= numk / 2 + 3 && i == 0) { - bd.type = b2_staticBody; + bodyDef.type = b2_staticBody; } else { - bd.type = b2_dynamicBody; + bodyDef.type = b2_dynamicBody; } - bd.position = {fk * shift, -fi * shift}; + bodyDef.position = {fk * shift, -fi * shift}; - b2BodyId body = b2World_CreateBody(m_worldId, &bd); + b2BodyId body = b2World_CreateBody(m_worldId, &bodyDef); b2Body_CreateCircle(body, &shapeDef, &circle); diff --git a/samples/collection/human.cpp b/samples/collection/human.cpp new file mode 100644 index 00000000..7a12671a --- /dev/null +++ b/samples/collection/human.cpp @@ -0,0 +1,380 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "human.h" + +#include "box2d/box2d.h" +#include "box2d/math.h" + +#include + +Human::Human() +{ + for (int i = 0; i < Bone::e_count; ++i) + { + m_bones[i].bodyId = b2_nullBodyId; + m_bones[i].jointId = b2_nullJointId; + m_bones[i].parentIndex = -1; + } + + m_isSpawned = false; +} + +void Human::Spawn(b2WorldId worldId, b2Vec2 position, float scale, int groupIndex) +{ + m_worldId = worldId; + + for (int i = 0; i < Bone::e_count; ++i) + { + assert(B2_IS_NULL(m_bones[i].bodyId)); + assert(B2_IS_NULL(m_bones[i].jointId)); + } + + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + + b2ShapeDef shapeDef = b2_defaultShapeDef; + shapeDef.friction = 0.2f; + shapeDef.filter.groupIndex = -groupIndex; + + b2ShapeDef footShapeDef = shapeDef; + footShapeDef.friction = 0.05f; + + float s = scale; + float maxTorque = 0.05f * s; + bool enableMotor = true; + float drawSize = 0.05f; + + // hip + { + Bone* bone = m_bones + Bone::e_hip; + bone->parentIndex = -1; + + bodyDef.position = b2Add({0.0f, 0.95f * s}, position); + bone->bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Capsule capsule = {{0.0f, -0.02f * s}, {0.0f, 0.02f * s}, 0.095f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + } + + // torso + { + Bone* bone = m_bones + Bone::e_torso; + bone->parentIndex = Bone::e_hip; + + bodyDef.position = b2Add({0.0f, 1.2f * s}, position); + //bodyDef.type = b2_staticBody; + bone->bodyId = b2World_CreateBody(m_worldId, &bodyDef); + bodyDef.type = b2_dynamicBody; + + b2Capsule capsule = {{0.0f, -0.135f * s}, {0.0f, 0.135f * s}, 0.09f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + + b2Vec2 pivot = b2Add({0.0f, 1.0f * s}, position); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.enableLimit = true; + jointDef.lowerAngle = -0.25f * b2_pi; + jointDef.upperAngle = 0.0f; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = 0.5f * maxTorque; + jointDef.drawSize = drawSize; + + bone->jointId = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + // head + { + Bone* bone = m_bones + Bone::e_head; + bone->parentIndex = Bone::e_torso; + + bodyDef.position = b2Add({0.0f * s, 1.5f * s}, position); + bone->bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Capsule capsule = {{0.0f, -0.0325f * s}, {0.0f, 0.0325f * s}, 0.08f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + + capsule = {{0.0f, -0.12f * s}, {0.0f, -0.08f * s}, 0.05f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + + b2Vec2 pivot = b2Add({0.0f, 1.4f * s}, position); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.enableLimit = true; + jointDef.lowerAngle = -0.3f * b2_pi; + jointDef.upperAngle = 0.1f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = 0.25f * maxTorque; + jointDef.drawSize = drawSize; + + bone->jointId = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + // upper left leg + { + Bone* bone = m_bones + Bone::e_upperLeftLeg; + bone->parentIndex = Bone::e_hip; + + bodyDef.position = b2Add({0.0f, 0.775f * s}, position); + bone->bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Capsule capsule = {{0.0f, -0.125f * s}, {0.0f, 0.125f * s}, 0.06f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + + b2Vec2 pivot = b2Add({0.0f, 0.9f * s}, position); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.enableLimit = true; + jointDef.lowerAngle = -0.05f * b2_pi; + jointDef.upperAngle = 0.4f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = maxTorque; + jointDef.drawSize = drawSize; + + bone->jointId = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + // lower left leg + { + Bone* bone = m_bones + Bone::e_lowerLeftLeg; + bone->parentIndex = Bone::e_upperLeftLeg; + + bodyDef.position = b2Add({0.0f, 0.475f * s}, position); + bone->bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Capsule capsule = {{0.0f, -0.14f * s}, {0.0f, 0.125f * s}, 0.05f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + + //b2Polygon box = b2MakeOffsetBox(0.1f * s, 0.03f * s, {0.05f * s, -0.175f * s}, 0.0f); + //b2Body_CreatePolygon(bone->bodyId, &shapeDef, &box); + + capsule = {{-0.02f * s, -0.175f * s}, {0.13f * s, -0.175f * s}, 0.03f * s}; + b2Body_CreateCapsule(bone->bodyId, &footShapeDef, &capsule); + + b2Vec2 pivot = b2Add({0.0f, 0.625f * s}, position); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.enableLimit = true; + jointDef.lowerAngle = -0.5f * b2_pi; + jointDef.upperAngle = -0.02f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = 0.5f * maxTorque; + jointDef.drawSize = drawSize; + + bone->jointId = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + // upper right leg + { + Bone* bone = m_bones + Bone::e_upperRightLeg; + bone->parentIndex = Bone::e_hip; + + bodyDef.position = b2Add({0.0f, 0.775f * s}, position); + bone->bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Capsule capsule = {{0.0f, -0.125f * s}, {0.0f, 0.125f * s}, 0.06f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + + b2Vec2 pivot = b2Add({0.0f, 0.9f * s}, position); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.enableLimit = true; + jointDef.lowerAngle = -0.05f * b2_pi; + jointDef.upperAngle = 0.4f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = maxTorque; + jointDef.drawSize = drawSize; + + bone->jointId = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + // lower right leg + { + Bone* bone = m_bones + Bone::e_lowerRightLeg; + bone->parentIndex = Bone::e_upperRightLeg; + + bodyDef.position = b2Add({0.0f, 0.475f * s}, position); + bone->bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Capsule capsule = {{0.0f, -0.14f * s}, {0.0f, 0.125f * s}, 0.05f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + + //b2Polygon box = b2MakeOffsetBox(0.1f * s, 0.03f * s, {0.05f * s, -0.175f * s}, 0.0f); + //b2Body_CreatePolygon(bone->bodyId, &shapeDef, &box); + + capsule = {{-0.02f * s, -0.175f * s}, {0.13f * s, -0.175f * s}, 0.03f * s}; + b2Body_CreateCapsule(bone->bodyId, &footShapeDef, &capsule); + + b2Vec2 pivot = b2Add({0.0f, 0.625f * s}, position); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.enableLimit = true; + jointDef.lowerAngle = -0.5f * b2_pi; + jointDef.upperAngle = -0.02f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = 0.5f * maxTorque; + jointDef.drawSize = drawSize; + + bone->jointId = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + // upper left arm + { + Bone* bone = m_bones + Bone::e_upperLeftArm; + bone->parentIndex = Bone::e_torso; + + bodyDef.position = b2Add({0.0f, 1.225f * s}, position); + bone->bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Capsule capsule = {{0.0f, -0.125f * s}, {0.0f, 0.125f * s}, 0.035f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + + b2Vec2 pivot = b2Add({0.0f, 1.35f * s}, position); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.enableLimit = true; + jointDef.lowerAngle = -0.05f * b2_pi; + jointDef.upperAngle = 0.8f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = 0.25f * maxTorque; + jointDef.drawSize = drawSize; + + bone->jointId = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + // lower left arm + { + Bone* bone = m_bones + Bone::e_lowerLeftArm; + bone->parentIndex = Bone::e_upperLeftArm; + + bodyDef.position = b2Add({0.0f, 0.975f * s}, position); + bone->bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Capsule capsule = {{0.0f, -0.125f * s}, {0.0f, 0.125f * s}, 0.03f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + + b2Vec2 pivot = b2Add({0.0f, 1.1f * s}, position); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.enableLimit = true; + jointDef.lowerAngle = 0.01f * b2_pi; + jointDef.upperAngle = 0.5f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = 0.1f * maxTorque; + jointDef.drawSize = drawSize; + + bone->jointId = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + // upper right arm + { + Bone* bone = m_bones + Bone::e_upperRightArm; + bone->parentIndex = Bone::e_torso; + + bodyDef.position = b2Add({0.0f, 1.225f * s}, position); + bone->bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Capsule capsule = {{0.0f, -0.125f * s}, {0.0f, 0.125f * s}, 0.035f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + + b2Vec2 pivot = b2Add({0.0f, 1.35f * s}, position); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.enableLimit = true; + jointDef.lowerAngle = -0.05f * b2_pi; + jointDef.upperAngle = 0.8f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = 0.25f * maxTorque; + jointDef.drawSize = drawSize; + + bone->jointId = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + // lower right arm + { + Bone* bone = m_bones + Bone::e_lowerRightArm; + bone->parentIndex = Bone::e_upperRightArm; + + bodyDef.position = b2Add({0.0f, 0.975f * s}, position); + bone->bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Capsule capsule = {{0.0f, -0.125f * s}, {0.0f, 0.125f * s}, 0.03f * s}; + b2Body_CreateCapsule(bone->bodyId, &shapeDef, &capsule); + + b2Vec2 pivot = b2Add({0.0f, 1.1f * s}, position); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.enableLimit = true; + jointDef.lowerAngle = 0.01f * b2_pi; + jointDef.upperAngle = 0.5f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = 0.1f * maxTorque; + jointDef.drawSize = drawSize; + + bone->jointId = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + m_isSpawned = true; +} + +void Human::Despawn() +{ + if (m_isSpawned == false) + { + return; + } + + for (int i = 0; i < Bone::e_count; ++i) + { + if (B2_IS_NULL(m_bones[i].jointId)) + { + continue; + } + + b2World_DestroyJoint(m_bones[i].jointId); + m_bones[i].jointId = b2_nullJointId; + } + + for (int i = 0; i < Bone::e_count; ++i) + { + if (B2_IS_NULL(m_bones[i].bodyId)) + { + continue; + } + + b2World_DestroyBody(m_bones[i].bodyId); + m_bones[i].bodyId = b2_nullBodyId; + } + + m_isSpawned = false; +} diff --git a/samples/collection/human.h b/samples/collection/human.h new file mode 100644 index 00000000..a9f932ed --- /dev/null +++ b/samples/collection/human.h @@ -0,0 +1,42 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/types.h" + +struct Bone +{ + enum + { + e_hip = 0, + e_torso = 1, + e_head = 2, + e_upperLeftLeg = 3, + e_lowerLeftLeg = 4, + e_upperRightLeg = 5, + e_lowerRightLeg = 6, + e_upperLeftArm = 7, + e_lowerLeftArm = 8, + e_upperRightArm = 9, + e_lowerRightArm = 10, + e_count = 11, + }; + + b2BodyId bodyId; + b2JointId jointId; + int parentIndex; +}; + +class Human +{ + public: + Human(); + + void Spawn(b2WorldId worldId, b2Vec2 position, float scale, int groupIndex); + void Despawn(); + + b2WorldId m_worldId; + Bone m_bones[Bone::e_count]; + bool m_isSpawned; +}; diff --git a/samples/collection/sample_events.cpp b/samples/collection/sample_events.cpp index 8c13d1d0..292bd9f7 100644 --- a/samples/collection/sample_events.cpp +++ b/samples/collection/sample_events.cpp @@ -368,7 +368,7 @@ class ContactEvent : public Sample } m_wait = 0.5f; - m_force = 100.0f; + m_force = 200.0f; } void SpawnDebris() @@ -393,13 +393,14 @@ class ContactEvent : public Sample bodyDef.type = b2_dynamicBody; bodyDef.position = {RandomFloat(-38.0f, 38.0f), RandomFloat(-38.0f, 38.0f)}; bodyDef.angle = RandomFloat(-b2_pi, b2_pi); + bodyDef.linearVelocity = {RandomFloat(-5.0f, 5.0f), RandomFloat(-5.0f, 5.0f)}; + bodyDef.angularVelocity = RandomFloat(-1.0f, 1.0f); bodyDef.gravityScale = 0.0f; - bodyDef.linearDamping = 0.5f; - bodyDef.angularDamping = 0.5f; bodyDef.userData = m_bodyUserData + index; m_debrisIds[index] = b2World_CreateBody(m_worldId, &bodyDef); b2ShapeDef shapeDef = b2_defaultShapeDef; + shapeDef.restitution = 0.8f; // No events when debris hits debris shapeDef.enableContactEvents = false; @@ -437,24 +438,26 @@ class ContactEvent : public Sample g_draw.DrawString(5, m_textLine, "move using WASD"); m_textLine += m_textIncrement; + b2Vec2 position = b2Body_GetPosition(m_playerId); + if (glfwGetKey(g_mainWindow, GLFW_KEY_A) == GLFW_PRESS) { - b2Body_ApplyForceToCenter(m_playerId, {-m_force, 0.0f}, true); + b2Body_ApplyForce(m_playerId, {-m_force, 0.0f}, position, true); } if (glfwGetKey(g_mainWindow, GLFW_KEY_D) == GLFW_PRESS) { - b2Body_ApplyForceToCenter(m_playerId, {m_force, 0.0f}, true); + b2Body_ApplyForce(m_playerId, {m_force, 0.0f}, position, true); } if (glfwGetKey(g_mainWindow, GLFW_KEY_W) == GLFW_PRESS) { - b2Body_ApplyForceToCenter(m_playerId, {0.0f, m_force}, true); + b2Body_ApplyForce(m_playerId, {0.0f, m_force}, position, true); } if (glfwGetKey(g_mainWindow, GLFW_KEY_S) == GLFW_PRESS) { - b2Body_ApplyForceToCenter(m_playerId, {0.0f, -m_force}, true); + b2Body_ApplyForce(m_playerId, {0.0f, -m_force}, position, true); } Sample::Step(settings); diff --git a/samples/collection/sample_joints.cpp b/samples/collection/sample_joints.cpp index f0f3b405..98200584 100644 --- a/samples/collection/sample_joints.cpp +++ b/samples/collection/sample_joints.cpp @@ -1,131 +1,667 @@ // SPDX-FileCopyrightText: 2022 Erin Catto // SPDX-License-Identifier: MIT +#include "human.h" #include "sample.h" #include "settings.h" #include "box2d/box2d.h" #include "box2d/geometry.h" #include "box2d/hull.h" +#include "box2d/joint_util.h" +#include "box2d/math.h" + +#include +#include + +// Test the distance joint and all options +class DistanceJoint : public Sample +{ +public: + enum + { + e_maxCount = 10 + }; + + DistanceJoint(const Settings& settings) + : Sample(settings) + { + if (settings.restart == false) + { + g_camera.m_zoom = 0.25f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + m_groundId = b2World_CreateBody(m_worldId, &bodyDef); + } + + m_count = 0; + m_hertz = 2.0f; + m_dampingRatio = 0.5f; + m_length = 1.0f; + m_minLength = 0.5f; + m_maxLength = 2.0f; + m_fixedLength = false; + + for (int i = 0; i < e_maxCount; ++i) + { + m_bodyIds[i] = b2_nullBodyId; + m_jointIds[i] = b2_nullJointId; + } + + CreateScene(1); + } + + void CreateScene(int newCount) + { + // Must destroy joints before bodies + for (int i = 0; i < m_count; ++i) + { + b2World_DestroyJoint(m_jointIds[i]); + m_jointIds[i] = b2_nullJointId; + } + + for (int i = 0; i < m_count; ++i) + { + b2World_DestroyBody(m_bodyIds[i]); + m_bodyIds[i] = b2_nullBodyId; + } + + m_count = newCount; + + float radius = 0.25f; + b2Circle circle = {{0.0f, 0.0f}, radius}; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 20.0f; + + float yOffset = 20.0f; + + b2DistanceJointDef jointDef = b2DefaultDistanceJointDef(); + + b2BodyId prevBodyId = m_groundId; + for (int32_t i = 0; i < m_count; ++i) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = {m_length * (i + 1.0f), yOffset}; + m_bodyIds[i] = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreateCircle(m_bodyIds[i], &shapeDef, &circle); + + b2Vec2 pivotA = {m_length * i, yOffset}; + b2Vec2 pivotB = {m_length * (i + 1.0f), yOffset}; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = m_bodyIds[i]; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivotA); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivotB); + jointDef.hertz = m_hertz; + jointDef.dampingRatio = m_dampingRatio; + jointDef.length = m_length; + jointDef.minLength = m_minLength; + jointDef.maxLength = m_maxLength; + jointDef.collideConnected = true; + m_jointIds[i] = b2World_CreateDistanceJoint(m_worldId, &jointDef); + + prevBodyId = m_bodyIds[i]; + } + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 300.0f), ImGuiCond_Once); + ImGui::SetNextWindowSize(ImVec2(300.0f, 220.0f)); + ImGui::Begin("Options", nullptr, ImGuiWindowFlags_NoResize); + + if (ImGui::SliderFloat("length", &m_length, 0.1f, 4.0f, "%3.1f")) + { + if (m_fixedLength) + { + m_minLength = m_length; + m_maxLength = m_length; + } + + for (int32_t i = 0; i < m_count; ++i) + { + b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); + } + } + + if (ImGui::Checkbox("fixed length", &m_fixedLength)) + { + if (m_fixedLength) + { + m_minLength = m_length; + m_maxLength = m_length; + for (int32_t i = 0; i < m_count; ++i) + { + b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); + } + } + } + + if (m_fixedLength == false) + { + if (ImGui::SliderFloat("min length", &m_minLength, 0.1f, 4.0f, "%3.1f")) + { + for (int32_t i = 0; i < m_count; ++i) + { + b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); + } + } + + if (ImGui::SliderFloat("max length", &m_maxLength, 0.1f, 4.0f, "%3.1f")) + { + for (int32_t i = 0; i < m_count; ++i) + { + b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); + } + } + + if (ImGui::SliderFloat("hertz", &m_hertz, 0.0f, 15.0f, "%3.1f")) + { + for (int32_t i = 0; i < m_count; ++i) + { + b2DistanceJoint_SetTuning(m_jointIds[i], m_hertz, m_dampingRatio); + } + } + + if (ImGui::SliderFloat("damping", &m_dampingRatio, 0.0f, 4.0f, "%3.1f")) + { + for (int32_t i = 0; i < m_count; ++i) + { + b2DistanceJoint_SetTuning(m_jointIds[i], m_hertz, m_dampingRatio); + } + } + } + + int count = m_count; + if (ImGui::SliderInt("count", &count, 1, e_maxCount)) + { + CreateScene(count); + } + + ImGui::End(); + } + + static Sample* Create(const Settings& settings) + { + return new DistanceJoint(settings); + } + + b2BodyId m_groundId; + b2BodyId m_bodyIds[e_maxCount]; + b2JointId m_jointIds[e_maxCount]; + int32_t m_count; + float m_hertz; + float m_dampingRatio; + float m_length; + float m_minLength; + float m_maxLength; + bool m_fixedLength; +}; + +static int sampleDistanceJoint = RegisterSample("Joints", "Distance Joint", DistanceJoint::Create); + +/// This test shows how to use a motor joint. A motor joint +/// can be used to animate a dynamic body. With finite motor forces +/// the body can be blocked by collision with other bodies. +/// By setting the correction factor to zero, the motor joint acts +/// like top-down dry friction. +class MotorJoint : public Sample +{ +public: + MotorJoint(const Settings& settings) + : Sample(settings) + { + b2BodyId groundId; + { + groundId = b2World_CreateBody(m_worldId, &b2_defaultBodyDef); + b2Segment segment = {{-20.0f, 0.0f}, {20.0f, 0.0f}}; + b2Body_CreateSegment(groundId, &b2_defaultShapeDef, &segment); + } + + // Define motorized body + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = {0.0f, 8.0f}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Polygon box = b2MakeBox(2.0f, 0.5f); + b2ShapeDef shapeDef = b2_defaultShapeDef; + shapeDef.density = 1.0f; + b2Body_CreatePolygon(bodyId, &b2_defaultShapeDef, &box); + + m_maxForce = 500.0f; + m_maxTorque = 500.0f; + m_correctionFactor = 0.3f; + + b2MotorJointDef jointDef = b2DefaultMotorJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.maxForce = m_maxForce; + jointDef.maxTorque = m_maxTorque; + jointDef.correctionFactor = m_correctionFactor; + + m_jointId = b2World_CreateMotorJoint(m_worldId, &jointDef); + } + + m_go = false; + m_time = 0.0f; + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); + ImGui::SetNextWindowSize(ImVec2(250.0f, 140.0f)); + ImGui::Begin("Motor Joint", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize); + + if (ImGui::Checkbox("Go", &m_go)) + { + } + + if (ImGui::SliderFloat("Max Force", &m_maxForce, 0.0f, 1000.0f, "%.0f")) + { + b2MotorJoint_SetMaxForce(m_jointId, m_maxForce); + } + + if (ImGui::SliderFloat("Max Torque", &m_maxTorque, 0.0f, 1000.0f, "%.0f")) + { + b2MotorJoint_SetMaxTorque(m_jointId, m_maxTorque); + } + + if (ImGui::SliderFloat("Correction", &m_correctionFactor, 0.0f, 1.0f, "%.1f")) + { + b2MotorJoint_SetCorrectionFactor(m_jointId, m_correctionFactor); + } + + ImGui::End(); + } + + void Step(Settings& settings) override + { + if (m_go && settings.hertz > 0.0f) + { + m_time += 1.0f / settings.hertz; + } + + b2Vec2 linearOffset; + linearOffset.x = 6.0f * sinf(2.0f * m_time); + linearOffset.y = 8.0f + 4.0f * sinf(1.0f * m_time); + + float angularOffset = b2_pi * sinf(-0.5f * m_time); + + b2MotorJoint_SetLinearOffset(m_jointId, linearOffset); + b2MotorJoint_SetAngularOffset(m_jointId, angularOffset); + + b2Transform transform = {linearOffset, b2MakeRot(angularOffset)}; + g_draw.DrawTransform(transform); + + Sample::Step(settings); + + b2Vec2 force = b2MotorJoint_GetConstraintForce(m_jointId, settings.hertz); + float torque = b2MotorJoint_GetConstraintTorque(m_jointId, settings.hertz); + + g_draw.DrawString(5, m_textLine, "force = {%g, %g}, torque = %g", force.x, force.y, torque); + m_textLine += 15; + } + + static Sample* Create(const Settings& settings) + { + return new MotorJoint(settings); + } + + b2JointId m_jointId; + float m_time; + float m_maxForce; + float m_maxTorque; + float m_correctionFactor; + bool m_go; +}; + +static int sampleMotorJoint = RegisterSample("Joints", "Motor Joint", MotorJoint::Create); + +class RevoluteJoint : public Sample +{ +public: + RevoluteJoint(const Settings& settings) + : Sample(settings) + { + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = {0.0f, -1.0f}; + groundId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Polygon box = b2MakeBox(40.0f, 1.0f); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Body_CreatePolygon(groundId, &shapeDef, &box); + } + + m_enableLimit = true; + m_enableMotor = false; + m_motorSpeed = 1.0f; + m_motorTorque = 100.0f; + + { + b2Polygon box = b2MakeOffsetBox(0.25f, 3.0f, {0.0f, 3.0f}, 0.0f); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = {-10.0f, 20.0f}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 5.0f; + b2Body_CreatePolygon(bodyId, &shapeDef, &box); + + b2Vec2 pivot = {-10.0f, 20.5f}; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.motorSpeed = m_motorSpeed; + jointDef.maxMotorTorque = m_motorTorque; + jointDef.enableMotor = m_enableMotor; + jointDef.lowerAngle = -0.25f * b2_pi; + jointDef.upperAngle = 0.5f * b2_pi; + jointDef.enableLimit = m_enableLimit; + + m_jointId1 = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + { + b2Circle circle = {0}; + circle.radius = 2.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = {5.0f, 30.0f}; + m_ball = b2World_CreateBody(m_worldId, &bodyDef); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 5.0f; + + b2Body_CreateCircle(m_ball, &shapeDef, &circle); + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = {20.0f, 10.0f}; + bodyDef.type = b2_dynamicBody; + b2BodyId body = b2World_CreateBody(m_worldId, &bodyDef); + + b2Polygon box = b2MakeOffsetBox(10.0f, 0.5f, {-10.0f, 0.0f}, 0.0f); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 2.0f; + b2Body_CreatePolygon(body, &shapeDef, &box); + + b2Vec2 pivot = {19.0f, 10.0f}; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = body; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.lowerAngle = -0.25f * b2_pi; + jointDef.upperAngle = 0.0f * b2_pi; + jointDef.enableLimit = true; + jointDef.enableMotor = true; + jointDef.motorSpeed = 0.0f; + jointDef.maxMotorTorque = m_motorTorque; + + m_jointId2 = b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); + ImGui::SetNextWindowSize(ImVec2(250.0f, 140.0f)); + ImGui::Begin("Joint Controls", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize); + + if (ImGui::Checkbox("Limit", &m_enableLimit)) + { + b2RevoluteJoint_EnableLimit(m_jointId1, m_enableLimit); + } + + if (ImGui::Checkbox("Motor", &m_enableMotor)) + { + b2RevoluteJoint_EnableMotor(m_jointId1, m_enableMotor); + } + + if (ImGui::SliderFloat("Max Torque", &m_motorTorque, 0.0f, 500.0f, "%.0f")) + { + b2RevoluteJoint_SetMaxMotorTorque(m_jointId1, m_motorTorque); + } + + if (ImGui::SliderFloat("Speed", &m_motorSpeed, -20.0f, 20.0f, "%.0f")) + { + b2RevoluteJoint_SetMotorSpeed(m_jointId1, m_motorSpeed); + } + + ImGui::End(); + } + + void Step(Settings& settings) override + { + Sample::Step(settings); + + float torque1 = b2RevoluteJoint_GetMotorTorque(m_jointId1, settings.hertz); + g_draw.DrawString(5, m_textLine, "Motor Torque 1 = %4.1f", torque1); + m_textLine += m_textIncrement; + + float torque2 = b2RevoluteJoint_GetMotorTorque(m_jointId2, settings.hertz); + g_draw.DrawString(5, m_textLine, "Motor Torque 2 = %4.1f", torque2); + m_textLine += m_textIncrement; + } + + static Sample* Create(const Settings& settings) + { + return new RevoluteJoint(settings); + } + + b2BodyId m_ball; + b2JointId m_jointId1; + b2JointId m_jointId2; + float m_motorSpeed; + float m_motorTorque; + bool m_enableMotor; + bool m_enableLimit; +}; + +static int sampleRevolute = RegisterSample("Joints", "Revolute", RevoluteJoint::Create); + +class PrismaticJoint : public Sample +{ +public: + PrismaticJoint(const Settings& settings) + : Sample(settings) + { + if (settings.restart == false) + { + g_camera.m_center = {0.0f, 8.0f}; + g_camera.m_zoom = 0.5f; + } + + b2BodyId groundId = b2World_CreateBody(m_worldId, &b2_defaultBodyDef); + + m_enableLimit = true; + m_enableMotor = false; + m_motorSpeed = 2.0f; + m_motorForce = 25.0f; + + { + b2Polygon box = b2MakeBox(0.5f, 2.0f); + + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.position = {0.0f, 10.0f}; + bodyDef.type = b2_dynamicBody; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Body_CreatePolygon(bodyId, &b2_defaultShapeDef, &box); + + b2Vec2 pivot = {0.0f, 9.0f}; + b2Vec2 axis = b2Normalize({1.0f, 1.0f}); + b2PrismaticJointDef jointDef = b2DefaultPrismaticJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAxisA = b2Body_GetLocalVector(jointDef.bodyIdA, axis); + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.motorSpeed = m_motorSpeed; + jointDef.maxMotorForce = m_motorForce; + jointDef.enableMotor = m_enableMotor; + jointDef.lowerTranslation = -10.0f; + jointDef.upperTranslation = 10.0f; + jointDef.enableLimit = m_enableLimit; + + m_jointId = b2World_CreatePrismaticJoint(m_worldId, &jointDef); + } + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); + ImGui::SetNextWindowSize(ImVec2(250.0f, 140.0f)); + ImGui::Begin("Joint Controls", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize); + + if (ImGui::Checkbox("Limit", &m_enableLimit)) + { + b2PrismaticJoint_EnableLimit(m_jointId, m_enableLimit); + } + + if (ImGui::Checkbox("Motor", &m_enableMotor)) + { + b2PrismaticJoint_EnableMotor(m_jointId, m_enableMotor); + } + + if (ImGui::SliderFloat("Max Force", &m_motorForce, 0.0f, 50.0f, "%.0f")) + { + b2PrismaticJoint_SetMaxMotorForce(m_jointId, m_motorForce); + } + + if (ImGui::SliderFloat("Speed", &m_motorSpeed, -20.0f, 20.0f, "%.0f")) + { + b2PrismaticJoint_SetMotorSpeed(m_jointId, m_motorSpeed); + } + + ImGui::End(); + } + + void Step(Settings& settings) override + { + Sample::Step(settings); -// #include -#include "box2d/math.h" + float force = b2PrismaticJoint_GetMotorForce(m_jointId, settings.hertz); + g_draw.DrawString(5, m_textLine, "Motor Force = %4.1f", force); + m_textLine += m_textIncrement; + } -#include + static Sample* Create(const Settings& settings) + { + return new PrismaticJoint(settings); + } + b2JointId m_jointId; + float m_motorSpeed; + float m_motorForce; + bool m_enableMotor; + bool m_enableLimit; +}; -class RevoluteJoint : public Sample +static int samplePrismatic = RegisterSample("Joints", "Prismatic", PrismaticJoint::Create); + +class WheelJoint : public Sample { public: - RevoluteJoint(const Settings& settings) + WheelJoint(const Settings& settings) : Sample(settings) { - b2BodyId ground = b2_nullBodyId; + if (settings.restart == false) { - b2BodyDef bd = b2DefaultBodyDef(); - bd.position = {0.0f, -1.0f}; - ground = b2World_CreateBody(m_worldId, &bd); - - b2Polygon box = b2MakeBox(40.0f, 1.0f); - - b2ShapeDef sd = b2DefaultShapeDef(); - b2Body_CreatePolygon(ground, &sd, &box); + g_camera.m_center = {0.0f, 10.0f}; + g_camera.m_zoom = 0.25f; } + b2BodyId groundId = b2World_CreateBody(m_worldId, &b2_defaultBodyDef); + m_enableLimit = true; m_enableMotor = false; - m_motorSpeed = 1.0f; - - { - b2Polygon box = b2MakeOffsetBox(0.25f, 3.0f, {0.0f, 3.0f}, 0.0f); - - b2BodyDef bd = b2DefaultBodyDef(); - bd.type = b2_dynamicBody; - bd.position = {-10.0f, 20.0f}; - b2BodyId body = b2World_CreateBody(m_worldId, &bd); - - b2ShapeDef sd = b2DefaultShapeDef(); - sd.density = 5.0f; - b2Body_CreatePolygon(body, &sd, &box); - - b2Vec2 pivot = {-10.0f, 20.5f}; - b2RevoluteJointDef jd = b2DefaultRevoluteJointDef(); - jd.bodyIdA = ground; - jd.bodyIdB = body; - jd.localAnchorA = b2Body_GetLocalPoint(jd.bodyIdA, pivot); - jd.localAnchorB = b2Body_GetLocalPoint(jd.bodyIdB, pivot); - jd.motorSpeed = m_motorSpeed; - jd.maxMotorTorque = 10000.0f; - jd.enableMotor = m_enableMotor; - jd.lowerAngle = -0.25f * b2_pi; - jd.upperAngle = 0.5f * b2_pi; - jd.enableLimit = m_enableLimit; - - m_joint1 = b2World_CreateRevoluteJoint(m_worldId, &jd); - } + m_motorSpeed = 2.0f; + m_motorTorque = 5.0f; { - b2Circle circle = {0}; - circle.radius = 2.0f; - - b2BodyDef bd = b2DefaultBodyDef(); - bd.type = b2_dynamicBody; - bd.position = {5.0f, 30.0f}; - m_ball = b2World_CreateBody(m_worldId, &bd); - - b2ShapeDef sd = b2DefaultShapeDef(); - sd.density = 5.0f; - - b2Body_CreateCircle(m_ball, &sd, &circle); - } + b2Capsule capsule = {{0.0f, -0.5f}, {0.0f, 0.5f}, 0.5f}; - { - b2BodyDef bd = b2DefaultBodyDef(); - bd.position = {20.0f, 10.0f}; - bd.type = b2_dynamicBody; - b2BodyId body = b2World_CreateBody(m_worldId, &bd); + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.position = {0.0f, 10.25f}; + bodyDef.type = b2_dynamicBody; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); - b2Polygon box = b2MakeOffsetBox(10.0f, 0.5f, {-10.0f, 0.0f}, 0.0f); - b2ShapeDef sd = b2DefaultShapeDef(); - sd.density = 2.0f; - b2Body_CreatePolygon(body, &sd, &box); + b2Body_CreateCapsule(bodyId, &b2_defaultShapeDef, &capsule); - b2Vec2 pivot = {19.0f, 10.0f}; - b2RevoluteJointDef jd = b2DefaultRevoluteJointDef(); - jd.bodyIdA = ground; - jd.bodyIdB = body; - jd.localAnchorA = b2Body_GetLocalPoint(jd.bodyIdA, pivot); - jd.localAnchorB = b2Body_GetLocalPoint(jd.bodyIdB, pivot); - jd.lowerAngle = -0.25f * b2_pi; - jd.upperAngle = 0.0f * b2_pi; - jd.enableLimit = true; - jd.enableMotor = true; - jd.motorSpeed = 0.0f; - jd.maxMotorTorque = 10000.0f; + float hertz = 1.0f; + float dampingRatio = 0.7f; + b2LinearStiffness(&m_stiffness, &m_damping, hertz, dampingRatio, groundId, bodyId); - m_joint2 = b2World_CreateRevoluteJoint(m_worldId, &jd); + b2Vec2 pivot = {0.0f, 10.0f}; + b2Vec2 axis = b2Normalize({1.0f, 1.0f}); + b2WheelJointDef jointDef = b2DefaultWheelJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAxisA = b2Body_GetLocalVector(jointDef.bodyIdA, axis); + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.motorSpeed = m_motorSpeed; + jointDef.maxMotorTorque = m_motorTorque; + jointDef.enableMotor = m_enableMotor; + jointDef.lowerTranslation = -3.0f; + jointDef.upperTranslation = 3.0f; + jointDef.enableLimit = m_enableLimit; + jointDef.stiffness = m_stiffness; + jointDef.damping = m_damping; + + m_jointId = b2World_CreateWheelJoint(m_worldId, &jointDef); } } void UpdateUI() override { ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); - ImGui::SetNextWindowSize(ImVec2(200.0f, 100.0f)); - ImGui::Begin("Joint Controls", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize); + ImGui::SetNextWindowSize(ImVec2(250.0f, 180.0f)); + ImGui::Begin("Wheel Joint", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize); if (ImGui::Checkbox("Limit", &m_enableLimit)) { - b2RevoluteJoint_EnableLimit(m_joint1, m_enableLimit); + b2WheelJoint_EnableLimit(m_jointId, m_enableLimit); } if (ImGui::Checkbox("Motor", &m_enableMotor)) { - b2RevoluteJoint_EnableMotor(m_joint1, m_enableMotor); + b2WheelJoint_EnableMotor(m_jointId, m_enableMotor); + } + + if (ImGui::SliderFloat("Torque", &m_motorTorque, 0.0f, 20.0f, "%.0f")) + { + b2WheelJoint_SetMaxMotorTorque(m_jointId, m_motorTorque); } if (ImGui::SliderFloat("Speed", &m_motorSpeed, -20.0f, 20.0f, "%.0f")) { - b2RevoluteJoint_SetMotorSpeed(m_joint1, m_motorSpeed); + b2WheelJoint_SetMotorSpeed(m_jointId, m_motorSpeed); + } + + if (ImGui::SliderFloat("Stiffness", &m_stiffness, 0.0f, 100.0f, "%.0f")) + { + b2WheelJoint_SetStiffness(m_jointId, m_stiffness); + } + + if (ImGui::SliderFloat("Damping", &m_damping, 0.0f, 50.0f, "%.0f")) + { + b2WheelJoint_SetDamping(m_jointId, m_damping); } ImGui::End(); @@ -135,34 +671,31 @@ class RevoluteJoint : public Sample { Sample::Step(settings); - float torque1 = b2RevoluteJoint_GetMotorTorque(m_joint1, settings.hertz); - g_draw.DrawString(5, m_textLine, "Motor Torque 1= %4.0f", torque1); - m_textLine += m_textIncrement; - - float torque2 = b2RevoluteJoint_GetMotorTorque(m_joint2, settings.hertz); - g_draw.DrawString(5, m_textLine, "Motor Torque 2= %4.0f", torque2); + float torque = b2WheelJoint_GetMotorTorque(m_jointId, settings.hertz); + g_draw.DrawString(5, m_textLine, "Motor Torque = %4.1f", torque); m_textLine += m_textIncrement; } static Sample* Create(const Settings& settings) { - return new RevoluteJoint(settings); + return new WheelJoint(settings); } - b2BodyId m_ball; - b2JointId m_joint1; - b2JointId m_joint2; + b2JointId m_jointId; + float m_stiffness; + float m_damping; float m_motorSpeed; + float m_motorTorque; bool m_enableMotor; bool m_enableLimit; }; -static int sampleRevolute = RegisterSample("Joints", "Revolute", RevoluteJoint::Create); +static int sampleWheel = RegisterSample("Joints", "Wheel", WheelJoint::Create); // A suspension bridge class Bridge : public Sample { - public: +public: enum { e_count = 160 @@ -171,6 +704,11 @@ class Bridge : public Sample Bridge(const Settings& settings) : Sample(settings) { + if (settings.restart == false) + { + g_camera.m_zoom = 2.5f; + } + b2BodyId groundId = b2_nullBodyId; { b2BodyDef bodyDef = b2DefaultBodyDef(); @@ -187,18 +725,20 @@ class Bridge : public Sample int32_t jointIndex = 0; m_frictionTorque = 200.0f; + float xbase = -80.0f; + b2BodyId prevBodyId = groundId; for (int32_t i = 0; i < e_count; ++i) { b2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = b2_dynamicBody; - bodyDef.position = {-34.5f + 1.0f * i, 20.0f}; - // bodyDef.linearDamping = 0.1f; - // bodyDef.angularDamping = 0.1f; + bodyDef.position = {xbase + 0.5f + 1.0f * i, 20.0f}; + bodyDef.linearDamping = 0.1f; + bodyDef.angularDamping = 0.1f; b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); b2Body_CreatePolygon(bodyId, &shapeDef, &box); - b2Vec2 pivot = {-35.0f + 1.0f * i, 20.0f}; + b2Vec2 pivot = {xbase + 1.0f * i, 20.0f}; jointDef.bodyIdA = prevBodyId; jointDef.bodyIdB = bodyId; jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); @@ -210,7 +750,7 @@ class Bridge : public Sample prevBodyId = bodyId; } - b2Vec2 pivot = {-35.0f + 1.0f * e_count, 20.0f}; + b2Vec2 pivot = {xbase + 1.0f * e_count, 20.0f}; jointDef.bodyIdA = prevBodyId; jointDef.bodyIdB = groundId; jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); @@ -288,7 +828,7 @@ static int sampleBridgeIndex = RegisterSample("Joints", "Bridge", Bridge::Create class BallAndChain : public Sample { - public: +public: enum { e_count = 30 @@ -297,6 +837,11 @@ class BallAndChain : public Sample BallAndChain(const Settings& settings) : Sample(settings) { + if (settings.restart == false) + { + g_camera.m_center = {0.0f, -5.0f}; + } + b2BodyId groundId = b2_nullBodyId; { b2BodyDef bodyDef = b2DefaultBodyDef(); @@ -330,7 +875,7 @@ class BallAndChain : public Sample jointDef.bodyIdB = bodyId; jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); - jointDef.enableMotor = true; + // jointDef.enableMotor = true; jointDef.maxMotorTorque = m_frictionTorque; m_jointIds[jointIndex++] = b2World_CreateRevoluteJoint(m_worldId, &jointDef); @@ -387,9 +932,11 @@ class BallAndChain : public Sample static int sampleBallAndChainIndex = RegisterSample("Joints", "Ball & Chain", BallAndChain::Create); +// This sample shows the limitations of an iterative solver. The cantilever sags even though the weld +// joint is stiff as possible. class Cantilever : public Sample { - public: +public: enum { e_count = 8 @@ -398,6 +945,12 @@ class Cantilever : public Sample Cantilever(const Settings& settings) : Sample(settings) { + if (settings.restart == false) + { + g_camera.m_zoom = 0.25f; + g_camera.m_center = {0.0f, 0.0f}; + } + b2BodyId groundId = b2_nullBodyId; { b2BodyDef bodyDef = b2DefaultBodyDef(); @@ -422,245 +975,57 @@ class Cantilever : public Sample b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); b2Body_CreatePolygon(bodyId, &shapeDef, &box); - b2Vec2 pivot = {(2.0f * i) * hx, 0.0f}; - jointDef.bodyIdA = prevBodyId; - jointDef.bodyIdB = bodyId; - jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); - jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); - // jointDef.angularHertz = i == 0 ? 0.0f : 1.0f; - // jointDef.linearHertz = 5.0f; - b2World_CreateWeldJoint(m_worldId, &jointDef); - - prevBodyId = bodyId; - } - - m_tipId = prevBodyId; - } - } - - void Step(Settings& settings) override - { - Sample::Step(settings); - - b2Vec2 tipPosition = b2Body_GetPosition(m_tipId); - g_draw.DrawString(5, m_textLine, "tip-y = %.2f", tipPosition.y); - m_textLine += m_textIncrement; - } - - static Sample* Create(const Settings& settings) - { - return new Cantilever(settings); - } - - b2BodyId m_tipId; -}; - -static int sampleCantileverIndex = RegisterSample("Joints", "Cantilever", Cantilever::Create); - -// Test the distance joint and all options -class DistanceJoint : public Sample -{ - public: - enum - { - e_maxCount = 10 - }; - - DistanceJoint(const Settings& settings) - : Sample(settings) - { - if (settings.restart == false) - { - g_camera.m_zoom = 0.25f; - } - - { - b2BodyDef bodyDef = b2DefaultBodyDef(); - m_groundId = b2World_CreateBody(m_worldId, &bodyDef); - } - - m_count = 0; - m_hertz = 2.0f; - m_dampingRatio = 0.5f; - m_length = 1.0f; - m_minLength = 0.5f; - m_maxLength = 2.0f; - m_fixedLength = false; - - for (int i = 0; i < e_maxCount; ++i) - { - m_bodyIds[i] = b2_nullBodyId; - m_jointIds[i] = b2_nullJointId; - } - - CreateScene(1); - } - - void CreateScene(int newCount) - { - // Must destroy joints before bodies - for (int i = 0; i < m_count; ++i) - { - b2World_DestroyJoint(m_jointIds[i]); - m_jointIds[i] = b2_nullJointId; - } - - for (int i = 0; i < m_count; ++i) - { - b2World_DestroyBody(m_bodyIds[i]); - m_bodyIds[i] = b2_nullBodyId; - } - - m_count = newCount; - - float radius = 0.25f; - b2Circle circle = {{0.0f, 0.0f}, radius}; - - b2ShapeDef shapeDef = b2DefaultShapeDef(); - shapeDef.density = 20.0f; - - float yOffset = 20.0f; - - b2DistanceJointDef jointDef = b2DefaultDistanceJointDef(); - - b2BodyId prevBodyId = m_groundId; - for (int32_t i = 0; i < m_count; ++i) - { - b2BodyDef bodyDef = b2DefaultBodyDef(); - bodyDef.type = b2_dynamicBody; - bodyDef.position = {m_length * (i + 1.0f), yOffset}; - m_bodyIds[i] = b2World_CreateBody(m_worldId, &bodyDef); - b2Body_CreateCircle(m_bodyIds[i], &shapeDef, &circle); - - b2Vec2 pivotA = {m_length * i, yOffset}; - b2Vec2 pivotB = {m_length * (i + 1.0f), yOffset}; - jointDef.bodyIdA = prevBodyId; - jointDef.bodyIdB = m_bodyIds[i]; - jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivotA); - jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivotB); - jointDef.hertz = m_hertz; - jointDef.dampingRatio = m_dampingRatio; - jointDef.length = m_length; - jointDef.minLength = m_minLength; - jointDef.maxLength = m_maxLength; - jointDef.collideConnected = true; - m_jointIds[i] = b2World_CreateDistanceJoint(m_worldId, &jointDef); - - prevBodyId = m_bodyIds[i]; - } - } - - void UpdateUI() override - { - ImGui::SetNextWindowPos(ImVec2(10.0f, 300.0f), ImGuiCond_Once); - ImGui::SetNextWindowSize(ImVec2(300.0f, 220.0f)); - ImGui::Begin("Options", nullptr, ImGuiWindowFlags_NoResize); - - if (ImGui::SliderFloat("length", &m_length, 0.1f, 4.0f, "%3.1f")) - { - if (m_fixedLength) - { - m_minLength = m_length; - m_maxLength = m_length; - } - - for (int32_t i = 0; i < m_count; ++i) - { - b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); - } - } - - if (ImGui::Checkbox("fixed length", &m_fixedLength)) - { - if (m_fixedLength) - { - m_minLength = m_length; - m_maxLength = m_length; - for (int32_t i = 0; i < m_count; ++i) - { - b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); - } - } - } - - if (m_fixedLength == false) - { - if (ImGui::SliderFloat("min length", &m_minLength, 0.1f, 4.0f, "%3.1f")) - { - for (int32_t i = 0; i < m_count; ++i) - { - b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); - } - } - - if (ImGui::SliderFloat("max length", &m_maxLength, 0.1f, 4.0f, "%3.1f")) - { - for (int32_t i = 0; i < m_count; ++i) - { - b2DistanceJoint_SetLength(m_jointIds[i], m_length, m_minLength, m_maxLength); - } - } - - if (ImGui::SliderFloat("hertz", &m_hertz, 0.0f, 15.0f, "%3.1f")) - { - for (int32_t i = 0; i < m_count; ++i) - { - b2DistanceJoint_SetTuning(m_jointIds[i], m_hertz, m_dampingRatio); - } - } + b2Vec2 pivot = {(2.0f * i) * hx, 0.0f}; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + // jointDef.angularHertz = i == 0 ? 0.0f : 1.0f; + // jointDef.linearHertz = 5.0f; + b2World_CreateWeldJoint(m_worldId, &jointDef); - if (ImGui::SliderFloat("damping", &m_dampingRatio, 0.0f, 4.0f, "%3.1f")) - { - for (int32_t i = 0; i < m_count; ++i) - { - b2DistanceJoint_SetTuning(m_jointIds[i], m_hertz, m_dampingRatio); - } + prevBodyId = bodyId; } - } - int count = m_count; - if (ImGui::SliderInt("count", &count, 1, e_maxCount)) - { - CreateScene(count); + m_tipId = prevBodyId; } + } - ImGui::End(); + void Step(Settings& settings) override + { + Sample::Step(settings); + + b2Vec2 tipPosition = b2Body_GetPosition(m_tipId); + g_draw.DrawString(5, m_textLine, "tip-y = %.2f", tipPosition.y); + m_textLine += m_textIncrement; } static Sample* Create(const Settings& settings) { - return new DistanceJoint(settings); + return new Cantilever(settings); } - b2BodyId m_groundId; - b2BodyId m_bodyIds[e_maxCount]; - b2JointId m_jointIds[e_maxCount]; - int32_t m_count; - float m_hertz; - float m_dampingRatio; - float m_length; - float m_minLength; - float m_maxLength; - bool m_fixedLength; + b2BodyId m_tipId; }; -static int sampleDistanceJoint = RegisterSample("Joints", "Distance Joint", DistanceJoint::Create); +static int sampleCantileverIndex = RegisterSample("Joints", "Cantilever", Cantilever::Create); -class AllJoints : public Sample +// This test ensures joints work correctly with bodies that have fixed rotation +class FixedRotation : public Sample { public: enum { - e_count = 5 + e_count = 6 }; - AllJoints(const Settings& settings) + FixedRotation(const Settings& settings) : Sample(settings) { m_groundId = b2World_CreateBody(m_worldId, &b2_defaultBodyDef); - m_fixedRotation = false; + m_fixedRotation = true; - for (int i = 0; i < 5; ++i) + for (int i = 0; i < e_count; ++i) { m_bodyIds[i] = b2_nullBodyId; m_jointIds[i] = b2_nullJointId; @@ -671,7 +1036,7 @@ class AllJoints : public Sample void CreateScene() { - for (int i = 0; i < 5; ++i) + for (int i = 0; i < e_count; ++i) { if (B2_NON_NULL(m_jointIds[i])) { @@ -693,31 +1058,82 @@ class AllJoints : public Sample bodyDef.fixedRotation = m_fixedRotation; b2Polygon box = b2MakeBox(1.0f, 1.0f); - b2ShapeDef shapeDef = b2_defaultShapeDef; - shapeDef.density = 1.0f; int index = 0; + + // distance joint + { + assert(index < e_count); + + bodyDef.position = position; + m_bodyIds[index] = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreatePolygon(m_bodyIds[index], &b2_defaultShapeDef, &box); + + float length = 2.0f; + b2Vec2 pivot1 = {position.x, position.y + 1.0f + length}; + b2Vec2 pivot2 = {position.x, position.y + 1.0f}; + b2DistanceJointDef jointDef = b2DefaultDistanceJointDef(); + jointDef.bodyIdA = m_groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot1); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot2); + jointDef.minLength = length; + jointDef.maxLength = length; + m_jointIds[index] = b2World_CreateDistanceJoint(m_worldId, &jointDef); + } + + position.x += 5.0f; + ++index; + + // motor joint { + assert(index < e_count); + bodyDef.position = position; m_bodyIds[index] = b2World_CreateBody(m_worldId, &bodyDef); - b2Body_CreatePolygon(m_bodyIds[index], &shapeDef, &box); + b2Body_CreatePolygon(m_bodyIds[index], &b2_defaultShapeDef, &box); b2Vec2 pivot = {position.x - 1.0f, position.y}; - b2WeldJointDef jointDef = b2DefaultWeldJointDef(); + b2MotorJointDef jointDef = b2DefaultMotorJointDef(); + jointDef.bodyIdA = m_groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.linearOffset = position; + jointDef.maxForce = 200.0f; + jointDef.maxTorque = 200.0f; + m_jointIds[index] = b2World_CreateMotorJoint(m_worldId, &jointDef); + } + + position.x += 5.0f; + ++index; + + // prismatic joint + { + assert(index < e_count); + + bodyDef.position = position; + m_bodyIds[index] = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreatePolygon(m_bodyIds[index], &b2_defaultShapeDef, &box); + + b2Vec2 pivot = {position.x - 1.0f, position.y}; + b2PrismaticJointDef jointDef = b2DefaultPrismaticJointDef(); jointDef.bodyIdA = m_groundId; jointDef.bodyIdB = m_bodyIds[index]; jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); - m_jointIds[index] = b2World_CreateWeldJoint(m_worldId, &jointDef); + jointDef.localAxisA = b2Body_GetLocalVector(jointDef.bodyIdA, {1.0f, 0.0f}); + m_jointIds[index] = b2World_CreatePrismaticJoint(m_worldId, &jointDef); } position.x += 5.0f; ++index; + // revolute joint { + assert(index < e_count); + bodyDef.position = position; m_bodyIds[index] = b2World_CreateBody(m_worldId, &bodyDef); - b2Body_CreatePolygon(m_bodyIds[index], &shapeDef, &box); + b2Body_CreatePolygon(m_bodyIds[index], &b2_defaultShapeDef, &box); b2Vec2 pivot = {position.x - 1.0f, position.y}; b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); @@ -731,48 +1147,65 @@ class AllJoints : public Sample position.x += 5.0f; ++index; + // weld joint { + assert(index < e_count); + bodyDef.position = position; m_bodyIds[index] = b2World_CreateBody(m_worldId, &bodyDef); - b2Body_CreatePolygon(m_bodyIds[index], &shapeDef, &box); + b2Body_CreatePolygon(m_bodyIds[index], &b2_defaultShapeDef, &box); b2Vec2 pivot = {position.x - 1.0f, position.y}; - b2PrismaticJointDef jointDef = b2DefaultPrismaticJointDef(); + b2WeldJointDef jointDef = b2DefaultWeldJointDef(); jointDef.bodyIdA = m_groundId; jointDef.bodyIdB = m_bodyIds[index]; jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); - jointDef.localAxisA = b2Body_GetLocalVector(jointDef.bodyIdA, {1.0f, 0.0f}); - m_jointIds[index] = b2World_CreatePrismaticJoint(m_worldId, &jointDef); + jointDef.angularHertz = 1.0f; + jointDef.angularDampingRatio = 0.5f; + jointDef.linearHertz = 1.0f; + jointDef.linearDampingRatio = 0.5f; + m_jointIds[index] = b2World_CreateWeldJoint(m_worldId, &jointDef); } position.x += 5.0f; ++index; + // wheel joint { + assert(index < e_count); + bodyDef.position = position; m_bodyIds[index] = b2World_CreateBody(m_worldId, &bodyDef); - b2Body_CreatePolygon(m_bodyIds[index], &shapeDef, &box); + b2Body_CreatePolygon(m_bodyIds[index], &b2_defaultShapeDef, &box); - float length = 2.0f; - b2Vec2 pivot1 = {position.x, position.y + 1.0f + length}; - b2Vec2 pivot2 = {position.x, position.y + 1.0f}; - b2DistanceJointDef jointDef = b2DefaultDistanceJointDef(); + b2Vec2 pivot = {position.x - 1.0f, position.y}; + b2WheelJointDef jointDef = b2DefaultWheelJointDef(); jointDef.bodyIdA = m_groundId; jointDef.bodyIdB = m_bodyIds[index]; - jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot1); - jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot2); - jointDef.minLength = length; - jointDef.maxLength = length; - m_jointIds[index] = b2World_CreateDistanceJoint(m_worldId, &jointDef); + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.localAxisA = b2Body_GetLocalVector(jointDef.bodyIdA, {1.0f, 0.0f}); + jointDef.stiffness = 30.0f; + jointDef.damping = 10.0f; + jointDef.lowerTranslation = -1.0f; + jointDef.upperTranslation = 1.0f; + jointDef.enableLimit = true; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 10.0f; + jointDef.motorSpeed = 1.0f; + m_jointIds[index] = b2World_CreateWheelJoint(m_worldId, &jointDef); } + + position.x += 5.0f; + ++index; } void UpdateUI() override { ImGui::SetNextWindowPos(ImVec2(10.0f, 300.0f), ImGuiCond_Once); ImGui::SetNextWindowSize(ImVec2(300.0f, 60.0f)); - ImGui::Begin("Options", nullptr, ImGuiWindowFlags_NoResize); + ImGui::Begin("Fixed Rotation", nullptr, ImGuiWindowFlags_NoResize); if (ImGui::Checkbox("fixed rotation", &m_fixedRotation)) { @@ -784,20 +1217,21 @@ class AllJoints : public Sample static Sample* Create(const Settings& settings) { - return new AllJoints(settings); + return new FixedRotation(settings); } b2BodyId m_groundId; - b2BodyId m_bodyIds[5]; - b2JointId m_jointIds[5]; + b2BodyId m_bodyIds[e_count]; + b2JointId m_jointIds[e_count]; bool m_fixedRotation; }; -static int sampleAllJoints = RegisterSample("Joints", "All Joints", AllJoints::Create); +static int sampleFixedRotation = RegisterSample("Joints", "Fixed Rotation", FixedRotation::Create); +// This shows how you can implement a constraint outside of Box2D class UserConstraint : public Sample { - public: +public: UserConstraint(const Settings& settings) : Sample(settings) { @@ -910,3 +1344,303 @@ class UserConstraint : public Sample }; static int sampleUserConstraintIndex = RegisterSample("Joints", "User Constraint", UserConstraint::Create); + +// This is a fun demo that shows off the wheel joint +class Car : public Sample +{ +public: + Car(const Settings& settings) + : Sample(settings) + { + b2BodyId groundId; + { + groundId = b2World_CreateBody(m_worldId, &b2_defaultBodyDef); + + b2Segment segment = {{-20.0f, 0.0f}, {20.0f, 0.0f}}; + b2Body_CreateSegment(groundId, &b2_defaultShapeDef, &segment); + + float hs[10] = {0.25f, 1.0f, 4.0f, 0.0f, 0.0f, -1.0f, -2.0f, -2.0f, -1.25f, 0.0f}; + + float x = 20.0f, y1 = 0.0f, dx = 5.0f; + + for (int i = 0; i < 10; ++i) + { + float y2 = hs[i]; + segment = {{x, y1}, {x + dx, y2}}; + b2Body_CreateSegment(groundId, &b2_defaultShapeDef, &segment); + y1 = y2; + x += dx; + } + + for (int i = 0; i < 10; ++i) + { + float y2 = hs[i]; + segment = {{x, y1}, {x + dx, y2}}; + b2Body_CreateSegment(groundId, &b2_defaultShapeDef, &segment); + y1 = y2; + x += dx; + } + + segment = {{x, 0.0f}, {x + 40.0f, 0.0f}}; + b2Body_CreateSegment(groundId, &b2_defaultShapeDef, &segment); + + x += 80.0f; + segment = {{x, 0.0f}, {x + 40.0f, 0.0f}}; + b2Body_CreateSegment(groundId, &b2_defaultShapeDef, &segment); + + x += 40.0f; + segment = {{x, 0.0f}, {x + 10.0f, 5.0f}}; + b2Body_CreateSegment(groundId, &b2_defaultShapeDef, &segment); + + x += 20.0f; + segment = {{x, 0.0f}, {x + 40.0f, 0.0f}}; + b2Body_CreateSegment(groundId, &b2_defaultShapeDef, &segment); + + x += 40.0f; + segment = {{x, 0.0f}, {x, 20.0f}}; + b2Body_CreateSegment(groundId, &b2_defaultShapeDef, &segment); + } + + // Teeter + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.position = {140.0f, 1.0f}; + bodyDef.angularVelocity = 1.0f; + bodyDef.type = b2_dynamicBody; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Polygon box = b2MakeBox(10.0f, 0.25f); + b2Body_CreatePolygon(bodyId, &b2_defaultShapeDef, &box); + + b2Vec2 pivot = bodyDef.position; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.lowerAngle = -8.0f * b2_pi / 180.0f; + jointDef.upperAngle = 8.0f * b2_pi / 180.0f; + jointDef.enableLimit = true; + b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + // Bridge + { + int N = 20; + b2Capsule capsule = {{-1.0f, 0.0f}, {1.0f, 0.0f}, 0.125f}; + + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + + b2BodyId prevBodyId = groundId; + for (int i = 0; i < N; ++i) + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = {161.0f + 2.0f * i, -0.125f}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreateCapsule(bodyId, &b2_defaultShapeDef, &capsule); + + b2Vec2 pivot = {160.0f + 2.0f * i, -0.125f}; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + b2World_CreateRevoluteJoint(m_worldId, &jointDef); + + prevBodyId = bodyId; + } + + b2Vec2 pivot = {160.0f + 2.0f * N, -0.125f}; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = groundId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 50.0f; + b2World_CreateRevoluteJoint(m_worldId, &jointDef); + } + + // Boxes + { + b2Polygon box = b2MakeBox(0.5f, 0.5f); + + b2BodyId bodyId; + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + + b2ShapeDef shapeDef = b2_defaultShapeDef; + shapeDef.friction = 0.25f; + shapeDef.restitution = 0.25f; + shapeDef.density = 0.25f; + + bodyDef.position = {230.0f, 0.5f}; + bodyId = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreatePolygon(bodyId, &shapeDef, &box); + + bodyDef.position = {230.0f, 1.5f}; + bodyId = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreatePolygon(bodyId, &shapeDef, &box); + + bodyDef.position = {230.0f, 2.5f}; + bodyId = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreatePolygon(bodyId, &shapeDef, &box); + + bodyDef.position = {230.0f, 3.5f}; + bodyId = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreatePolygon(bodyId, &shapeDef, &box); + + bodyDef.position = {230.0f, 4.5f}; + bodyId = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreatePolygon(bodyId, &shapeDef, &box); + } + + // Car + { + b2Vec2 vertices[] = { + {-1.5f, -0.5f}, {1.5f, -0.5f}, {1.5f, 0.0f}, {0.0f, 0.9f}, {-1.15f, 0.9f}, {-1.5f, 0.2f}, + }; + + b2Hull hull = b2ComputeHull(vertices, B2_ARRAY_COUNT(vertices)); + b2Polygon chassis = b2MakePolygon(&hull, 0.0f); + + b2Circle circle = {{0.0f, 0.0f}, 0.4f}; + + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = {0.0f, 1.0f}; + m_carId = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreatePolygon(m_carId, &b2_defaultShapeDef, &chassis); + + b2ShapeDef shapeDef = b2_defaultShapeDef; + shapeDef.density = 1.0f; + shapeDef.friction = 0.6f; + + bodyDef.position = {-1.0f, 0.35f}; + m_wheelId1 = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreateCircle(m_wheelId1, &shapeDef, &circle); + + bodyDef.position = {1.0f, 0.4f}; + m_wheelId2 = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreateCircle(m_wheelId2, &shapeDef, &circle); + + b2Vec2 axis = {0.0f, 1.0f}; + + float mass1 = b2Body_GetMass(m_wheelId1); + float mass2 = b2Body_GetMass(m_wheelId2); + + float hertz = 4.0f; + float dampingRatio = 0.7f; + float omega = 2.0f * b2_pi * hertz; + + b2Vec2 pivot = b2Body_GetPosition(m_wheelId1); + + b2WheelJointDef jointDef = b2DefaultWheelJointDef(); + + jointDef.bodyIdA = m_carId; + jointDef.bodyIdB = m_wheelId1; + jointDef.localAxisA = b2Body_GetLocalVector(jointDef.bodyIdA, axis); + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.motorSpeed = 0.0f; + jointDef.maxMotorTorque = 20.0f; + jointDef.enableMotor = true; + jointDef.stiffness = mass1 * omega * omega; + jointDef.damping = 2.0f * mass1 * dampingRatio * omega; + jointDef.lowerTranslation = -0.25f; + jointDef.upperTranslation = 0.25f; + jointDef.enableLimit = true; + m_jointId1 = b2World_CreateWheelJoint(m_worldId, &jointDef); + + pivot = b2Body_GetPosition(m_wheelId2); + jointDef.bodyIdA = m_carId; + jointDef.bodyIdB = m_wheelId2; + jointDef.localAxisA = b2Body_GetLocalVector(jointDef.bodyIdA, axis); + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.motorSpeed = 0.0f; + jointDef.maxMotorTorque = 10.0f; + jointDef.enableMotor = false; + jointDef.stiffness = mass2 * omega * omega; + jointDef.damping = 2.0f * mass2 * dampingRatio * omega; + jointDef.lowerTranslation = -0.25f; + jointDef.upperTranslation = 0.25f; + jointDef.enableLimit = true; + m_jointId2 = b2World_CreateWheelJoint(m_worldId, &jointDef); + } + + m_speed = 50.0f; + } + + void Step(Settings& settings) override + { + if (glfwGetKey(g_mainWindow, GLFW_KEY_A) == GLFW_PRESS) + { + b2WheelJoint_SetMotorSpeed(m_jointId1, m_speed); + } + + if (glfwGetKey(g_mainWindow, GLFW_KEY_S) == GLFW_PRESS) + { + b2WheelJoint_SetMotorSpeed(m_jointId1, 0.0f); + } + + if (glfwGetKey(g_mainWindow, GLFW_KEY_D) == GLFW_PRESS) + { + b2WheelJoint_SetMotorSpeed(m_jointId1, -m_speed); + } + + g_draw.DrawString(5, m_textLine, "Keys: left = a, brake = s, right = d"); + m_textLine += m_textIncrement; + + b2Vec2 carPosition = b2Body_GetPosition(m_carId); + g_camera.m_center.x = carPosition.x; + + Sample::Step(settings); + } + + static Sample* Create(const Settings& settings) + { + return new Car(settings); + } + + b2BodyId m_carId; + b2BodyId m_wheelId1; + b2BodyId m_wheelId2; + + float m_speed; + b2JointId m_jointId1; + b2JointId m_jointId2; +}; + +static int sampleCar = RegisterSample("Joints", "Car", Car::Create); + +class Ragdoll : public Sample +{ +public: + Ragdoll(const Settings& settings) + : Sample(settings) + { + if (settings.restart == false) + { + g_camera.m_zoom = 0.25f; + g_camera.m_center = {0.0f, 5.0f}; + } + + b2BodyId groundId; + { + groundId = b2World_CreateBody(m_worldId, &b2_defaultBodyDef); + b2Segment segment = {{-20.0f, 0.0f}, {20.0f, 0.0f}}; + b2Body_CreateSegment(groundId, &b2_defaultShapeDef, &segment); + } + + m_human.Spawn(m_worldId, {0.0f, 10.0f}, 2.0f, 1); + } + + static Sample* Create(const Settings& settings) + { + return new Ragdoll(settings); + } + + Human m_human; +}; + +static int sampleRagdoll = RegisterSample("Joints", "Ragdoll", Ragdoll::Create); diff --git a/samples/collection/sample_shapes.cpp b/samples/collection/sample_shapes.cpp index 423cabab..c1fc6da6 100644 --- a/samples/collection/sample_shapes.cpp +++ b/samples/collection/sample_shapes.cpp @@ -6,6 +6,7 @@ #include "box2d/box2d.h" #include "box2d/geometry.h" +#include "box2d/hull.h" #include "box2d/math.h" #include @@ -27,7 +28,6 @@ static float RayCastClosestCallback(b2ShapeId shapeId, b2Vec2 point, b2Vec2 norm return fraction; } -// TODO_ERIN fix bounce tunnel class ChainShape : public Sample { public: @@ -224,3 +224,186 @@ class ChainShape : public Sample }; static int sampleChainShape = RegisterSample("Shapes", "Chain Shape", ChainShape::Create); + +// This sample shows how careful creation of compound shapes leads to better simulation and avoids +// objects getting stuck. +class CompoundShapes : public Sample +{ +public: + CompoundShapes(const Settings& settings) + : Sample(settings) + { + if (settings.restart == false) + { + g_camera.m_center = {0.0f, 0.0f}; + g_camera.m_zoom = 0.5f; + } + + { + b2BodyId groundId = b2World_CreateBody(m_worldId, &b2_defaultBodyDef); + b2Segment segment = {{50.0f, 0.0f}, {-50.0f, 0.0f}}; + b2Body_CreateSegment(groundId, &b2_defaultShapeDef, &segment); + } + + // Table 1 + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = {-15.0f, 1.0f}; + m_table1Id = b2World_CreateBody(m_worldId, &bodyDef); + + b2Polygon top = b2MakeOffsetBox(3.0f, 0.5f, {0.0f, 3.5f}, 0.0f); + b2Polygon leftLeg = b2MakeOffsetBox(0.5f, 1.5f, {-2.5f, 1.5f}, 0.0f); + b2Polygon rightLeg = b2MakeOffsetBox(0.5f, 1.5f, {2.5f, 1.5f}, 0.0f); + + b2Body_CreatePolygon(m_table1Id, &b2_defaultShapeDef, &top); + b2Body_CreatePolygon(m_table1Id, &b2_defaultShapeDef, &leftLeg); + b2Body_CreatePolygon(m_table1Id, &b2_defaultShapeDef, &rightLeg); + } + + // Table 2 + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = {-5.0f, 1.0f}; + m_table2Id = b2World_CreateBody(m_worldId, &bodyDef); + + b2Polygon top = b2MakeOffsetBox(3.0f, 0.5f, {0.0f, 3.5f}, 0.0f); + b2Polygon leftLeg = b2MakeOffsetBox(0.5f, 2.0f, {-2.5f, 2.0f}, 0.0f); + b2Polygon rightLeg = b2MakeOffsetBox(0.5f, 2.0f, {2.5f, 2.0f}, 0.0f); + + b2Body_CreatePolygon(m_table2Id, &b2_defaultShapeDef, &top); + b2Body_CreatePolygon(m_table2Id, &b2_defaultShapeDef, &leftLeg); + b2Body_CreatePolygon(m_table2Id, &b2_defaultShapeDef, &rightLeg); + } + + // Spaceship 1 + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = {5.0f, 1.0f}; + m_ship1Id = b2World_CreateBody(m_worldId, &bodyDef); + + b2Vec2 vertices[3]; + + vertices[0] = {-2.0f, 0.0f}; + vertices[1] = {0.0f, 4.0f / 3.0f}; + vertices[2] = {0.0f, 4.0f}; + b2Hull hull = b2ComputeHull(vertices, 3); + b2Polygon left = b2MakePolygon(&hull, 0.0f); + + vertices[0] = {2.0f, 0.0f}; + vertices[1] = {0.0f, 4.0f / 3.0f}; + vertices[2] = {0.0f, 4.0f}; + hull = b2ComputeHull(vertices, 3); + b2Polygon right = b2MakePolygon(&hull, 0.0f); + + b2Body_CreatePolygon(m_ship1Id, &b2_defaultShapeDef, &left); + b2Body_CreatePolygon(m_ship1Id, &b2_defaultShapeDef, &right); + } + + // Spaceship 2 + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = {15.0f, 1.0f}; + m_ship2Id = b2World_CreateBody(m_worldId, &bodyDef); + + b2Vec2 vertices[3]; + + vertices[0] = {-2.0f, 0.0f}; + vertices[1] = {1.0f, 2.0f}; + vertices[2] = {0.0f, 4.0f}; + b2Hull hull = b2ComputeHull(vertices, 3); + b2Polygon left = b2MakePolygon(&hull, 0.0f); + + vertices[0] = {2.0f, 0.0f}; + vertices[1] = {-1.0f, 2.0f}; + vertices[2] = {0.0f, 4.0f}; + hull = b2ComputeHull(vertices, 3); + b2Polygon right = b2MakePolygon(&hull, 0.0f); + + b2Body_CreatePolygon(m_ship2Id, &b2_defaultShapeDef, &left); + b2Body_CreatePolygon(m_ship2Id, &b2_defaultShapeDef, &right); + } + } + + void Spawn() + { + // Table 1 obstruction + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = b2Body_GetPosition(m_table1Id); + bodyDef.angle = b2Body_GetAngle(m_table1Id); + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Polygon box = b2MakeOffsetBox(4.0f, 0.1f, {0.0f, 3.0f}, 0.0f); + b2Body_CreatePolygon(bodyId, &b2_defaultShapeDef, &box); + } + + // Table 2 obstruction + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = b2Body_GetPosition(m_table2Id); + bodyDef.angle = b2Body_GetAngle(m_table2Id); + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Polygon box = b2MakeOffsetBox(4.0f, 0.1f, {0.0f, 3.0f}, 0.0f); + b2Body_CreatePolygon(bodyId, &b2_defaultShapeDef, &box); + } + + // Ship 1 obstruction + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = b2Body_GetPosition(m_ship1Id); + bodyDef.angle = b2Body_GetAngle(m_ship1Id); + //bodyDef.gravityScale = 0.0f; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Circle circle = {{0.0f, 2.0f}, 0.5f}; + b2Body_CreateCircle(bodyId, &b2_defaultShapeDef, &circle); + } + + // Ship 2 obstruction + { + b2BodyDef bodyDef = b2_defaultBodyDef; + bodyDef.type = b2_dynamicBody; + bodyDef.position = b2Body_GetPosition(m_ship2Id); + bodyDef.angle = b2Body_GetAngle(m_ship2Id); + //bodyDef.gravityScale = 0.0f; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + b2Circle circle = {{0.0f, 2.0f}, 0.5f}; + b2Body_CreateCircle(bodyId, &b2_defaultShapeDef, &circle); + } + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); + ImGui::SetNextWindowSize(ImVec2(200.0f, 100.0f)); + ImGui::Begin("Compound Shapes", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize); + + if (ImGui::Button("Intrude")) + { + Spawn(); + } + + ImGui::End(); + } + + static Sample* Create(const Settings& settings) + { + return new CompoundShapes(settings); + } + + b2BodyId m_table1Id; + b2BodyId m_table2Id; + b2BodyId m_ship1Id; + b2BodyId m_ship2Id; +}; + +static int sampleCompoundShape = RegisterSample("Shapes", "Compound Shapes", CompoundShapes::Create); diff --git a/samples/draw.cpp b/samples/draw.cpp index 54a30960..31d358fc 100644 --- a/samples/draw.cpp +++ b/samples/draw.cpp @@ -1278,7 +1278,7 @@ void Draw::DrawSegment(b2Vec2 p1, b2Vec2 p2, b2Color color) void Draw::DrawTransform(b2Transform xf) { - const float k_axisScale = 0.4f; + const float k_axisScale = 0.2f; b2Color red = {1.0f, 0.0f, 0.0f, 1.0f}; b2Color green = {0.0f, 1.0f, 0.0f, 1.0f}; b2Vec2 p1 = xf.p, p2; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6ad74c85..df9dc0f8 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -2,6 +2,8 @@ set(BOX2D_SOURCE_FILES aabb.c allocate.c allocate.h + arena_allocator.c + arena_allocator.h array.c array.h bitset.c @@ -31,6 +33,7 @@ set(BOX2D_SOURCE_FILES joint.h manifold.c math.c + motor_joint.c mouse_joint.c polygon_shape.h pool.c @@ -40,13 +43,12 @@ set(BOX2D_SOURCE_FILES shape.c shape.h solver_data.h - stack_allocator.c - stack_allocator.h table.c table.h timer.c types.c weld_joint.c + wheel_joint.c world.c world.h ) diff --git a/src/stack_allocator.c b/src/arena_allocator.c similarity index 99% rename from src/stack_allocator.c rename to src/arena_allocator.c index 0e0e3a3f..512c8919 100644 --- a/src/stack_allocator.c +++ b/src/arena_allocator.c @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT -#include "stack_allocator.h" +#include "arena_allocator.h" #include "allocate.h" #include "array.h" diff --git a/src/stack_allocator.h b/src/arena_allocator.h similarity index 100% rename from src/stack_allocator.h rename to src/arena_allocator.h diff --git a/src/body.c b/src/body.c index aa5d046a..6542edda 100644 --- a/src/body.c +++ b/src/body.c @@ -343,7 +343,7 @@ void b2World_DestroyBody(b2BodyId bodyId) b2DestroyBody(world, body); } -int32_t b2Body_GetContactCount(b2BodyId bodyId) +int32_t b2Body_GetContactCapacity(b2BodyId bodyId) { b2World* world = b2GetWorldFromIndex(bodyId.world); B2_ASSERT(world->locked == false); @@ -737,6 +737,8 @@ b2ChainId b2Body_CreateChain(b2BodyId bodyId, const b2ChainDef* def) shapeDef.restitution = def->restitution; shapeDef.friction = def->friction; shapeDef.filter = def->filter; + shapeDef.enableContactEvents = false; + shapeDef.enableSensorEvents = false; int32_t n = def->count; const b2Vec2* points = def->points; diff --git a/src/broad_phase.c b/src/broad_phase.c index dff4aefd..5d8ffeab 100644 --- a/src/broad_phase.c +++ b/src/broad_phase.c @@ -6,12 +6,12 @@ #include "broad_phase.h" #include "allocate.h" +#include "arena_allocator.h" #include "array.h" #include "body.h" #include "contact.h" #include "core.h" #include "shape.h" -#include "stack_allocator.h" #include "world.h" #include "box2d/aabb.h" @@ -21,17 +21,17 @@ #include #include -//#include +// #include -//static FILE* s_file = NULL; +// static FILE* s_file = NULL; void b2CreateBroadPhase(b2BroadPhase* bp) { - //if (s_file == NULL) + // if (s_file == NULL) //{ // s_file = fopen("pairs01.txt", "a"); // fprintf(s_file, "============\n\n"); - //} + // } bp->proxyCount = 0; @@ -66,11 +66,11 @@ void b2DestroyBroadPhase(b2BroadPhase* bp) memset(bp, 0, sizeof(b2BroadPhase)); - //if (s_file != NULL) + // if (s_file != NULL) //{ // fclose(s_file); // s_file = NULL; - //} + // } } static inline void b2UnBufferMove(b2BroadPhase* bp, int32_t proxyKey) @@ -357,7 +357,7 @@ void b2UpdateBroadPhasePairs(b2World* world) // - Create contacts in deterministic order b2Shape* shapes = world->shapes; - //int32_t pairCount = 0; + // int32_t pairCount = 0; for (int32_t i = 0; i < moveCount; ++i) { @@ -374,10 +374,10 @@ void b2UpdateBroadPhasePairs(b2World* world) int32_t shapeIndexA = pair->shapeIndexA; int32_t shapeIndexB = pair->shapeIndexB; - //if (s_file != NULL) + // if (s_file != NULL) //{ // fprintf(s_file, "%d %d\n", shapeIndexA, shapeIndexB); - //} + // } //++pairCount; @@ -398,16 +398,16 @@ void b2UpdateBroadPhasePairs(b2World* world) } } - //if (s_file != NULL) + // if (s_file != NULL) //{ // fprintf(s_file, "\n"); - //} + // } } - //if (s_file != NULL) + // if (s_file != NULL) //{ // fprintf(s_file, "count = %d\n\n", pairCount); - //} + // } // Reset move buffer b2Array_Clear(bp->moveArray); diff --git a/src/distance_joint.c b/src/distance_joint.c index 0a6afb61..085fc190 100644 --- a/src/distance_joint.c +++ b/src/distance_joint.c @@ -28,7 +28,7 @@ // K = J * invM * JT // = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 -void b2PrepareDistance(b2Joint* base, b2StepContext* context) +void b2PrepareDistanceJoint(b2Joint* base, b2StepContext* context) { B2_ASSERT(base->type == b2_distanceJoint); @@ -117,7 +117,7 @@ void b2PrepareDistance(b2Joint* base, b2StepContext* context) } } -void b2WarmStartDistance(b2Joint* base, b2StepContext* context) +void b2WarmStartDistanceJoint(b2Joint* base, b2StepContext* context) { B2_ASSERT(base->type == b2_distanceJoint); @@ -149,7 +149,7 @@ void b2WarmStartDistance(b2Joint* base, b2StepContext* context) bodyB->angularVelocity += iB * b2Cross(rB, P); } -void b2SolveDistanceVelocity(b2Joint* base, b2StepContext* context, bool useBias) +void b2SolveDistanceJoint(b2Joint* base, b2StepContext* context, bool useBias) { B2_ASSERT(base->type == b2_distanceJoint); diff --git a/src/geometry.c b/src/geometry.c index a2f49b20..2cfcea64 100644 --- a/src/geometry.c +++ b/src/geometry.c @@ -238,8 +238,13 @@ b2MassData b2ComputeCapsuleMass(const b2Capsule* shape, float density) // m * ((h + lc)^2 - lc^2) = m * (h^2 + 2 * h * lc) // See: https://en.wikipedia.org/wiki/Parallel_axis_theorem // I verified this formula by computing the convex hull of a 128 vertex capsule + + // half circle centroid float lc = 4.0f * radius / (3.0f * b2_pi); + + // half length of rectangular portion of capsule float h = 0.5f * length; + float circleInertia = circleMass * (0.5f * rr + h * h + 2.0f * h * lc); float boxInertia = boxMass * (4.0f * rr + ll) / 12.0f; massData.I = circleInertia + boxInertia; diff --git a/src/graph.c b/src/graph.c index 869aaa43..6175080e 100644 --- a/src/graph.c +++ b/src/graph.c @@ -4,6 +4,7 @@ #include "graph.h" #include "allocate.h" +#include "arena_allocator.h" #include "array.h" #include "body.h" #include "contact.h" @@ -12,13 +13,11 @@ #include "joint.h" #include "shape.h" #include "solver_data.h" -#include "stack_allocator.h" #include "world.h" +#include "x86/sse2.h" #include "box2d/aabb.h" -#include "x86/sse2.h" - #include #include #include @@ -28,10 +27,10 @@ // 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. +// 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 making all constraints be assigned to overflow. #define B2_FORCE_OVERFLOW 0 @@ -539,7 +538,7 @@ static void b2SolveJoints(int32_t startIndex, int32_t endIndex, b2SolverTaskCont b2Joint* joint = joints + index; B2_ASSERT(b2ObjectValid(&joint->object) == true); - b2SolveJointVelocity(joint, stepContext, useBias); + b2SolveJoint(joint, stepContext, useBias); } b2TracyCZoneEnd(solve_joints); @@ -702,7 +701,7 @@ static void b2FinalizeBodiesTask(int32_t startIndex, int32_t endIndex, uint32_t b2SetBit(shapeBitSet, shapeIndex); } } - + shapeIndex = shape->nextShapeIndex; } @@ -833,7 +832,8 @@ static inline int32_t GetWorkerStartIndex(int32_t workerIndex, int32_t blockCoun return blocksPerWorker * workerIndex + B2_MIN(remainder, workerIndex); } -static void b2ExecuteStage(b2SolverStage* stage, b2SolverTaskContext* context, int previousSyncIndex, int syncIndex, int32_t workerIndex) +static void b2ExecuteStage(b2SolverStage* stage, b2SolverTaskContext* context, int previousSyncIndex, int syncIndex, + int32_t workerIndex) { int32_t completedCount = 0; b2SolverBlock* blocks = stage->blocks; @@ -1144,7 +1144,8 @@ static void b2SolveGraph(b2World* world, b2StepContext* stepContext) // Reserve space for awake bodies b2Body* bodies = world->bodies; b2Body** awakeBodies = b2AllocateStackItem(world->stackAllocator, awakeBodyCount * sizeof(b2Body*), "awake bodies"); - b2SolverBody* solverBodies = b2AllocateStackItem(world->stackAllocator, awakeBodyCount * sizeof(b2SolverBody), "solver bodies"); + b2SolverBody* solverBodies = + b2AllocateStackItem(world->stackAllocator, awakeBodyCount * sizeof(b2SolverBody), "solver bodies"); // Map from solver body to body // TODO_ERIN have body directly reference solver body for user access? @@ -1302,8 +1303,8 @@ static void b2SolveGraph(b2World* world, b2StepContext* stepContext) int32_t overflowContactCount = b2Array(graph->overflow.contactArray).count; graph->occupancy[b2_overflowIndex] = overflowContactCount; - graph->overflow.contactConstraints = - b2AllocateStackItem(world->stackAllocator, overflowContactCount * sizeof(b2ContactConstraint), "overflow contact constraint"); + graph->overflow.contactConstraints = b2AllocateStackItem( + world->stackAllocator, overflowContactCount * sizeof(b2ContactConstraint), "overflow contact constraint"); // Distribute transient constraints to each graph color { @@ -1342,7 +1343,6 @@ static void b2SolveGraph(b2World* world, b2StepContext* stepContext) base += colorContactCountSIMD; } - } // Define work blocks for preparing contacts and storing contact impulses @@ -1398,9 +1398,12 @@ static void b2SolveGraph(b2World* world, b2StepContext* stepContext) b2SolverStage* stages = b2AllocateStackItem(world->stackAllocator, stageCount * sizeof(b2SolverStage), "stages"); b2SolverBlock* bodyBlocks = b2AllocateStackItem(world->stackAllocator, bodyBlockCount * sizeof(b2SolverBlock), "body blocks"); - b2SolverBlock* contactBlocks = b2AllocateStackItem(world->stackAllocator, contactBlockCount * sizeof(b2SolverBlock), "contact blocks"); - b2SolverBlock* jointBlocks = b2AllocateStackItem(world->stackAllocator, jointBlockCount * sizeof(b2SolverBlock), "joint blocks"); - b2SolverBlock* graphBlocks = b2AllocateStackItem(world->stackAllocator, graphBlockCount * sizeof(b2SolverBlock), "graph blocks"); + b2SolverBlock* contactBlocks = + b2AllocateStackItem(world->stackAllocator, contactBlockCount * sizeof(b2SolverBlock), "contact blocks"); + b2SolverBlock* jointBlocks = + b2AllocateStackItem(world->stackAllocator, jointBlockCount * sizeof(b2SolverBlock), "joint blocks"); + b2SolverBlock* graphBlocks = + b2AllocateStackItem(world->stackAllocator, graphBlockCount * sizeof(b2SolverBlock), "graph blocks"); // Split an awake island. This modifies: // - stack allocator @@ -1488,7 +1491,8 @@ static void b2SolveGraph(b2World* world, b2StepContext* stepContext) if (colorJointBlockCount > 0) { - baseGraphBlock[colorJointBlockCount - 1].count = (int16_t)(colorJointCounts[i] - (colorJointBlockCount - 1) * colorJointBlockSize); + baseGraphBlock[colorJointBlockCount - 1].count = + (int16_t)(colorJointCounts[i] - (colorJointBlockCount - 1) * colorJointBlockSize); baseGraphBlock += colorJointBlockCount; } @@ -1505,7 +1509,8 @@ static void b2SolveGraph(b2World* world, b2StepContext* stepContext) if (colorContactBlockCount > 0) { - baseGraphBlock[colorContactBlockCount - 1].count = (int16_t)(colorContactCounts[i] - (colorContactBlockCount - 1) * colorContactBlockSize); + baseGraphBlock[colorContactBlockCount - 1].count = + (int16_t)(colorContactCounts[i] - (colorContactBlockCount - 1) * colorContactBlockSize); baseGraphBlock += colorContactBlockCount; } } @@ -1665,7 +1670,7 @@ static void b2SolveGraph(b2World* world, b2StepContext* stepContext) world->finishTaskFcn(splitIslandTask, world->userTaskContext); world->activeTaskCount -= 1; } - + world->splitIslandIndex = B2_NULL_INDEX; // Finish solve diff --git a/src/island.c b/src/island.c index cb0d3010..3acd0e5b 100644 --- a/src/island.c +++ b/src/island.c @@ -3,6 +3,7 @@ #include "island.h" +#include "arena_allocator.h" #include "array.h" #include "body.h" #include "contact.h" @@ -11,7 +12,6 @@ #include "joint.h" #include "shape.h" #include "solver_data.h" -#include "stack_allocator.h" #include "world.h" #include "box2d/aabb.h" @@ -1019,15 +1019,15 @@ void b2ValidateIsland(b2Island* island, bool checkSleep) B2_ASSERT(contact->colorIndex != B2_NULL_INDEX); B2_ASSERT(contact->colorSubIndex != B2_NULL_INDEX); - //int32_t awakeIndex = world->contactAwakeIndexArray[contactIndex]; - //B2_ASSERT(0 <= awakeIndex && awakeIndex < b2Array(world->awakeContactArray).count); - //B2_ASSERT(world->awakeContactArray[awakeIndex] == contactIndex); + // int32_t awakeIndex = world->contactAwakeIndexArray[contactIndex]; + // B2_ASSERT(0 <= awakeIndex && awakeIndex < b2Array(world->awakeContactArray).count); + // B2_ASSERT(world->awakeContactArray[awakeIndex] == contactIndex); } else { B2_ASSERT(contact->colorIndex == B2_NULL_INDEX); B2_ASSERT(contact->colorSubIndex == B2_NULL_INDEX); - //B2_ASSERT(world->contactAwakeIndexArray[contactIndex] == B2_NULL_INDEX); + // B2_ASSERT(world->contactAwakeIndexArray[contactIndex] == B2_NULL_INDEX); } } diff --git a/src/joint.c b/src/joint.c index 1780e94b..c4dcc152 100644 --- a/src/joint.c +++ b/src/joint.c @@ -153,6 +153,7 @@ static b2Joint* b2CreateJoint(b2World* world, b2Body* bodyA, b2Body* bodyB) joint->colorIndex = B2_NULL_INDEX; joint->colorSubIndex = B2_NULL_INDEX; + joint->drawSize = 1.0f; joint->isMarked = false; if ((bodyA->type == b2_dynamicBody || bodyB->type == b2_dynamicBody) && bodyA->isEnabled == true && bodyB->isEnabled == true) @@ -244,7 +245,41 @@ b2JointId b2World_CreateDistanceJoint(b2WorldId worldId, const b2DistanceJointDe } b2JointId jointId = {joint->object.index, world->index, joint->object.revision}; + return jointId; +} + +b2JointId b2World_CreateMotorJoint(b2WorldId worldId, const b2MotorJointDef* def) +{ + b2World* world = b2GetWorldFromId(worldId); + B2_ASSERT(world->locked == false); + + if (world->locked) + { + return b2_nullJointId; + } + + B2_ASSERT(b2IsBodyIdValid(world, def->bodyIdA)); + B2_ASSERT(b2IsBodyIdValid(world, def->bodyIdB)); + + b2Body* bodyA = world->bodies + def->bodyIdA.index; + b2Body* bodyB = world->bodies + def->bodyIdB.index; + + b2Joint* joint = b2CreateJoint(world, bodyA, bodyB); + + joint->type = b2_motorJoint; + joint->localAnchorA = (b2Vec2){0.0f, 0.0f}; + joint->localAnchorB = (b2Vec2){0.0f, 0.0f}; + joint->collideConnected = true; + + joint->motorJoint = (b2MotorJoint){0}; + joint->motorJoint.linearOffset = def->linearOffset; + joint->motorJoint.angularOffset = def->angularOffset; + joint->motorJoint.maxForce = def->maxForce; + joint->motorJoint.maxTorque = def->maxTorque; + joint->motorJoint.correctionFactor = B2_CLAMP(def->correctionFactor, 0.0f, 1.0f); + + b2JointId jointId = {joint->object.index, world->index, joint->object.revision}; return jointId; } @@ -280,7 +315,6 @@ b2JointId b2World_CreateMouseJoint(b2WorldId worldId, const b2MouseJointDef* def joint->mouseJoint.damping = def->damping; b2JointId jointId = {joint->object.index, world->index, joint->object.revision}; - return jointId; } @@ -307,12 +341,13 @@ b2JointId b2World_CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDe joint->localAnchorA = def->localAnchorA; joint->localAnchorB = def->localAnchorB; joint->collideConnected = def->collideConnected; + joint->drawSize = def->drawSize; b2RevoluteJoint empty = {0}; joint->revoluteJoint = empty; joint->revoluteJoint.referenceAngle = def->referenceAngle; - joint->revoluteJoint.impulse = b2Vec2_zero; + joint->revoluteJoint.linearImpulse = b2Vec2_zero; joint->revoluteJoint.axialMass = 0.0f; joint->revoluteJoint.motorImpulse = 0.0f; joint->revoluteJoint.lowerImpulse = 0.0f; @@ -331,7 +366,6 @@ b2JointId b2World_CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDe } b2JointId jointId = {joint->object.index, world->index, joint->object.revision}; - return jointId; } @@ -383,7 +417,6 @@ b2JointId b2World_CreatePrismaticJoint(b2WorldId worldId, const b2PrismaticJoint } b2JointId jointId = {joint->object.index, world->index, joint->object.revision}; - return jointId; } @@ -418,8 +451,8 @@ b2JointId b2World_CreateWeldJoint(b2WorldId worldId, const b2WeldJointDef* def) joint->weldJoint.linearDampingRatio = def->linearDampingRatio; joint->weldJoint.angularHertz = def->angularHertz; joint->weldJoint.angularDampingRatio = def->angularDampingRatio; - joint->weldJoint.pivotImpulse = b2Vec2_zero; - joint->weldJoint.axialImpulse = 0.0f; + joint->weldJoint.linearImpulse = b2Vec2_zero; + joint->weldJoint.angularImpulse = 0.0f; // If the joint prevents collisions, then destroy all contacts between attached bodies if (def->collideConnected == false) @@ -428,7 +461,58 @@ b2JointId b2World_CreateWeldJoint(b2WorldId worldId, const b2WeldJointDef* def) } b2JointId jointId = {joint->object.index, world->index, joint->object.revision}; + return jointId; +} +b2JointId b2World_CreateWheelJoint(b2WorldId worldId, const b2WheelJointDef* def) +{ + b2World* world = b2GetWorldFromId(worldId); + + B2_ASSERT(world->locked == false); + + if (world->locked) + { + return b2_nullJointId; + } + + B2_ASSERT(b2IsBodyIdValid(world, def->bodyIdA)); + B2_ASSERT(b2IsBodyIdValid(world, def->bodyIdB)); + + b2Body* bodyA = world->bodies + def->bodyIdA.index; + b2Body* bodyB = world->bodies + def->bodyIdB.index; + + b2Joint* joint = b2CreateJoint(world, bodyA, bodyB); + + joint->type = b2_wheelJoint; + joint->localAnchorA = def->localAnchorA; + joint->localAnchorB = def->localAnchorB; + joint->collideConnected = def->collideConnected; + + // todo test this + joint->wheelJoint = (b2WheelJoint){0}; + + joint->wheelJoint.localAxisA = b2Normalize(def->localAxisA); + joint->wheelJoint.perpMass = 0.0f; + joint->wheelJoint.axialMass = 0.0f; + joint->wheelJoint.motorImpulse = 0.0f; + joint->wheelJoint.lowerImpulse = 0.0f; + joint->wheelJoint.upperImpulse = 0.0f; + joint->wheelJoint.lowerTranslation = def->lowerTranslation; + joint->wheelJoint.upperTranslation = def->upperTranslation; + joint->wheelJoint.maxMotorTorque = def->maxMotorTorque; + joint->wheelJoint.motorSpeed = def->motorSpeed; + joint->wheelJoint.stiffness = def->stiffness; + joint->wheelJoint.damping = def->damping; + joint->wheelJoint.enableLimit = def->enableLimit; + joint->wheelJoint.enableMotor = def->enableMotor; + + // 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, world->index, joint->object.revision}; return jointId; } @@ -550,34 +634,44 @@ b2BodyId b2Joint_GetBodyB(b2JointId jointId) return bodyId; } -extern void b2PrepareDistance(b2Joint* base, b2StepContext* context); -extern void b2PrepareMouse(b2Joint* base, b2StepContext* context); -extern void b2PreparePrismatic(b2Joint* base, b2StepContext* context); -extern void b2PrepareRevolute(b2Joint* base, b2StepContext* context); -extern void b2PrepareWeld(b2Joint* base, b2StepContext* context); +extern void b2PrepareDistanceJoint(b2Joint* base, b2StepContext* context); +extern void b2PrepareMotorJoint(b2Joint* base, b2StepContext* context); +extern void b2PrepareMouseJoint(b2Joint* base, b2StepContext* context); +extern void b2PreparePrismaticJoint(b2Joint* base, b2StepContext* context); +extern void b2PrepareRevoluteJoint(b2Joint* base, b2StepContext* context); +extern void b2PrepareWeldJoint(b2Joint* base, b2StepContext* context); +extern void b2PrepareWheelJoint(b2Joint* base, b2StepContext* context); void b2PrepareJoint(b2Joint* joint, b2StepContext* context) { switch (joint->type) { case b2_distanceJoint: - b2PrepareDistance(joint, context); + b2PrepareDistanceJoint(joint, context); + break; + + case b2_motorJoint: + b2PrepareMotorJoint(joint, context); break; case b2_mouseJoint: - b2PrepareMouse(joint, context); + b2PrepareMouseJoint(joint, context); break; case b2_prismaticJoint: - b2PreparePrismatic(joint, context); + b2PreparePrismaticJoint(joint, context); break; case b2_revoluteJoint: - b2PrepareRevolute(joint, context); + b2PrepareRevoluteJoint(joint, context); break; case b2_weldJoint: - b2PrepareWeld(joint, context); + b2PrepareWeldJoint(joint, context); + break; + + case b2_wheelJoint: + b2PrepareWheelJoint(joint, context); break; default: @@ -585,34 +679,44 @@ void b2PrepareJoint(b2Joint* joint, b2StepContext* context) } } -extern void b2WarmStartDistance(b2Joint* base, b2StepContext* context); -extern void b2WarmStartMouse(b2Joint* base, b2StepContext* context); -extern void b2WarmStartPrismatic(b2Joint* base, b2StepContext* context); -extern void b2WarmStartRevolute(b2Joint* base, b2StepContext* context); -extern void b2WarmStartWeld(b2Joint* base, b2StepContext* context); +extern void b2WarmStartDistanceJoint(b2Joint* base, b2StepContext* context); +extern void b2WarmStartMotorJoint(b2Joint* base, b2StepContext* context); +extern void b2WarmStartMouseJoint(b2Joint* base, b2StepContext* context); +extern void b2WarmStartPrismaticJoint(b2Joint* base, b2StepContext* context); +extern void b2WarmStartRevoluteJoint(b2Joint* base, b2StepContext* context); +extern void b2WarmStartWeldJoint(b2Joint* base, b2StepContext* context); +extern void b2WarmStartWheelJoint(b2Joint* base, b2StepContext* context); void b2WarmStartJoint(b2Joint* joint, b2StepContext* context) { switch (joint->type) { case b2_distanceJoint: - b2WarmStartDistance(joint, context); + b2WarmStartDistanceJoint(joint, context); + break; + + case b2_motorJoint: + b2WarmStartMotorJoint(joint, context); break; case b2_mouseJoint: - b2WarmStartMouse(joint, context); + b2WarmStartMouseJoint(joint, context); break; case b2_prismaticJoint: - b2WarmStartPrismatic(joint, context); + b2WarmStartPrismaticJoint(joint, context); break; case b2_revoluteJoint: - b2WarmStartRevolute(joint, context); + b2WarmStartRevoluteJoint(joint, context); break; case b2_weldJoint: - b2WarmStartWeld(joint, context); + b2WarmStartWeldJoint(joint, context); + break; + + case b2_wheelJoint: + b2WarmStartWheelJoint(joint, context); break; default: @@ -620,37 +724,47 @@ void b2WarmStartJoint(b2Joint* joint, b2StepContext* context) } } -extern void b2SolveDistanceVelocity(b2Joint* base, b2StepContext* context, bool useBias); -extern void b2SolveMouseVelocity(b2Joint* base, b2StepContext* context); -extern void b2SolvePrismaticVelocity(b2Joint* base, b2StepContext* context, bool useBias); -extern void b2SolveRevoluteVelocity(b2Joint* base, b2StepContext* context, bool useBias); -extern void b2SolveWeldVelocity(b2Joint* base, b2StepContext* context, bool useBias); +extern void b2SolveDistanceJoint(b2Joint* base, b2StepContext* context, bool useBias); +extern void b2SolveMotorJoint(b2Joint* base, b2StepContext* context, bool useBias); +extern void b2SolveMouseJoint(b2Joint* base, b2StepContext* context); +extern void b2SolvePrismaticJoint(b2Joint* base, b2StepContext* context, bool useBias); +extern void b2SolveRevoluteJoint(b2Joint* base, b2StepContext* context, bool useBias); +extern void b2SolveWeldJoint(b2Joint* base, b2StepContext* context, bool useBias); +extern void b2SolveWheelJoint(b2Joint* base, b2StepContext* context, bool useBias); -void b2SolveJointVelocity(b2Joint* joint, b2StepContext* context, bool useBias) +void b2SolveJoint(b2Joint* joint, b2StepContext* context, bool useBias) { switch (joint->type) { case b2_distanceJoint: - b2SolveDistanceVelocity(joint, context, useBias); + b2SolveDistanceJoint(joint, context, useBias); + break; + + case b2_motorJoint: + b2SolveMotorJoint(joint, context, useBias); break; case b2_mouseJoint: if (useBias) { - b2SolveMouseVelocity(joint, context); + b2SolveMouseJoint(joint, context); } break; case b2_prismaticJoint: - b2SolvePrismaticVelocity(joint, context, useBias); + b2SolvePrismaticJoint(joint, context, useBias); break; case b2_revoluteJoint: - b2SolveRevoluteVelocity(joint, context, useBias); + b2SolveRevoluteJoint(joint, context, useBias); break; case b2_weldJoint: - b2SolveWeldVelocity(joint, context, useBias); + b2SolveWeldJoint(joint, context, useBias); + break; + + case b2_wheelJoint: + b2SolveWheelJoint(joint, context, useBias); break; default: @@ -708,7 +822,7 @@ void b2SolveOverflowJoints(b2SolverTaskContext* context, bool useBias) b2Joint* joint = joints + index; B2_ASSERT(b2ObjectValid(&joint->object) == true); - b2SolveJointVelocity(joint, stepContext, useBias); + b2SolveJoint(joint, stepContext, useBias); } b2TracyCZoneEnd(solve_joints); @@ -763,6 +877,7 @@ b2JointId b2Body_GetNextJoint(b2BodyId bodyId, b2JointId jointId) extern void b2DrawDistance(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bodyB); extern void b2DrawPrismatic(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bodyB); extern void b2DrawRevolute(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bodyB); +extern void b2DrawWheelJoint(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bodyB); void b2DrawJoint(b2DebugDraw* draw, b2World* world, b2Joint* joint) { @@ -818,6 +933,10 @@ void b2DrawJoint(b2DebugDraw* draw, b2World* world, b2Joint* joint) b2DrawRevolute(draw, joint, bodyA, bodyB); break; + case b2_wheelJoint: + b2DrawWheelJoint(draw, joint, bodyA, bodyB); + break; + default: draw->DrawSegment(xfA.p, pA, color, draw->context); draw->DrawSegment(pA, pB, color, draw->context); diff --git a/src/joint.h b/src/joint.h index ec9cb92c..ac078d6a 100644 --- a/src/joint.h +++ b/src/joint.h @@ -15,12 +15,9 @@ typedef struct b2World b2World; typedef enum b2JointType { b2_distanceJoint, - b2_frictionJoint, - b2_gearJoint, b2_motorJoint, b2_mouseJoint, b2_prismaticJoint, - b2_pulleyJoint, b2_revoluteJoint, b2_weldJoint, b2_wheelJoint, @@ -66,6 +63,28 @@ typedef struct b2DistanceJoint float axialMass; } b2DistanceJoint; +typedef struct b2MotorJoint +{ + // Solver shared + b2Vec2 linearOffset; + float angularOffset; + b2Vec2 linearImpulse; + float angularImpulse; + float maxForce; + float maxTorque; + float correctionFactor; + + // Solver temp + int32_t indexA; + int32_t indexB; + b2Vec2 rA; + b2Vec2 rB; + b2Vec2 linearSeparation; + float angularSeparation; + b2Mat22 linearMass; + float angularMass; +} b2MotorJoint; + typedef struct b2MouseJoint { b2Vec2 targetA; @@ -87,66 +106,69 @@ typedef struct b2MouseJoint b2Vec2 C; } b2MouseJoint; -typedef struct b2RevoluteJoint +typedef struct b2PrismaticJoint { // Solver shared + b2Vec2 localAxisA; b2Vec2 impulse; float motorImpulse; float lowerImpulse; float upperImpulse; bool enableMotor; - float maxMotorTorque; + float maxMotorForce; float motorSpeed; bool enableLimit; float referenceAngle; - float lowerAngle; - float upperAngle; + float lowerTranslation; + float upperTranslation; // Solver temp int32_t indexA; int32_t indexB; - float angleA; - float angleB; b2Vec2 rA; b2Vec2 rB; - b2Vec2 separation; + b2Vec2 axisA; + b2Vec2 pivotSeparation; + float angleSeparation; b2Mat22 pivotMass; + float axialMass; float biasCoefficient; float massCoefficient; float impulseCoefficient; - float axialMass; -} b2RevoluteJoint; +} b2PrismaticJoint; -typedef struct b2PrismaticJoint +typedef struct b2RevoluteJoint { // Solver shared - b2Vec2 localAxisA; - b2Vec2 impulse; + b2Vec2 linearImpulse; float motorImpulse; float lowerImpulse; float upperImpulse; bool enableMotor; - float maxMotorForce; + float maxMotorTorque; float motorSpeed; bool enableLimit; float referenceAngle; - float lowerTranslation; - float upperTranslation; + float lowerAngle; + float upperAngle; // Solver temp int32_t indexA; int32_t indexB; - b2Vec2 positionA; - b2Vec2 positionB; float angleA; float angleB; - b2Vec2 localCenterA; - b2Vec2 localCenterB; + b2Vec2 rA; + b2Vec2 rB; + b2Vec2 separation; + b2Mat22 pivotMass; + float limitBiasCoefficient; + float limitMassCoefficient; + float limitImpulseCoefficient; float biasCoefficient; float massCoefficient; float impulseCoefficient; float axialMass; -} b2PrismaticJoint; +} b2RevoluteJoint; typedef struct b2WeldJoint { @@ -162,8 +184,8 @@ typedef struct b2WeldJoint float angularBiasCoefficient; float angularMassCoefficient; float angularImpulseCoefficient; - b2Vec2 pivotImpulse; - float axialImpulse; + b2Vec2 linearImpulse; + float angularImpulse; // Solver temp int32_t indexA; @@ -176,6 +198,42 @@ typedef struct b2WeldJoint float axialMass; } b2WeldJoint; +typedef struct b2WheelJoint +{ + // Solver shared + b2Vec2 localAxisA; + float perpImpulse; + float motorImpulse; + float springImpulse; + float lowerImpulse; + float upperImpulse; + float maxMotorTorque; + float motorSpeed; + float lowerTranslation; + float upperTranslation; + float stiffness; + float damping; + bool enableMotor; + bool enableLimit; + + // Solver temp + int32_t indexA; + int32_t indexB; + b2Vec2 rA; + b2Vec2 rB; + b2Vec2 axisA; + b2Vec2 pivotSeparation; + float perpMass; + float motorMass; + float axialMass; + float springMass; + float bias; + float gamma; + float biasCoefficient; + float massCoefficient; + float impulseCoefficient; +} b2WheelJoint; + /// The base joint class. Joints are used to constraint two bodies together in /// various fashions. Some joints also feature limits and motors. typedef struct b2Joint @@ -200,24 +258,29 @@ typedef struct b2Joint union { b2DistanceJoint distanceJoint; + b2MotorJoint motorJoint; b2MouseJoint mouseJoint; b2RevoluteJoint revoluteJoint; b2PrismaticJoint prismaticJoint; b2WeldJoint weldJoint; + b2WheelJoint wheelJoint; }; + float drawSize; bool isMarked; bool collideConnected; } b2Joint; +b2Joint* b2GetJoint(b2World* world, b2JointId jointId); + +// todo remove this +b2Joint* b2GetJointCheckType(b2JointId id, b2JointType type); + void b2PrepareJoint(b2Joint* joint, b2StepContext* context); void b2WarmStartJoint(b2Joint* joint, b2StepContext* context); -void b2SolveJointVelocity(b2Joint* joint, b2StepContext* context, bool useBias); +void b2SolveJoint(b2Joint* joint, b2StepContext* context, bool useBias); void b2PrepareAndWarmStartOverflowJoints(b2SolverTaskContext* context); void b2SolveOverflowJoints(b2SolverTaskContext* context, bool useBias); void b2DrawJoint(b2DebugDraw* draw, b2World* world, b2Joint* joint); - -// Get joint from id with validation -b2Joint* b2GetJointCheckType(b2JointId id, b2JointType type); diff --git a/src/motor_joint.c b/src/motor_joint.c new file mode 100644 index 00000000..dccdbcf8 --- /dev/null +++ b/src/motor_joint.c @@ -0,0 +1,310 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver_data.h" +#include "world.h" + +#include "box2d/debug_draw.h" + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Angle constraint +// C = angle2 - angle1 - referenceAngle +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +void b2PrepareMotorJoint(b2Joint* base, b2StepContext* context) +{ + B2_ASSERT(base->type == b2_motorJoint); + + int32_t indexA = base->edges[0].bodyIndex; + int32_t indexB = base->edges[1].bodyIndex; + B2_ASSERT(0 <= indexA && indexA < context->bodyCapacity); + B2_ASSERT(0 <= indexB && indexB < context->bodyCapacity); + + b2Body* bodyA = context->bodies + indexA; + b2Body* bodyB = context->bodies + indexB; + B2_ASSERT(bodyA->object.index == bodyA->object.next); + B2_ASSERT(bodyB->object.index == bodyB->object.next); + + float mA = bodyA->invMass; + float iA = bodyA->invI; + float mB = bodyB->invMass; + float iB = bodyB->invI; + + b2MotorJoint* joint = &base->motorJoint; + joint->indexA = context->bodyToSolverMap[indexA]; + joint->indexB = context->bodyToSolverMap[indexB]; + + joint->rA = b2RotateVector(bodyA->transform.q, b2Sub(base->localAnchorA, bodyA->localCenter)); + joint->rB = b2RotateVector(bodyB->transform.q, b2Sub(base->localAnchorB, bodyB->localCenter)); + joint->linearSeparation = b2Sub(b2Add(b2Sub(joint->rB, joint->rA), b2Sub(bodyB->position, bodyA->position)), joint->linearOffset); + joint->angularSeparation = bodyB->angle - bodyA->angle - joint->angularOffset; + + b2Vec2 rA = joint->rA; + b2Vec2 rB = joint->rB; + + b2Mat22 K; + K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; + K.cx.y = -rA.y * rA.x * iA - rB.y * rB.x * iB; + K.cy.x = K.cx.y; + K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; + joint->linearMass = b2GetInverse22(K); + + float ka = iA + iB; + joint->angularMass = ka > 0.0f ? 1.0f / ka : 0.0f; + + if (context->enableWarmStarting) + { + float dtRatio = context->dtRatio; + joint->linearImpulse.x *= dtRatio; + joint->linearImpulse.y *= dtRatio; + joint->angularImpulse *= dtRatio; + } + else + { + joint->linearImpulse = b2Vec2_zero; + joint->angularImpulse = 0.0f; + } +} + +void b2WarmStartMotorJoint(b2Joint* base, b2StepContext* context) +{ + b2MotorJoint* joint = &base->motorJoint; + + b2SolverBody* bodyA = context->solverBodies + joint->indexA; + b2Vec2 vA = bodyA->linearVelocity; + float wA = bodyA->angularVelocity; + float mA = bodyA->invMass; + float iA = bodyA->invI; + + b2SolverBody* bodyB = context->solverBodies + joint->indexB; + b2Vec2 vB = bodyB->linearVelocity; + float wB = bodyB->angularVelocity; + float mB = bodyB->invMass; + float iB = bodyB->invI; + + vA = b2MulSub(vA, mA, joint->linearImpulse); + wA -= iA * (b2Cross(joint->rA, joint->linearImpulse) + joint->angularImpulse); + + vB = b2MulAdd(vB, mB, joint->linearImpulse); + wB += iB * (b2Cross(joint->rB, joint->linearImpulse) + joint->angularImpulse); + + bodyA->linearVelocity = vA; + bodyA->angularVelocity = wA; + bodyB->linearVelocity = vB; + bodyB->angularVelocity = wB; +} + +void b2SolveMotorJoint(b2Joint* base, const b2StepContext* context, bool useBias) +{ + if (useBias == false) + { + return; + } + + B2_ASSERT(base->type == b2_motorJoint); + + b2MotorJoint* joint = &base->motorJoint; + + // This is a dummy body to represent a static body since static bodies don't have a solver body. + b2SolverBody dummyBody = {0}; + + b2SolverBody* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexA; + b2Vec2 vA = bodyA->linearVelocity; + float wA = bodyA->angularVelocity; + float mA = bodyA->invMass; + float iA = bodyA->invI; + + b2SolverBody* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexB; + b2Vec2 vB = bodyB->linearVelocity; + float wB = bodyB->angularVelocity; + float mB = bodyB->invMass; + float iB = bodyB->invI; + + // Approximate change in anchors + // small angle approximation of sin(delta_angle) == delta_angle, cos(delta_angle) == 1 + b2Vec2 drA = b2CrossSV(bodyA->deltaAngle, joint->rA); + b2Vec2 drB = b2CrossSV(bodyB->deltaAngle, joint->rB); + + b2Vec2 rA = b2Add(joint->rA, drA); + b2Vec2 rB = b2Add(joint->rB, drB); + + b2Vec2 ds = b2Add(b2Sub(bodyB->deltaPosition, bodyA->deltaPosition), b2Sub(drB, drA)); + b2Vec2 linearSeparation = b2Add(joint->linearSeparation, ds); + b2Vec2 linearBias = b2MulSV(context->inv_dt * joint->correctionFactor, linearSeparation); + + float angularSeperation = joint->angularSeparation + bodyB->deltaAngle - bodyA->deltaAngle; + float angularBias = context->inv_dt * joint->correctionFactor * angularSeperation; + + // Note: don't relax user softness + + // Axial constraint + { + float Cdot = wB - wA; + float impulse = -joint->angularMass * (Cdot + angularBias); + + float oldImpulse = joint->angularImpulse; + float maxImpulse = context->dt * joint->maxTorque; + joint->angularImpulse = B2_CLAMP(joint->angularImpulse + impulse, -maxImpulse, maxImpulse); + impulse = joint->angularImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Linear constraint + { + b2Vec2 Cdot = b2Sub(b2Add(vB, b2CrossSV(wB, rB)), b2Add(vA, b2CrossSV(wA, rA))); + b2Vec2 b = b2MulMV(joint->linearMass, b2Add(Cdot, linearBias)); + b2Vec2 impulse = {-b.x, -b.y}; + + b2Vec2 oldImpulse = joint->linearImpulse; + float maxImpulse = context->dt * joint->maxForce; + joint->linearImpulse = b2Add(joint->linearImpulse, impulse); + + if (b2LengthSquared(joint->linearImpulse) > maxImpulse * maxImpulse) + { + joint->linearImpulse = b2Normalize(joint->linearImpulse); + joint->linearImpulse.x *= maxImpulse; + joint->linearImpulse.y *= maxImpulse; + } + + impulse = b2Sub(joint->linearImpulse, oldImpulse); + + vA = b2MulSub(vA, mA, impulse); + wA -= iA * b2Cross(rA, impulse); + + vB = b2MulAdd(vB, mB, impulse); + wB += iB * b2Cross(rB, impulse); + } + + bodyA->linearVelocity = vA; + bodyA->angularVelocity = wA; + bodyB->linearVelocity = vB; + bodyB->angularVelocity = wB; +} + +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); + + joint->motorJoint.linearOffset = linearOffset; +} + +void b2MotorJoint_SetAngularOffset(b2JointId jointId, float angularOffset) +{ + 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); + + joint->motorJoint.angularOffset = angularOffset; +} + +void b2MotorJoint_SetMaxForce(b2JointId jointId, float maxForce) +{ + 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); + + joint->motorJoint.maxForce = B2_MAX(0.0f, maxForce); +} + +void b2MotorJoint_SetMaxTorque(b2JointId jointId, float maxTorque) +{ + 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); + + joint->motorJoint.maxTorque = B2_MAX(0.0f, maxTorque); +} + +void b2MotorJoint_SetCorrectionFactor(b2JointId jointId, float correctionFactor) +{ + 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); + + joint->motorJoint.correctionFactor = B2_CLAMP(correctionFactor, 0.0f, 1.0f); +} + +b2Vec2 b2MotorJoint_GetConstraintForce(b2JointId jointId, float inverseTimeStep) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* base = b2GetJoint(world, jointId); + B2_ASSERT(base->type == b2_motorJoint); + + b2MotorJoint* joint = &base->motorJoint; + b2Vec2 force = b2MulSV(inverseTimeStep, joint->linearImpulse); + return force; +} + +float b2MotorJoint_GetConstraintTorque(b2JointId jointId, float inverseTimeStep) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_motorJoint); + + return inverseTimeStep * joint->motorJoint.angularImpulse; +} + +#if 0 +void b2DumpMotorJoint() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2MotorJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle); + b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); + b2Dump(" jd.damping = %.9g;\n", m_damping); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} +#endif diff --git a/src/mouse_joint.c b/src/mouse_joint.c index f681d091..52c9b16a 100644 --- a/src/mouse_joint.c +++ b/src/mouse_joint.c @@ -33,7 +33,7 @@ void b2MouseJoint_SetTarget(b2JointId jointId, b2Vec2 target) base->mouseJoint.targetA = target; } -void b2PrepareMouse(b2Joint* base, b2StepContext* context) +void b2PrepareMouseJoint(b2Joint* base, b2StepContext* context) { B2_ASSERT(base->type == b2_mouseJoint); @@ -90,7 +90,7 @@ void b2PrepareMouse(b2Joint* base, b2StepContext* context) } } -void b2WarmStartMouse(b2Joint* base, b2StepContext* context) +void b2WarmStartMouseJoint(b2Joint* base, b2StepContext* context) { B2_ASSERT(base->type == b2_mouseJoint); @@ -114,7 +114,7 @@ void b2WarmStartMouse(b2Joint* base, b2StepContext* context) bodyB->angularVelocity = wB; } -void b2SolveMouseVelocity(b2Joint* base, b2StepContext* context) +void b2SolveMouseJoint(b2Joint* base, b2StepContext* context) { b2MouseJoint* joint = &base->mouseJoint; b2SolverBody* bodyB = context->solverBodies + joint->indexB; diff --git a/src/prismatic_joint.c b/src/prismatic_joint.c index f97880e7..7d783ce1 100644 --- a/src/prismatic_joint.c +++ b/src/prismatic_joint.c @@ -1,8 +1,6 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT -#define _CRT_SECURE_NO_WARNINGS - #include "body.h" #include "core.h" #include "joint.h" @@ -58,7 +56,7 @@ // s1 = cross(d + r1, u), s2 = cross(r2, u) // a1 = cross(d + r1, v), a2 = cross(r2, v) -void b2PreparePrismatic(b2Joint* base, b2StepContext* context) +void b2PreparePrismaticJoint(b2Joint* base, b2StepContext* context) { B2_ASSERT(base->type == b2_prismaticJoint); @@ -74,29 +72,48 @@ void b2PreparePrismatic(b2Joint* base, b2StepContext* context) joint->indexA = context->bodyToSolverMap[indexA]; joint->indexB = context->bodyToSolverMap[indexB]; - joint->localCenterA = bodyA->localCenter; - joint->localCenterB = bodyB->localCenter; - joint->positionA = bodyA->position; - joint->positionB = bodyB->position; - joint->angleA = bodyA->angle; - joint->angleB = bodyB->angle; float mA = bodyA->invMass; float iA = bodyA->invI; float mB = bodyB->invMass; float iB = bodyB->invI; + float angleA = bodyA->angle; + float angleB = bodyB->angle; b2Rot qA = bodyA->transform.q; b2Rot qB = bodyB->transform.q; // Compute the effective masses. - b2Vec2 rA = b2RotateVector(qA, b2Sub(base->localAnchorA, joint->localCenterA)); - b2Vec2 rB = b2RotateVector(qB, b2Sub(base->localAnchorB, joint->localCenterB)); - b2Vec2 d = b2Add(b2Sub(bodyB->position, bodyA->position), b2Sub(rB, rA)); + b2Vec2 rA = b2RotateVector(qA, b2Sub(base->localAnchorA, bodyA->localCenter)); + b2Vec2 rB = b2RotateVector(qB, b2Sub(base->localAnchorB, bodyB->localCenter)); + + joint->rA = rA; + joint->rB = rB; + b2Vec2 d = b2Add(b2Sub(bodyB->position, bodyA->position), b2Sub(rB, rA)); + joint->pivotSeparation = d; + joint->angleSeparation = angleB - angleA - joint->referenceAngle; + + b2Vec2 axisA = b2RotateVector(qA, joint->localAxisA); + joint->axisA = axisA; + b2Vec2 perpA = b2LeftPerp(axisA); + + float s1 = b2Cross(b2Add(d, rA), perpA); + float s2 = b2Cross(rB, perpA); + + float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float k12 = iA * s1 + iB * s2; + float k22 = iA + iB; + if (k22 == 0.0f) + { + // For bodies with fixed rotation. + k22 = 1.0f; + } - b2Vec2 axis = b2RotateVector(qA, joint->localAxisA); - float a1 = b2Cross(b2Add(d, rA), axis); - float a2 = b2Cross(rB, axis); + b2Mat22 K = {{k11, k12}, {k12, k22}}; + joint->pivotMass = b2GetInverse22(K); + + float a1 = b2Cross(b2Add(d, rA), axisA); + float a2 = b2Cross(rB, axisA); float k = mA + mB + iA * a1 * a1 + iB * a2 * a2; joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; @@ -142,7 +159,7 @@ void b2PreparePrismatic(b2Joint* base, b2StepContext* context) } } -void b2WarmStartPrismatic(b2Joint* base, b2StepContext* context) +void b2WarmStartPrismaticJoint(b2Joint* base, b2StepContext* context) { B2_ASSERT(base->type == b2_prismaticJoint); @@ -160,20 +177,17 @@ void b2WarmStartPrismatic(b2Joint* base, b2StepContext* context) float mB = bodyB->invMass; float iB = bodyB->invI; - b2Rot qA = b2MakeRot(joint->angleA); - b2Rot qB = b2MakeRot(joint->angleB); - - b2Vec2 rA = b2RotateVector(qA, b2Sub(base->localAnchorA, joint->localCenterA)); - b2Vec2 rB = b2RotateVector(qB, b2Sub(base->localAnchorB, joint->localCenterB)); - b2Vec2 d = b2Add(b2Sub(joint->positionB, joint->positionA), b2Sub(rB, rA)); + b2Vec2 rA = joint->rA; + b2Vec2 rB = joint->rB; + b2Vec2 d = joint->pivotSeparation; - b2Vec2 axis = b2RotateVector(qA, joint->localAxisA); - float a1 = b2Cross(b2Add(d, rA), axis); - float a2 = b2Cross(rB, axis); + b2Vec2 axisA = joint->axisA; + float a1 = b2Cross(b2Add(d, rA), axisA); + float a2 = b2Cross(rB, axisA); float axialImpulse = joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; - b2Vec2 P = b2MulSV(axialImpulse, axis); + b2Vec2 P = b2MulSV(axialImpulse, axisA); float LA = axialImpulse * a1; float LB = axialImpulse * a2; @@ -183,7 +197,7 @@ void b2WarmStartPrismatic(b2Joint* base, b2StepContext* context) bodyB->angularVelocity += iB * LB; } -void b2SolvePrismaticVelocity(b2Joint* base, b2StepContext* context, bool useBias) +void b2SolvePrismaticJoint(b2Joint* base, b2StepContext* context, bool useBias) { B2_ASSERT(base->type == b2_prismaticJoint); @@ -204,33 +218,35 @@ void b2SolvePrismaticVelocity(b2Joint* base, b2StepContext* context, bool useBia float mB = bodyB->invMass; float iB = bodyB->invI; - const b2Vec2 cA = b2Add(joint->positionA, bodyA->deltaPosition); - const float aA = joint->angleA + bodyA->deltaAngle; - const b2Vec2 cB = b2Add(joint->positionB, bodyB->deltaPosition); - const float aB = joint->angleB + bodyB->deltaAngle; + // Small angle approximation + b2Vec2 drA = b2CrossSV(bodyA->deltaAngle, joint->rA); + b2Vec2 drB = b2CrossSV(bodyB->deltaAngle, joint->rB); - b2Rot qA = b2MakeRot(aA); - b2Rot qB = b2MakeRot(aB); + b2Vec2 rA = b2Add(joint->rA, drA); + b2Vec2 rB = b2Add(joint->rB, drB); - b2Vec2 rA = b2RotateVector(qA, b2Sub(base->localAnchorA, joint->localCenterA)); - b2Vec2 rB = b2RotateVector(qB, b2Sub(base->localAnchorB, joint->localCenterB)); - b2Vec2 d = b2Add(b2Sub(cB, cA), b2Sub(rB, rA)); + b2Vec2 d = b2Add(joint->pivotSeparation, b2Sub(drB, drA)); - b2Vec2 axis = b2RotateVector(qA, joint->localAxisA); - float a1 = b2Cross(b2Add(d, rA), axis); - float a2 = b2Cross(rB, axis); + float dAngleA = bodyA->deltaAngle; + + // Small angle approximation + b2Vec2 axisA = {joint->axisA.x - dAngleA * joint->axisA.y, dAngleA * joint->axisA.x + joint->axisA.y}; + axisA = b2Normalize(axisA); + + float a1 = b2Cross(b2Add(d, rA), axisA); + float a2 = b2Cross(rB, axisA); // Solve motor constraint if (joint->enableMotor) { - float Cdot = b2Dot(axis, b2Sub(vB, vA)) + a2 * wB - a1 * wA; + 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; joint->motorImpulse = B2_CLAMP(joint->motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = joint->motorImpulse - oldImpulse; - b2Vec2 P = b2MulSV(impulse, axis); + b2Vec2 P = b2MulSV(impulse, axisA); float LA = impulse * a1; float LB = impulse * a2; @@ -242,7 +258,7 @@ void b2SolvePrismaticVelocity(b2Joint* base, b2StepContext* context, bool useBia if (joint->enableLimit) { - float translation = b2Dot(axis, d); + float translation = b2Dot(axisA, d); // Lower limit { @@ -264,12 +280,12 @@ void b2SolvePrismaticVelocity(b2Joint* base, b2StepContext* context, bool useBia } float oldImpulse = joint->lowerImpulse; - float Cdot = b2Dot(axis, b2Sub(vB, vA)) + a2 * wB - a1 * wA; + float Cdot = b2Dot(axisA, b2Sub(vB, vA)) + a2 * wB - a1 * wA; float impulse = -joint->axialMass * massScale * (Cdot + bias) - impulseScale * oldImpulse; joint->lowerImpulse = B2_MAX(oldImpulse + impulse, 0.0f); impulse = joint->lowerImpulse - oldImpulse; - b2Vec2 P = b2MulSV(impulse, axis); + b2Vec2 P = b2MulSV(impulse, axisA); float LA = impulse * a1; float LB = impulse * a2; @@ -303,12 +319,12 @@ void b2SolvePrismaticVelocity(b2Joint* base, b2StepContext* context, bool useBia float oldImpulse = joint->upperImpulse; // sign flipped - float Cdot = b2Dot(axis, b2Sub(vA, vB)) + a1 * wA - a2 * wB; + float Cdot = b2Dot(axisA, b2Sub(vA, vB)) + a1 * wA - a2 * wB; float impulse = -joint->axialMass * massScale * (Cdot + bias) - impulseScale * oldImpulse; joint->upperImpulse = B2_MAX(oldImpulse + impulse, 0.0f); impulse = joint->upperImpulse - oldImpulse; - b2Vec2 P = b2MulSV(impulse, axis); + b2Vec2 P = b2MulSV(impulse, axisA); float LA = impulse * a1; float LB = impulse * a2; @@ -322,24 +338,13 @@ void b2SolvePrismaticVelocity(b2Joint* base, b2StepContext* context, bool useBia // Solve the prismatic constraint in block form { - b2Vec2 perp = b2LeftPerp(axis); - - float s1 = b2Cross(b2Add(d, rA), perp); - float s2 = b2Cross(rB, perp); - - float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; - float k12 = iA * s1 + iB * s2; - float k22 = iA + iB; - if (k22 == 0.0f) - { - // For bodies with fixed rotation. - k22 = 1.0f; - } + b2Vec2 perpA = b2LeftPerp(axisA); - b2Mat22 K = {{k11, k12}, {k12, k22}}; + float s1 = b2Cross(b2Add(d, rA), perpA); + float s2 = b2Cross(rB, perpA); b2Vec2 Cdot; - Cdot.x = b2Dot(perp, b2Sub(vB, vA)) + s2 * wB - s1 * wA; + Cdot.x = b2Dot(perpA, b2Sub(vB, vA)) + s2 * wB - s1 * wA; Cdot.y = wB - wA; b2Vec2 bias = b2Vec2_zero; @@ -348,20 +353,23 @@ void b2SolvePrismaticVelocity(b2Joint* base, b2StepContext* context, bool useBia if (useBias) { b2Vec2 C; - C.x = b2Dot(perp, d); - C.y = aB - aA - joint->referenceAngle; + C.x = b2Dot(perpA, d); + C.y = joint-> angleSeparation + bodyB->deltaAngle - bodyA->deltaAngle; bias = b2MulSV(joint->biasCoefficient, C); massScale = joint->massCoefficient; impulseScale = joint->impulseCoefficient; } - b2Vec2 b = b2Solve22(K, b2Add(Cdot, bias)); + b2Vec2 b = b2MulMV(joint->pivotMass, b2Add(Cdot, bias)); b2Vec2 impulse; impulse.x = -massScale * b.x - impulseScale * joint->impulse.x; impulse.y = -massScale * b.y - impulseScale * joint->impulse.y; - b2Vec2 P = b2MulSV(impulse.x, perp); + joint->impulse.x += impulse.x; + joint->impulse.y += impulse.y; + + b2Vec2 P = b2MulSV(impulse.x, perpA); float LA = impulse.x * s1 + impulse.y; float LB = impulse.x * s2 + impulse.y; @@ -386,12 +394,9 @@ void b2PrismaticJoint_EnableLimit(b2JointId jointId, bool enableLimit) return; } - B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity); - - b2Joint* joint = world->joints + jointId.index; - B2_ASSERT(joint->object.index == joint->object.next); - B2_ASSERT(joint->object.revision == jointId.revision); + b2Joint* joint = b2GetJoint(world, jointId); B2_ASSERT(joint->type == b2_prismaticJoint); + joint->prismaticJoint.enableLimit = enableLimit; } @@ -404,12 +409,9 @@ void b2PrismaticJoint_EnableMotor(b2JointId jointId, bool enableMotor) return; } - B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity); - - b2Joint* joint = world->joints + jointId.index; - B2_ASSERT(joint->object.index == joint->object.next); - B2_ASSERT(joint->object.revision == jointId.revision); + b2Joint* joint = b2GetJoint(world, jointId); B2_ASSERT(joint->type == b2_prismaticJoint); + joint->prismaticJoint.enableMotor = enableMotor; } @@ -422,30 +424,18 @@ void b2PrismaticJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed) return; } - B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity); - - b2Joint* joint = world->joints + jointId.index; - B2_ASSERT(joint->object.index == joint->object.next); - B2_ASSERT(joint->object.revision == jointId.revision); + b2Joint* joint = b2GetJoint(world, jointId); B2_ASSERT(joint->type == b2_prismaticJoint); + joint->prismaticJoint.motorSpeed = motorSpeed; } float b2PrismaticJoint_GetMotorForce(b2JointId jointId, float inverseTimeStep) { b2World* world = b2GetWorldFromIndex(jointId.world); - B2_ASSERT(world->locked == false); - if (world->locked) - { - return 0.0f; - } - - B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity); - - b2Joint* joint = world->joints + jointId.index; - B2_ASSERT(joint->object.index == joint->object.next); - B2_ASSERT(joint->object.revision == jointId.revision); + b2Joint* joint = b2GetJoint(world, jointId); B2_ASSERT(joint->type == b2_prismaticJoint); + return inverseTimeStep * joint->prismaticJoint.motorImpulse; } @@ -458,31 +448,38 @@ void b2PrismaticJoint_SetMaxMotorForce(b2JointId jointId, float force) return; } - B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity); - - b2Joint* joint = world->joints + jointId.index; - B2_ASSERT(joint->object.index == joint->object.next); - B2_ASSERT(joint->object.revision == jointId.revision); + b2Joint* joint = b2GetJoint(world, jointId); B2_ASSERT(joint->type == b2_prismaticJoint); + joint->prismaticJoint.maxMotorForce = force; } -b2Vec2 b2PrismaticJoint_GetConstraintForce(b2JointId jointId) +b2Vec2 b2PrismaticJoint_GetConstraintForce(b2JointId jointId, float inverseTimeStep) { b2World* world = b2GetWorldFromIndex(jointId.world); - B2_ASSERT(world->locked == false); - if (world->locked) - { - return b2Vec2_zero; - } + b2Joint* base = b2GetJoint(world, jointId); + B2_ASSERT(base->type == b2_prismaticJoint); + + b2PrismaticJoint* joint = &base->prismaticJoint; + + // This is a frame behind + b2Vec2 axisA = joint->axisA; + b2Vec2 perpA = b2LeftPerp(axisA); - B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity); + float perpForce = inverseTimeStep * joint->impulse.x; + float axialForce = inverseTimeStep * (joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse); - b2Joint* joint = world->joints + jointId.index; - B2_ASSERT(joint->object.index == joint->object.next); - B2_ASSERT(joint->object.revision == jointId.revision); + b2Vec2 force = b2Add(b2MulSV(perpForce, perpA), b2MulSV(axialForce, axisA)); + return force; +} + +float b2PrismaticJoint_GetConstraintTorque(b2JointId jointId, float inverseTimeStep) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); B2_ASSERT(joint->type == b2_prismaticJoint); - return joint->prismaticJoint.impulse; + + return inverseTimeStep * joint->prismaticJoint.impulse.y; } #if 0 diff --git a/src/revolute_joint.c b/src/revolute_joint.c index b39d6c7a..69a15392 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -26,7 +26,7 @@ // J = [0 0 -1 0 0 1] // K = invI1 + invI2 -void b2PrepareRevolute(b2Joint* base, b2StepContext* context) +void b2PrepareRevoluteJoint(b2Joint* base, b2StepContext* context) { B2_ASSERT(base->type == b2_revoluteJoint); @@ -82,16 +82,33 @@ void b2PrepareRevolute(b2Joint* base, b2StepContext* context) K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; joint->pivotMass = b2GetInverse22(K); - // hertz = 1/4 * substep Hz - const float hertz = 0.25f * context->velocityIterations * context->inv_dt; - const float zeta = 1.0f; - float omega = 2.0f * b2_pi * hertz; - float h = context->dt; + { + // todo these coefficients could be in the context and shared + // hertz = 1/4 * substep Hz + const float hertz = 0.25f * context->velocityIterations * context->inv_dt; + const float zeta = 1.0f; + //const float hertz = 60.0f; + //const float zeta = 1.0f; + float omega = 2.0f * b2_pi * hertz; + float h = context->dt; + + joint->biasCoefficient = omega / (2.0f * zeta + h * omega); + float c = h * omega * (2.0f * zeta + h * omega); + joint->impulseCoefficient = 1.0f / (1.0f + c); + joint->massCoefficient = c * joint->impulseCoefficient; + } - joint->biasCoefficient = omega / (2.0f * zeta + h * omega); - float c = h * omega * (2.0f * zeta + h * omega); - joint->impulseCoefficient = 1.0f / (1.0f + c); - joint->massCoefficient = c * joint->impulseCoefficient; + { + const float hertz = 15.0f; + const float zeta = 0.7f; + float omega = 2.0f * b2_pi * hertz; + float h = context->dt; + float c = h * omega * (2.0f * zeta + h * omega); + + joint->limitBiasCoefficient = omega / (2.0f * zeta + h * omega); + joint->limitImpulseCoefficient = 1.0f / (1.0f + c); + joint->limitMassCoefficient = c * joint->limitImpulseCoefficient; + } if (joint->enableLimit == false || fixedRotation) { @@ -108,22 +125,23 @@ void b2PrepareRevolute(b2Joint* base, b2StepContext* context) { float dtRatio = context->dtRatio; - // Soft step works best when bilateral constraints have no warm starting. - joint->impulse = b2Vec2_zero; + // Soft step works best when stiff constraints have no warm starting. + //joint->linearImpulse = b2MulSV(dtRatio, joint->linearImpulse); + joint->linearImpulse = b2Vec2_zero; joint->motorImpulse *= dtRatio; joint->lowerImpulse *= dtRatio; joint->upperImpulse *= dtRatio; } else { - joint->impulse = b2Vec2_zero; + joint->linearImpulse = b2Vec2_zero; joint->motorImpulse = 0.0f; joint->lowerImpulse = 0.0f; joint->upperImpulse = 0.0f; } } -void b2WarmStartRevolute(b2Joint* base, b2StepContext* context) +void b2WarmStartRevoluteJoint(b2Joint* base, b2StepContext* context) { B2_ASSERT(base->type == b2_revoluteJoint); @@ -134,19 +152,24 @@ void b2WarmStartRevolute(b2Joint* base, b2StepContext* context) // Note: must warm start solver bodies b2SolverBody* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexA; + float mA = bodyA->invMass; float iA = bodyA->invI; b2SolverBody* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexB; + float mB = bodyB->invMass; float iB = bodyB->invI; - // TODO_ERIN is warm starting axial stuff useful? + // todo is warm starting axial stuff useful? float axialImpulse = joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; - bodyA->angularVelocity -= iA * axialImpulse; - bodyB->angularVelocity += iB * axialImpulse; + bodyA->linearVelocity = b2MulSub(bodyA->linearVelocity, mA, joint->linearImpulse); + bodyA->angularVelocity -= iA * (b2Cross(joint->rA, joint->linearImpulse) + axialImpulse); + + bodyB->linearVelocity = b2MulAdd(bodyB->linearVelocity, mB, joint->linearImpulse); + bodyB->angularVelocity += iB * (b2Cross(joint->rB, joint->linearImpulse) + axialImpulse); } -void b2SolveRevoluteVelocity(b2Joint* base, b2StepContext* context, bool useBias) +void b2SolveRevoluteJoint(b2Joint* base, b2StepContext* context, bool useBias) { B2_ASSERT(base->type == b2_revoluteJoint); @@ -172,20 +195,6 @@ void b2SolveRevoluteVelocity(b2Joint* base, b2StepContext* context, bool useBias bool fixedRotation = (iA + iB == 0.0f); - // Solve motor constraint. - if (joint->enableMotor && fixedRotation == false) - { - float Cdot = wB - wA - joint->motorSpeed; - float impulse = -joint->axialMass * Cdot; - float oldImpulse = joint->motorImpulse; - float maxImpulse = context->dt * joint->maxMotorTorque; - joint->motorImpulse = B2_CLAMP(joint->motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = joint->motorImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - if (joint->enableLimit && fixedRotation == false) { float jointAngle = aB - aA - joint->referenceAngle; @@ -203,9 +212,9 @@ void b2SolveRevoluteVelocity(b2Joint* base, b2StepContext* context, bool useBias } else if (useBias) { - bias = joint->biasCoefficient * C; - massScale = joint->massCoefficient; - impulseScale = joint->impulseCoefficient; + bias = joint->limitBiasCoefficient * C; + massScale = joint->limitMassCoefficient; + impulseScale = joint->limitImpulseCoefficient; } float Cdot = wB - wA; @@ -233,9 +242,9 @@ void b2SolveRevoluteVelocity(b2Joint* base, b2StepContext* context, bool useBias } else if (useBias) { - bias = joint->biasCoefficient * C; - massScale = joint->massCoefficient; - impulseScale = joint->impulseCoefficient; + bias = joint->limitBiasCoefficient * C; + massScale = joint->limitMassCoefficient; + impulseScale = joint->limitImpulseCoefficient; } float Cdot = wA - wB; @@ -275,11 +284,11 @@ void b2SolveRevoluteVelocity(b2Joint* base, b2StepContext* context, bool useBias b2Vec2 b = b2MulMV(joint->pivotMass, b2Add(Cdot, bias)); b2Vec2 impulse; - impulse.x = -massScale * b.x - impulseScale * joint->impulse.x; - impulse.y = -massScale * b.y - impulseScale * joint->impulse.y; + impulse.x = -massScale * b.x - impulseScale * joint->linearImpulse.x; + impulse.y = -massScale * b.y - impulseScale * joint->linearImpulse.y; - joint->impulse.x += impulse.x; - joint->impulse.y += impulse.y; + joint->linearImpulse.x += impulse.x; + joint->linearImpulse.y += impulse.y; vA = b2MulSub(vA, mA, impulse); wA -= iA * b2Cross(rA, impulse); @@ -288,6 +297,20 @@ void b2SolveRevoluteVelocity(b2Joint* base, b2StepContext* context, bool useBias wB += iB * b2Cross(rB, impulse); } + // Solve motor constraint. + if (joint->enableMotor && fixedRotation == false) + { + float Cdot = wB - wA - joint->motorSpeed; + float impulse = -joint->axialMass * Cdot; + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->dt * joint->maxMotorTorque; + joint->motorImpulse = B2_CLAMP(joint->motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = joint->motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + bodyA->linearVelocity = vA; bodyA->angularVelocity = wA; bodyB->linearVelocity = vB; @@ -303,12 +326,9 @@ void b2RevoluteJoint_EnableLimit(b2JointId jointId, bool enableLimit) return; } - B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity); - - b2Joint* joint = world->joints + jointId.index; - B2_ASSERT(joint->object.index == joint->object.next); - B2_ASSERT(joint->object.revision == jointId.revision); + b2Joint* joint = b2GetJoint(world, jointId); B2_ASSERT(joint->type == b2_revoluteJoint); + joint->revoluteJoint.enableLimit = enableLimit; } @@ -321,12 +341,9 @@ void b2RevoluteJoint_EnableMotor(b2JointId jointId, bool enableMotor) return; } - B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity); - - b2Joint* joint = world->joints + jointId.index; - B2_ASSERT(joint->object.index == joint->object.next); - B2_ASSERT(joint->object.revision == jointId.revision); + b2Joint* joint = b2GetJoint(world, jointId); B2_ASSERT(joint->type == b2_revoluteJoint); + joint->revoluteJoint.enableMotor = enableMotor; } @@ -339,43 +356,49 @@ void b2RevoluteJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed) return; } - B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity); - - b2Joint* joint = world->joints + jointId.index; - B2_ASSERT(joint->object.index == joint->object.next); - B2_ASSERT(joint->object.revision == jointId.revision); + b2Joint* joint = b2GetJoint(world, jointId); B2_ASSERT(joint->type == b2_revoluteJoint); + joint->revoluteJoint.motorSpeed = motorSpeed; } float b2RevoluteJoint_GetMotorTorque(b2JointId jointId, float inverseTimeStep) { - b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_revoluteJoint); + return inverseTimeStep * joint->revoluteJoint.motorImpulse; } void b2RevoluteJoint_SetMaxMotorTorque(b2JointId jointId, float torque) { - b2Joint* joint = b2GetJointCheckType(jointId, b2_revoluteJoint); + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_revoluteJoint); + joint->revoluteJoint.maxMotorTorque = torque; } -b2Vec2 b2RevoluteJoint_GetConstraintForce(b2JointId jointId) +b2Vec2 b2RevoluteJoint_GetConstraintForce(b2JointId jointId, float inverseTimeStep) { b2World* world = b2GetWorldFromIndex(jointId.world); - B2_ASSERT(world->locked == false); - if (world->locked) - { - return b2Vec2_zero; - } + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_revoluteJoint); - B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity); + b2Vec2 force = b2MulSV(inverseTimeStep, joint->revoluteJoint.linearImpulse); + return force; +} - b2Joint* joint = world->joints + jointId.index; - B2_ASSERT(joint->object.index == joint->object.next); - B2_ASSERT(joint->object.revision == jointId.revision); +float b2RevoluteJoint_GetConstraintTorque(b2JointId jointId, float inverseTimeStep) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); B2_ASSERT(joint->type == b2_revoluteJoint); - return joint->revoluteJoint.impulse; + + const b2RevoluteJoint* revolute = &joint->revoluteJoint; + float torque = inverseTimeStep * (revolute->motorImpulse + revolute->lowerImpulse - revolute->upperImpulse); + return torque; } #if 0 @@ -425,7 +448,7 @@ void b2DrawRevolute(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bod float aB = bodyB->angle; float angle = aB - aA - joint->referenceAngle; - const float L = 0.5f; + const float L = base->drawSize; b2Vec2 r = {L * cosf(angle), L * sinf(angle)}; draw->DrawSegment(pB, b2Add(pB, r), c1, draw->context); @@ -445,7 +468,7 @@ void b2DrawRevolute(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bod draw->DrawSegment(pA, pB, color, draw->context); draw->DrawSegment(xfB.p, pB, color, draw->context); - //char buffer[32]; - //sprintf(buffer, "%.1f", b2Length(joint->impulse)); - //draw->DrawString(pA, buffer, draw->context); + // char buffer[32]; + // sprintf(buffer, "%.1f", b2Length(joint->impulse)); + // draw->DrawString(pA, buffer, draw->context); } diff --git a/src/shape.c b/src/shape.c index a748354e..788855af 100644 --- a/src/shape.c +++ b/src/shape.c @@ -360,7 +360,7 @@ void b2Chain_SetRestitution(b2ChainId chainId, float restitution) } } -int32_t b2Shape_GetContactCount(b2ShapeId shapeId) +int32_t b2Shape_GetContactCapacity(b2ShapeId shapeId) { b2World* world = b2GetWorldFromIndex(shapeId.world); B2_ASSERT(world->locked == false); diff --git a/src/weld_joint.c b/src/weld_joint.c index a3724b0a..065c5dd3 100644 --- a/src/weld_joint.c +++ b/src/weld_joint.c @@ -23,7 +23,7 @@ // J = [0 0 -1 0 0 1] // K = invI1 + invI2 -void b2PrepareWeld(b2Joint* base, b2StepContext* context) +void b2PrepareWeldJoint(b2Joint* base, b2StepContext* context) { B2_ASSERT(base->type == b2_weldJoint); @@ -73,11 +73,11 @@ void b2PrepareWeld(b2Joint* base, b2StepContext* context) linearHertz = 0.25f * context->velocityIterations * context->inv_dt; // no warm staring - joint->pivotImpulse = b2Vec2_zero; + joint->linearImpulse = b2Vec2_zero; } else { - joint->pivotImpulse = b2MulSV(context->dtRatio, joint->pivotImpulse); + joint->linearImpulse = b2MulSV(context->dtRatio, joint->linearImpulse); } { @@ -95,11 +95,11 @@ void b2PrepareWeld(b2Joint* base, b2StepContext* context) angularHertz = 0.25f * context->velocityIterations * context->inv_dt; // no warm staring - joint->axialImpulse = 0.0f; + joint->angularImpulse = 0.0f; } else { - joint->axialImpulse = context->dtRatio * joint->axialImpulse; + joint->angularImpulse = context->dtRatio * joint->angularImpulse; } { @@ -110,9 +110,24 @@ void b2PrepareWeld(b2Joint* base, b2StepContext* context) joint->angularImpulseCoefficient = 1.0f / (1.0f + a); joint->angularMassCoefficient = a * joint->angularImpulseCoefficient; } + + if (context->enableWarmStarting) + { + float dtRatio = context->dtRatio; + + // Soft step works best when bilateral constraints have no warm starting. + joint->linearImpulse.x = dtRatio; + joint->linearImpulse.y = dtRatio; + joint->angularImpulse *= dtRatio; + } + else + { + joint->linearImpulse = b2Vec2_zero; + joint->angularImpulse = 0.0f; + } } -void b2WarmStartWeld(b2Joint* base, b2StepContext* context) +void b2WarmStartWeldJoint(b2Joint* base, b2StepContext* context) { b2WeldJoint* joint = &base->weldJoint; @@ -128,11 +143,11 @@ void b2WarmStartWeld(b2Joint* base, b2StepContext* context) float mB = bodyB->invMass; float iB = bodyB->invI; - vA = b2MulSub(vA, mA, joint->pivotImpulse); - wA -= iA * (b2Cross(joint->rA, joint->pivotImpulse) + joint->axialImpulse); + vA = b2MulSub(vA, mA, joint->linearImpulse); + wA -= iA * (b2Cross(joint->rA, joint->linearImpulse) + joint->angularImpulse); - vB = b2MulAdd(vB, mB, joint->pivotImpulse); - wB += iB * (b2Cross(joint->rB, joint->pivotImpulse) + joint->axialImpulse); + vB = b2MulAdd(vB, mB, joint->linearImpulse); + wB += iB * (b2Cross(joint->rB, joint->linearImpulse) + joint->angularImpulse); bodyA->linearVelocity = vA; bodyA->angularVelocity = wA; @@ -140,7 +155,7 @@ void b2WarmStartWeld(b2Joint* base, b2StepContext* context) bodyB->angularVelocity = wB; } -void b2SolveWeldVelocity(b2Joint* base, const b2StepContext* context, bool useBias) +void b2SolveWeldJoint(b2Joint* base, const b2StepContext* context, bool useBias) { B2_ASSERT(base->type == b2_weldJoint); @@ -198,8 +213,8 @@ void b2SolveWeldVelocity(b2Joint* base, const b2StepContext* context, bool useBi { float Cdot = wB - wA; float b = joint->axialMass * (Cdot + angularBias); - float impulse = -angularMassScale * b - angularImpulseScale * joint->axialImpulse; - joint->axialImpulse += impulse; + float impulse = -angularMassScale * b - angularImpulseScale * joint->angularImpulse; + joint->angularImpulse += impulse; wA -= iA * impulse; wB += iB * impulse; } @@ -211,11 +226,11 @@ void b2SolveWeldVelocity(b2Joint* base, const b2StepContext* context, bool useBi b2Vec2 b = b2MulMV(joint->pivotMass, b2Add(Cdot, linearBias)); b2Vec2 impulse = { - -linearMassScale * b.x - linearImpulseScale * joint->pivotImpulse.x, - -linearMassScale * b.y - linearImpulseScale * joint->pivotImpulse.y, + -linearMassScale * b.x - linearImpulseScale * joint->linearImpulse.x, + -linearMassScale * b.y - linearImpulseScale * joint->linearImpulse.y, }; - joint->pivotImpulse = b2Add(joint->pivotImpulse, impulse); + joint->linearImpulse = b2Add(joint->linearImpulse, impulse); vA = b2MulSub(vA, mA, impulse); wA -= iA * b2Cross(rA, impulse); diff --git a/src/wheel_joint.c b/src/wheel_joint.c new file mode 100644 index 00000000..1f931f50 --- /dev/null +++ b/src/wheel_joint.c @@ -0,0 +1,562 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver_data.h" +#include "world.h" + +#include "box2d/debug_draw.h" + +#include + +// Linear constraint (point-to-line) +// d = pB - pA = xB + rB - xA - rA +// C = dot(ay, d) +// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) +// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) +// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] + +// Spring linear constraint +// C = dot(ax, d) +// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) +// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] + +// Motor rotational constraint +// Cdot = wB - wA +// J = [0 0 -1 0 0 1] + +void b2PrepareWheelJoint(b2Joint* base, b2StepContext* context) +{ + B2_ASSERT(base->type == b2_wheelJoint); + + int32_t indexA = base->edges[0].bodyIndex; + 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)); + + b2WheelJoint* joint = &base->wheelJoint; + + joint->indexA = context->bodyToSolverMap[indexA]; + joint->indexB = context->bodyToSolverMap[indexB]; + + float mA = bodyA->invMass; + float iA = bodyA->invI; + float mB = bodyB->invMass; + float iB = bodyB->invI; + + b2Rot qA = bodyA->transform.q; + b2Rot qB = bodyB->transform.q; + + // Compute the effective masses. + b2Vec2 rA = b2RotateVector(qA, b2Sub(base->localAnchorA, bodyA->localCenter)); + b2Vec2 rB = b2RotateVector(qB, b2Sub(base->localAnchorB, bodyB->localCenter)); + + joint->rA = rA; + joint->rB = rB; + b2Vec2 d = b2Add(b2Sub(bodyB->position, bodyA->position), b2Sub(rB, rA)); + joint->pivotSeparation = d; + + b2Vec2 axisA = b2RotateVector(qA, joint->localAxisA); + joint->axisA = axisA; + b2Vec2 perpA = b2LeftPerp(axisA); + + // Perpendicular constraint (keep wheel on line) + float s1 = b2Cross(b2Add(d, rA), perpA); + float s2 = b2Cross(rB, perpA); + + float kp = mA + mB + iA * s1 * s1 + iB * s2 * s2; + joint->perpMass = kp > 0.0f ? 1.0f / kp : 0.0f; + + // Spring constraint + float a1 = b2Cross(b2Add(d, rA), axisA); + float a2 = b2Cross(rB, axisA); + + float ka = mA + mB + iA * a1 * a1 + iB * a2 * a2; + joint->axialMass = ka > 0.0f ? 1.0f / ka : 0.0f; + + joint->springMass = 0.0f; + joint->bias = 0.0f; + joint->gamma = 0.0f; + + if (joint->stiffness > 0.0f && ka > 0.0f) + { + float C = b2Dot(d, axisA); + + float dt = context->dt; + joint->gamma = dt * (joint->damping + dt * joint->stiffness); + joint->gamma = joint->gamma > 0.0f ? 1.0f / joint->gamma : 0.0f; + + joint->bias = dt * C * joint->stiffness * joint->gamma; + + float ks = ka + joint->gamma; + joint->springMass = ks > 0.0f ? 1.0f / ks : 0.0f; + } + + float km = iA + iB; + joint->motorMass = km > 0.0f ? 1.0f / km : 0.0f; + + //// hertz = 1/4 * substep Hz + const float hertz = 0.25f * context->velocityIterations * context->inv_dt; + const float zeta = 1.0f; + float omega = 2.0f * b2_pi * hertz; + float h = context->dt; + + joint->biasCoefficient = omega / (2.0f * zeta + h * omega); + float c = h * omega * (2.0f * zeta + h * omega); + joint->impulseCoefficient = 1.0f / (1.0f + c); + joint->massCoefficient = c * joint->impulseCoefficient; + + if (joint->enableLimit == false) + { + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } + + if (joint->enableMotor == false) + { + joint->motorImpulse = 0.0f; + } + + if (context->enableWarmStarting) + { + float dtRatio = context->dtRatio; + + // Soft step works best when bilateral constraints have no warm starting. + joint->perpImpulse = 0.0f; + joint->motorImpulse *= dtRatio; + joint->springImpulse *= dtRatio; + joint->lowerImpulse *= dtRatio; + joint->upperImpulse *= dtRatio; + } + else + { + joint->perpImpulse = 0.0f; + joint->springImpulse = 0.0f; + joint->motorImpulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } +} + +void b2WarmStartWheelJoint(b2Joint* base, b2StepContext* context) +{ + B2_ASSERT(base->type == b2_wheelJoint); + + b2WheelJoint* joint = &base->wheelJoint; + + // This is a dummy body to represent a static body since static bodies don't have a solver body. + b2SolverBody dummyBody = {0}; + + // Note: must warm start solver bodies + b2SolverBody* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexA; + float mA = bodyA->invMass; + float iA = bodyA->invI; + + b2SolverBody* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexB; + float mB = bodyB->invMass; + float iB = bodyB->invI; + + b2Vec2 rA = joint->rA; + b2Vec2 rB = joint->rB; + b2Vec2 d = joint->pivotSeparation; + + b2Vec2 axisA = joint->axisA; + float a1 = b2Cross(b2Add(d, rA), axisA); + float a2 = b2Cross(rB, axisA); + + float axialImpulse = joint->springImpulse + joint->lowerImpulse - joint->upperImpulse; + + b2Vec2 P = b2MulSV(axialImpulse, axisA); + float LA = axialImpulse * a1 + joint->motorImpulse; + float LB = axialImpulse * a2 + joint->motorImpulse; + + bodyA->linearVelocity = b2MulSub(bodyA->linearVelocity, mA, P); + bodyA->angularVelocity -= iA * LA; + bodyB->linearVelocity = b2MulAdd(bodyB->linearVelocity, mB, P); + bodyB->angularVelocity += iB * LB; +} + +void b2SolveWheelJoint(b2Joint* base, b2StepContext* context, bool useBias) +{ + B2_ASSERT(base->type == b2_wheelJoint); + + b2WheelJoint* joint = &base->wheelJoint; + + // This is a dummy body to represent a static body since static bodies don't have a solver body. + b2SolverBody dummyBody = {0}; + + b2SolverBody* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexA; + b2Vec2 vA = bodyA->linearVelocity; + float wA = bodyA->angularVelocity; + float mA = bodyA->invMass; + float iA = bodyA->invI; + + b2SolverBody* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexB; + b2Vec2 vB = bodyB->linearVelocity; + float wB = bodyB->angularVelocity; + float mB = bodyB->invMass; + float iB = bodyB->invI; + + bool fixedRotation = (iA + iB == 0.0f); + + // Small angle approximation + b2Vec2 drA = b2CrossSV(bodyA->deltaAngle, joint->rA); + b2Vec2 drB = b2CrossSV(bodyB->deltaAngle, joint->rB); + + b2Vec2 rA = b2Add(joint->rA, drA); + b2Vec2 rB = b2Add(joint->rB, drB); + + b2Vec2 d = b2Add(joint->pivotSeparation, b2Sub(drB, drA)); + + float dAngleA = bodyA->deltaAngle; + + // Small angle approximation + b2Vec2 axisA = {joint->axisA.x - dAngleA * joint->axisA.y, dAngleA * joint->axisA.x + joint->axisA.y}; + axisA = b2Normalize(axisA); + + // Solve motor constraint + if (joint->enableMotor && fixedRotation == false) + { + float Cdot = wB - wA - joint->motorSpeed; + float impulse = -joint->motorMass * Cdot; + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->dt * joint->maxMotorTorque; + joint->motorImpulse = B2_CLAMP(joint->motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = joint->motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + float a1 = b2Cross(b2Add(d, rA), axisA); + float a2 = b2Cross(rB, axisA); + + // Solve spring constraint + { + float Cdot = b2Dot(axisA, b2Sub(vB, vA)) + a2 * wB - a1 * wA; + float impulse = -joint->springMass * (Cdot + joint->bias + joint->gamma * joint->springImpulse); + joint->springImpulse += impulse; + + b2Vec2 P = b2MulSV(impulse, axisA); + float LA = impulse * a1; + float LB = impulse * a2; + + vA = b2MulSub(vA, mA, P); + wA -= iA * LA; + vB = b2MulAdd(vB, mB, P); + wB += iB * LB; + } + + if (joint->enableLimit) + { + float translation = b2Dot(axisA, d); + + // Lower limit + { + float C = translation - joint->lowerTranslation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if (C > 0.0f) + { + // speculation + bias = C * context->inv_dt; + } + else if (useBias) + { + bias = joint->biasCoefficient * C; + massScale = joint->massCoefficient; + impulseScale = joint->impulseCoefficient; + } + + float oldImpulse = joint->lowerImpulse; + float Cdot = b2Dot(axisA, b2Sub(vB, vA)) + a2 * wB - a1 * wA; + float impulse = -joint->axialMass * massScale * (Cdot + bias) - impulseScale * oldImpulse; + joint->lowerImpulse = B2_MAX(oldImpulse + impulse, 0.0f); + impulse = joint->lowerImpulse - oldImpulse; + + b2Vec2 P = b2MulSV(impulse, axisA); + float LA = impulse * a1; + float LB = impulse * a2; + + vA = b2MulSub(vA, mA, P); + wA -= iA * LA; + vB = b2MulAdd(vB, mB, P); + wB += iB * LB; + } + + // Upper limit + // Note: signs are flipped to keep C positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + // sign flipped + float C = joint->upperTranslation - translation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if (C > 0.0f) + { + // speculation + bias = C * context->inv_dt; + } + else if (useBias) + { + bias = joint->biasCoefficient * C; + massScale = joint->massCoefficient; + impulseScale = joint->impulseCoefficient; + } + + float oldImpulse = joint->upperImpulse; + // sign flipped + float Cdot = b2Dot(axisA, b2Sub(vA, vB)) + a1 * wA - a2 * wB; + float impulse = -joint->axialMass * massScale * (Cdot + bias) - impulseScale * oldImpulse; + joint->upperImpulse = B2_MAX(oldImpulse + impulse, 0.0f); + impulse = joint->upperImpulse - oldImpulse; + + b2Vec2 P = b2MulSV(impulse, axisA); + float LA = impulse * a1; + float LB = impulse * a2; + + // sign flipped + vA = b2MulAdd(vA, mA, P); + wA += iA * LA; + vB = b2MulSub(vB, mB, P); + wB -= iB * LB; + } + } + + // Solve the prismatic constraint in block form + { + b2Vec2 perpA = b2LeftPerp(axisA); + + float s1 = b2Cross(b2Add(d, rA), perpA); + float s2 = b2Cross(rB, perpA); + + float Cdot = b2Dot(perpA, b2Sub(vB, vA)) + s2 * wB - s1 * wA; + + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if (useBias) + { + float C = b2Dot(perpA, d); + bias = joint->biasCoefficient * C; + massScale = joint->massCoefficient; + impulseScale = joint->impulseCoefficient; + } + + float oldImpulse = joint->perpImpulse; + float impulse = -joint->perpMass * massScale * (Cdot + bias) - impulseScale * oldImpulse; + joint->perpImpulse = impulse; + + b2Vec2 P = b2MulSV(impulse, perpA); + float LA = impulse * s1; + float LB = impulse * s2; + + vA = b2MulSub(vA, mA, P); + wA -= iA * LA; + vB = b2MulAdd(vB, mB, P); + wB += iB * LB; + } + + bodyA->linearVelocity = vA; + bodyA->angularVelocity = wA; + bodyB->linearVelocity = vB; + bodyB->angularVelocity = wB; +} + +void b2WheelJoint_SetStiffness(b2JointId jointId, float stiffness) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return; + } + + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_wheelJoint); + + joint->wheelJoint.stiffness = stiffness; +} + +void b2WheelJoint_SetDamping(b2JointId jointId, float damping) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return; + } + + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_wheelJoint); + + joint->wheelJoint.damping = damping; +} + +void b2WheelJoint_EnableLimit(b2JointId jointId, bool enableLimit) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return; + } + + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_wheelJoint); + + joint->wheelJoint.enableLimit = enableLimit; +} + +void b2WheelJoint_EnableMotor(b2JointId jointId, bool enableMotor) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return; + } + + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_wheelJoint); + + joint->wheelJoint.enableMotor = enableMotor; +} + +void b2WheelJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return; + } + + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_wheelJoint); + + joint->wheelJoint.motorSpeed = motorSpeed; +} + +float b2WheelJoint_GetMotorTorque(b2JointId jointId, float inverseTimeStep) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_wheelJoint); + + return inverseTimeStep * joint->wheelJoint.motorImpulse; +} + +void b2WheelJoint_SetMaxMotorTorque(b2JointId jointId, float torque) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + return; + } + + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_wheelJoint); + + joint->wheelJoint.maxMotorTorque = torque; +} + +b2Vec2 b2WheelJoint_GetConstraintForce(b2JointId jointId, float inverseTimeStep) +{ + 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 = inverseTimeStep * joint->perpImpulse; + float axialForce = inverseTimeStep * (joint->springImpulse + joint->lowerImpulse - joint->upperImpulse); + + b2Vec2 force = b2Add(b2MulSV(perpForce, perpA), b2MulSV(axialForce, axisA)); + return force; +} + +float b2WheelJoint_GetConstraintTorque(b2JointId jointId, float inverseTimeStep) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + b2Joint* joint = b2GetJoint(world, jointId); + B2_ASSERT(joint->type == b2_wheelJoint); + + return inverseTimeStep * joint->wheelJoint.motorImpulse; +} + +#if 0 +void b2WheelJoint_Dump() +{ + int32 indexA = joint->bodyA->joint->islandIndex; + int32 indexB = joint->bodyB->joint->islandIndex; + + b2Dump(" b2WheelJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", joint->collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", joint->referenceAngle); + b2Dump(" jd.enableLimit = bool(%d);\n", joint->enableLimit); + b2Dump(" jd.lowerAngle = %.9g;\n", joint->lowerAngle); + b2Dump(" jd.upperAngle = %.9g;\n", joint->upperAngle); + b2Dump(" jd.enableMotor = bool(%d);\n", joint->enableMotor); + b2Dump(" jd.motorSpeed = %.9g;\n", joint->motorSpeed); + b2Dump(" jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque); + b2Dump(" joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index); +} +#endif + +void b2DrawWheelJoint(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bodyB) +{ + B2_ASSERT(base->type == b2_wheelJoint); + + b2WheelJoint* joint = &base->wheelJoint; + + b2Transform xfA = bodyA->transform; + b2Transform xfB = bodyB->transform; + b2Vec2 pA = b2TransformPoint(xfA, base->localAnchorA); + b2Vec2 pB = b2TransformPoint(xfB, base->localAnchorB); + + b2Vec2 axis = b2RotateVector(xfA.q, joint->localAxisA); + + b2Color c1 = {0.7f, 0.7f, 0.7f, 1.0f}; + b2Color c2 = {0.3f, 0.9f, 0.3f, 1.0f}; + b2Color c3 = {0.9f, 0.3f, 0.3f, 1.0f}; + b2Color c4 = {0.3f, 0.3f, 0.9f, 1.0f}; + b2Color c5 = {0.4f, 0.4f, 0.4f, 1.0f}; + + draw->DrawSegment(pA, pB, c5, draw->context); + + if (joint->enableLimit) + { + b2Vec2 lower = b2MulAdd(pA, joint->lowerTranslation, axis); + b2Vec2 upper = b2MulAdd(pA, joint->upperTranslation, axis); + b2Vec2 perp = b2LeftPerp(axis); + draw->DrawSegment(lower, upper, c1, draw->context); + draw->DrawSegment(b2MulSub(lower, 0.5f, perp), b2MulAdd(lower, 0.5f, perp), c2, draw->context); + draw->DrawSegment(b2MulSub(upper, 0.5f, perp), b2MulAdd(upper, 0.5f, perp), c3, draw->context); + } + else + { + draw->DrawSegment(b2MulSub(pA, 1.0f, axis), b2MulAdd(pA, 1.0f, axis), c1, draw->context); + } + + draw->DrawPoint(pA, 5.0f, c1, draw->context); + draw->DrawPoint(pB, 5.0f, c4, draw->context); +} diff --git a/src/world.c b/src/world.c index 2a461e61..9d0c8210 100644 --- a/src/world.c +++ b/src/world.c @@ -6,6 +6,7 @@ #include "world.h" #include "allocate.h" +#include "arena_allocator.h" #include "array.h" #include "bitset.h" #include "block_allocator.h" @@ -19,7 +20,6 @@ #include "pool.h" #include "shape.h" #include "solver_data.h" -#include "stack_allocator.h" #include "box2d/aabb.h" #include "box2d/box2d.h" @@ -572,7 +572,7 @@ void b2World_Step(b2WorldId worldId, float timeStep, int32_t velocityIterations, // Make sure all tasks that were started were also finished B2_ASSERT(world->activeTaskCount == 0); - + b2TracyCZoneEnd(world_step); } @@ -794,7 +794,7 @@ void b2World_Draw(b2WorldId worldId, b2DebugDraw* draw) b2Vec2 p = b2TransformPoint(transform, offset); char buffer[32]; - sprintf(buffer, "%.1f", body->mass); + sprintf(buffer, "%.2f", body->mass); draw->DrawString(p, buffer, draw->context); } } @@ -916,7 +916,6 @@ b2ContactEvents b2World_GetContactEvents(b2WorldId worldId) b2ContactEvents events = {world->contactBeginArray, world->contactEndArray, beginCount, endCount}; return events; - } bool b2World_IsValid(b2WorldId id)