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)