diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index c3defb11..f6845241 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -77,6 +77,7 @@ BOX2D_API bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point); BOX2D_API b2JointId b2World_CreateMouseJoint(b2WorldId worldId, const b2MouseJointDef* def); BOX2D_API b2JointId b2World_CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDef* def); +BOX2D_API b2JointId b2World_CreateWeldJoint(b2WorldId worldId, const b2WeldJointDef* def); BOX2D_API void b2World_DestroyJoint(b2JointId jointId); BOX2D_API void b2MouseJoint_SetTarget(b2JointId jointId, b2Vec2 target); @@ -85,6 +86,7 @@ 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); /// This function receives shapes found in the AABB query. /// @return true if the query should continue diff --git a/include/box2d/joint_types.h b/include/box2d/joint_types.h index cb9fa245..a78f3f77 100644 --- a/include/box2d/joint_types.h +++ b/include/box2d/joint_types.h @@ -115,3 +115,49 @@ static inline struct b2RevoluteJointDef b2DefaultRevoluteJointDef(void) def.collideConnected = false; return def; } + +typedef struct b2WeldJointDef +{ + /// 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 bodyB angle minus bodyA angle in the reference state (radians). + /// This defines the zero angle for the joint limit. + float referenceAngle; + + /// Stiffness expressed as hertz (oscillations per second). Use zero for maximum stiffness. + float linearHertz; + float angularHertz; + + /// Damping ratio, non-dimensional. Use 1 for critical damping. + float linearDampingRatio; + float angularDampingRatio; + + /// Set this flag to true if the attached bodies should collide. + bool collideConnected; +} b2WeldJointDef; + +static inline struct b2WeldJointDef b2DefaultWeldJointDef(void) +{ + b2WeldJointDef def = {0}; + 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.referenceAngle = 0.0f; + def.linearHertz = 0.0f; + def.angularHertz = 0.0f; + def.linearDampingRatio = 1.0f; + def.angularDampingRatio = 1.0f; + def.collideConnected = false; + return def; +} \ No newline at end of file diff --git a/include/box2d/math.h b/include/box2d/math.h index 315fefa0..a287765e 100644 --- a/include/box2d/math.h +++ b/include/box2d/math.h @@ -18,9 +18,11 @@ extern "C" #define B2_CLAMP(A, B, C) B2_MIN(B2_MAX(A, B), C) static const b2Vec2 b2Vec2_zero = {0.0f, 0.0f}; +static const b2Vec3 b2Vec3_zero = {0.0f, 0.0f, 0.0f}; static const b2Rot b2Rot_identity = {0.0f, 1.0f}; static const b2Transform b2Transform_identity = {{0.0f, 0.0f}, {0.0f, 1.0f}}; static const b2Mat22 b2Mat22_zero = {{0.0f, 0.0f}, {0.0f, 0.0f}}; +static const b2Mat33 b2Mat33_zero = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}; bool b2IsValid(float a); bool b2IsValidVec2(b2Vec2 v); @@ -43,6 +45,19 @@ static inline float b2Cross(b2Vec2 a, b2Vec2 b) return a.x * b.y - a.y * b.x; } +/// Perform the dot product on two 3-vectors. +static inline float b2Dot3(b2Vec3 a, b2Vec3 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} + +/// Perform the cross product on two 3-vectors. +static inline b2Vec3 b2Cross3(b2Vec3 a, b2Vec3 b) +{ + return B2_LITERAL(b2Vec3){a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x}; +} + + /// Perform the cross product on a vector and a scalar. In 2D this produces /// a vector. static inline b2Vec2 b2CrossVS(b2Vec2 v, float s) @@ -305,8 +320,7 @@ static inline b2Mat22 b2GetInverse22(b2Mat22 A) return B; } -/// Solve A * x = b, where b is a column vector. This is more efficient -/// than computing the inverse in one-shot cases. +/// Solve A * x = b, where b is a column vector. static inline b2Vec2 b2Solve22(b2Mat22 A, b2Vec2 b) { float a11 = A.cx.x, a12 = A.cy.x, a21 = A.cx.y, a22 = A.cy.y; @@ -319,6 +333,21 @@ static inline b2Vec2 b2Solve22(b2Mat22 A, b2Vec2 b) return x; } +/// Solve A * x = b, where b is a column vector. +static inline b2Vec3 b2Solve33(b2Mat33 A, b2Vec3 b) +{ + float det = b2Dot3(A.cx, b2Cross3(A.cy, A.cz)); + if (det != 0.0f) + { + det = 1.0f / det; + } + b2Vec3 x; + x.x = det * b2Dot3(b, b2Cross3(A.cy, A.cz)); + x.y = det * b2Dot3(A.cx, b2Cross3(b, A.cz)); + x.z = det * b2Dot3(A.cx, b2Cross3(A.cy, b)); + return x; +} + #ifdef __cplusplus } #endif diff --git a/include/box2d/types.h b/include/box2d/types.h index a77aa0ea..a3cab401 100644 --- a/include/box2d/types.h +++ b/include/box2d/types.h @@ -27,6 +27,12 @@ typedef struct b2Vec2 float x, y; } b2Vec2; +/// 3D vector +typedef struct b2Vec3 +{ + float x, y, z; +} b2Vec3; + /// 2D rotation typedef struct b2Rot { @@ -48,6 +54,13 @@ typedef struct b2Mat22 b2Vec2 cx, cy; } b2Mat22; +/// A 3-by-3 Matrix +typedef struct b2Mat33 +{ + /// columns + b2Vec3 cx, cy, cz; +} b2Mat33; + /// Axis-aligned bounding box typedef struct b2AABB { diff --git a/samples/collection/behavior.cpp b/samples/collection/behavior.cpp index a53a1ef6..a65d6118 100644 --- a/samples/collection/behavior.cpp +++ b/samples/collection/behavior.cpp @@ -218,7 +218,7 @@ class OverlapRecovery : public Sample b2Polygon box = b2MakeBox(extent, extent); - int count = 4; + int count = 2; float fraction = 0.75f; float y = fraction * extent; while (count > 0) diff --git a/samples/collection/sample_joints.cpp b/samples/collection/sample_joints.cpp index a154ee5c..f91fd6eb 100644 --- a/samples/collection/sample_joints.cpp +++ b/samples/collection/sample_joints.cpp @@ -1,15 +1,18 @@ // SPDX-FileCopyrightText: 2022 Erin Catto // SPDX-License-Identifier: MIT +#include "sample.h" + #include "box2d/box2d.h" #include "box2d/geometry.h" -#include "sample.h" +#include "box2d/hull.h" + +//#include +#include -// TODO_ERIN test more joint types -// TODO_ERIN try to stabilize revolute class BenchmarkJointGrid : public Sample { -public: + public: BenchmarkJointGrid(const Settings& settings) : Sample(settings) { @@ -102,12 +105,7 @@ class Bridge : public Sample b2BodyId groundId = b2_nullBodyId; { b2BodyDef bd = b2DefaultBodyDef(); - bd.position = {0.0f, -1.0f}; groundId = b2World_CreateBody(m_worldId, &bd); - - //b2Segment segment = {{-80.0f, 0.0f}, {80.0f, 0.0f}}; - //b2ShapeDef sd = b2DefaultShapeDef(); - //b2Body_CreateSegment(groundId, &sd, &segment); } { @@ -132,6 +130,8 @@ class Bridge : public Sample jd.bodyIdB = bodyId; jd.localAnchorA = b2Body_GetLocalPoint(jd.bodyIdA, pivot); jd.localAnchorB = b2Body_GetLocalPoint(jd.bodyIdB, pivot); + // jd.enableMotor = true; + // jd.maxMotorTorque = 1000.0f; b2World_CreateRevoluteJoint(m_worldId, &jd); prevBodyId = bodyId; @@ -142,47 +142,41 @@ class Bridge : public Sample jd.bodyIdB = groundId; jd.localAnchorA = b2Body_GetLocalPoint(jd.bodyIdA, pivot); jd.localAnchorB = b2Body_GetLocalPoint(jd.bodyIdB, pivot); + // jd.enableMotor = true; + // jd.maxMotorTorque = 1000.0f; b2World_CreateRevoluteJoint(m_worldId, &jd); } -#if 0 - for (int32 i = 0; i < 2; ++i) + for (int32_t i = 0; i < 2; ++i) { - b2Vec2 vertices[3]; - vertices[0].Set(-0.5f, 0.0f); - vertices[1].Set(0.5f, 0.0f); - vertices[2].Set(0.0f, 1.5f); + b2Vec2 vertices[3] = {{-0.5f, 0.0f}, {0.5f, 0.0f}, {0.0f, 1.5f}}; - b2PolygonShape shape; - shape.Set(vertices, 3); + b2Hull hull = b2ComputeHull(vertices, 3); + b2Polygon triangle = b2MakePolygon(&hull, 0.0f); - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; + b2ShapeDef sd = b2DefaultShapeDef(); + sd.density = 20.0f; - b2BodyDef bd; + b2BodyDef bd = b2DefaultBodyDef(); bd.type = b2_dynamicBody; - bd.position.Set(-8.0f + 8.0f * i, 12.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); + bd.position = {-8.0f + 8.0f * i, 22.0f}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bd); + b2Body_CreatePolygon(bodyId, &sd, &triangle); } - for (int32 i = 0; i < 3; ++i) + for (int32_t i = 0; i < 3; ++i) { - b2CircleShape shape; - shape.m_radius = 0.5f; + b2Circle circle = {{0.0f, 0.0f}, 0.5f}; - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; + b2ShapeDef sd = b2DefaultShapeDef(); + sd.density = 20.0f; - b2BodyDef bd; + b2BodyDef bd = b2DefaultBodyDef(); bd.type = b2_dynamicBody; - bd.position.Set(-6.0f + 6.0f * i, 10.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); + bd.position = {-6.0f + 6.0f * i, 25.0f}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bd); + b2Body_CreateCircle(bodyId, &sd, &circle); } -#endif } static Sample* Create(const Settings& settings) @@ -192,3 +186,181 @@ class Bridge : public Sample }; static int sampleBridgeIndex = RegisterSample("Joints", "Bridge", Bridge::Create); + +class BallAndChain : public Sample +{ + public: + enum + { + e_count = 30 + }; + + BallAndChain(const Settings& settings) + : Sample(settings) + { + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bd = b2DefaultBodyDef(); + groundId = b2World_CreateBody(m_worldId, &bd); + } + + m_maxMotorTorque = 0.0f; + + { + float hx = 0.5f; + b2Polygon box = b2MakeBox(hx, 0.125f); + + b2ShapeDef sd = b2DefaultShapeDef(); + sd.density = 20.0f; + + b2RevoluteJointDef jd = b2DefaultRevoluteJointDef(); + + int32_t jointIndex = 0; + + b2BodyId prevBodyId = groundId; + for (int32_t i = 0; i < e_count; ++i) + { + b2BodyDef bd = b2DefaultBodyDef(); + bd.type = b2_dynamicBody; + bd.position = {(1.0f + 2.0f * i) * hx, e_count * hx}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bd); + b2Body_CreatePolygon(bodyId, &sd, &box); + + b2Vec2 pivot = {(2.0f * i) * hx, e_count * hx}; + jd.bodyIdA = prevBodyId; + jd.bodyIdB = bodyId; + jd.localAnchorA = b2Body_GetLocalPoint(jd.bodyIdA, pivot); + jd.localAnchorB = b2Body_GetLocalPoint(jd.bodyIdB, pivot); + jd.enableMotor = true; + jd.maxMotorTorque = 0.0f; + m_jointIds[jointIndex] = b2World_CreateRevoluteJoint(m_worldId, &jd); + jointIndex += 1; + + prevBodyId = bodyId; + } + + b2Circle circle = {{0.0f, 0.0f}, 4.0f}; + + b2BodyDef bd = b2DefaultBodyDef(); + bd.type = b2_dynamicBody; + bd.position = {(1.0f + 2.0f * e_count) * hx + circle.radius - hx, e_count * hx}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bd); + b2Body_CreateCircle(bodyId, &sd, &circle); + + b2Vec2 pivot = {(2.0f * e_count) * hx, e_count * hx}; + jd.bodyIdA = prevBodyId; + jd.bodyIdB = bodyId; + jd.localAnchorA = b2Body_GetLocalPoint(jd.bodyIdA, pivot); + jd.localAnchorB = b2Body_GetLocalPoint(jd.bodyIdB, pivot); + jd.enableMotor = true; + jd.maxMotorTorque = 0.0f; + m_jointIds[jointIndex] = b2World_CreateRevoluteJoint(m_worldId, &jd); + jointIndex += 1; + assert(jointIndex == e_count + 1); + } + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 300.0f), ImGuiCond_Once); + ImGui::SetNextWindowSize(ImVec2(260.0f, 60.0f)); + ImGui::Begin("Options", nullptr, ImGuiWindowFlags_NoResize); + + bool updateFriction = ImGui::SliderFloat("Joint Friction", &m_maxMotorTorque, 0.0f, 100000.0f, "%1.f"); + if (updateFriction) + { + for (int32_t i = 0; i <= e_count; ++i) + { + b2RevoluteJoint_SetMaxMotorTorque(m_jointIds[i], m_maxMotorTorque); + } + } + + ImGui::End(); + } + + static Sample* Create(const Settings& settings) + { + return new BallAndChain(settings); + } + + b2JointId m_jointIds[e_count + 1]; + float m_maxMotorTorque; +}; + +static int sampleBallAndChainIndex = RegisterSample("Joints", "BallAndChain", BallAndChain::Create); + + +class Cantilever : public Sample +{ + public: + enum + { + e_count = 8 + }; + + Cantilever(const Settings& settings) + : Sample(settings) + { + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bd = b2DefaultBodyDef(); + groundId = b2World_CreateBody(m_worldId, &bd); + } + + { + float hx = 0.5f; + b2Polygon box = b2MakeBox(hx, 0.125f); + + b2ShapeDef sd = b2DefaultShapeDef(); + sd.density = 20.0f; + + b2WeldJointDef jd = b2DefaultWeldJointDef(); + + b2BodyId prevBodyId = groundId; + for (int32_t i = 0; i < e_count; ++i) + { + b2BodyDef bd = b2DefaultBodyDef(); + bd.type = b2_dynamicBody; + bd.position = {(1.0f + 2.0f * i) * hx, e_count * hx}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bd); + b2Body_CreatePolygon(bodyId, &sd, &box); + + b2Vec2 pivot = {(2.0f * i) * hx, e_count * hx}; + jd.bodyIdA = prevBodyId; + jd.bodyIdB = bodyId; + jd.localAnchorA = b2Body_GetLocalPoint(jd.bodyIdA, pivot); + jd.localAnchorB = b2Body_GetLocalPoint(jd.bodyIdB, pivot); + jd.linearHertz = 5.0f; + b2World_CreateWeldJoint(m_worldId, &jd); + + prevBodyId = bodyId; + } + + //b2Circle circle = {{0.0f, 0.0f}, 4.0f}; + + //b2BodyDef bd = b2DefaultBodyDef(); + //bd.type = b2_dynamicBody; + //bd.position = {(1.0f + 2.0f * e_count) * hx + circle.radius - hx, e_count * hx}; + //b2BodyId bodyId = b2World_CreateBody(m_worldId, &bd); + //b2Body_CreateCircle(bodyId, &sd, &circle); + + //b2Vec2 pivot = {(2.0f * e_count) * hx, e_count * hx}; + //jd.bodyIdA = prevBodyId; + //jd.bodyIdB = bodyId; + //jd.localAnchorA = b2Body_GetLocalPoint(jd.bodyIdA, pivot); + //jd.localAnchorB = b2Body_GetLocalPoint(jd.bodyIdB, pivot); + //jd.enableMotor = true; + //jd.maxMotorTorque = 0.0f; + //m_jointIds[jointIndex] = b2World_CreateRevoluteJoint(m_worldId, &jd); + //jointIndex += 1; + //assert(jointIndex == e_count + 1); + } + } + + static Sample* Create(const Settings& settings) + { + return new Cantilever(settings); + } +}; + +static int sampleCantileverIndex = RegisterSample("Joints", "Cantilever", Cantilever::Create); diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4affce80..a7203fff 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -45,6 +45,7 @@ set(BOX2D_SOURCE_FILES table.h timer.c types.c + weld_joint.c world.c world.h ) diff --git a/src/joint.c b/src/joint.c index 8f2e6597..858aa970 100644 --- a/src/joint.c +++ b/src/joint.c @@ -249,6 +249,50 @@ b2JointId b2World_CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDe return jointId; } +b2JointId b2World_CreateWeldJoint(b2WorldId worldId, const b2WeldJointDef* 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_weldJoint; + + joint->localAnchorA = def->localAnchorA; + joint->localAnchorB = def->localAnchorB; + + b2WeldJoint empty = {0}; + joint->weldJoint = empty; + joint->weldJoint.referenceAngle = def->referenceAngle; + joint->weldJoint.linearHertz = def->linearHertz; + joint->weldJoint.linearDampingRatio = def->linearDampingRatio; + joint->weldJoint.angularHertz = def->angularHertz; + joint->weldJoint.angularDampingRatio = def->angularDampingRatio; + joint->weldJoint.impulse = b2Vec3_zero; + + // 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; +} + void b2World_DestroyJoint(b2JointId jointId) { b2World* world = b2GetWorldFromIndex(jointId.world); @@ -325,6 +369,7 @@ void b2World_DestroyJoint(b2JointId jointId) extern void b2PrepareMouse(b2Joint* base, const b2StepContext* context); extern void b2PrepareRevolute(b2Joint* base, const b2StepContext* context); +extern void b2PrepareWeld(b2Joint* base, const b2StepContext* context); void b2PrepareJoint(b2Joint* joint, const b2StepContext* context) { @@ -338,6 +383,10 @@ void b2PrepareJoint(b2Joint* joint, const b2StepContext* context) b2PrepareRevolute(joint, context); break; + case b2_weldJoint: + b2PrepareWeld(joint, context); + break; + default: B2_ASSERT(false); } @@ -358,12 +407,16 @@ void b2SolveJointVelocity(b2Joint* joint, const b2StepContext* context) b2SolveRevoluteVelocity(joint, context); break; + case b2_weldJoint: + break; + default: B2_ASSERT(false); } } extern void b2SolveRevoluteVelocitySoft(b2Joint* base, const b2StepContext* context, bool removeOverlap); +extern void b2SolveWeldVelocitySoft(b2Joint* base, const b2StepContext* context, bool removeOverlap); void b2SolveJointVelocitySoft(b2Joint* joint, const b2StepContext* context, bool removeOverlap) { @@ -380,6 +433,10 @@ void b2SolveJointVelocitySoft(b2Joint* joint, const b2StepContext* context, bool b2SolveRevoluteVelocitySoft(joint, context, removeOverlap); break; + case b2_weldJoint: + b2SolveWeldVelocitySoft(joint, context, removeOverlap); + break; + default: B2_ASSERT(false); } diff --git a/src/joint.h b/src/joint.h index 047ae305..08104dc4 100644 --- a/src/joint.h +++ b/src/joint.h @@ -93,6 +93,31 @@ typedef struct b2RevoluteJoint float axialMass; } b2RevoluteJoint; +typedef struct b2WeldJoint +{ + // Solver shared + float referenceAngle; + float linearHertz; + float linearDampingRatio; + float angularHertz; + float angularDampingRatio; + float linearBiasCoefficient; + float linearMassCoefficient; + float linearImpulseCoefficient; + float angularBiasCoefficient; + float angularMassCoefficient; + float angularImpulseCoefficient; + b2Vec3 impulse; + + // Solver temp + b2Vec2 localCenterA; + b2Vec2 localCenterB; + float invMassA; + float invMassB; + float invIA; + float invIB; +} b2WeldJoint; + /// 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 @@ -114,6 +139,7 @@ typedef struct b2Joint { b2MouseJoint mouseJoint; b2RevoluteJoint revoluteJoint; + b2WeldJoint weldJoint; }; bool isMarked; diff --git a/src/revolute_joint.c b/src/revolute_joint.c index 79b39ada..ca47296a 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -87,8 +87,9 @@ void b2PrepareRevolute(b2Joint* base, b2StepContext* context) } // TODO_ERIN softness experiment - const float hertz = 120.0f; - const float zeta = 4.0f; + // hertz = 6.0f * subStep/dt + 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; @@ -119,7 +120,8 @@ void b2PrepareRevolute(b2Joint* base, b2StepContext* context) float dtRatio = context->dtRatio; // Scale impulses to support a variable time step. - joint->impulse = b2MulSV(dtRatio, joint->impulse); + //joint->impulse = b2MulSV(dtRatio, joint->impulse); + joint->impulse = b2Vec2_zero; joint->motorImpulse *= dtRatio; joint->lowerImpulse *= dtRatio; joint->upperImpulse *= dtRatio; @@ -507,6 +509,24 @@ float b2RevoluteJoint_GetMotorTorque(b2JointId jointId, float inverseTimeStep) return inverseTimeStep * joint->revoluteJoint.motorImpulse; } +void b2RevoluteJoint_SetMaxMotorTorque(b2JointId jointId, float torque) +{ + b2World* world = b2GetWorldFromIndex(jointId.world); + B2_ASSERT(world->locked == false); + if (world->locked) + { + 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); + B2_ASSERT(joint->type == b2_revoluteJoint); + joint->revoluteJoint.maxMotorTorque = torque; +} + #if 0 void b2RevoluteJoint::Dump() { diff --git a/src/solver_data.h b/src/solver_data.h index b09797d2..31372d72 100644 --- a/src/solver_data.h +++ b/src/solver_data.h @@ -14,6 +14,7 @@ typedef struct b2StepContext // inverse time step (0 if dt == 0). float inv_dt; + // TODO_ERIN eliminate support for variable time step // ratio between current and previous time step (dt * inv_dt0) float dtRatio; diff --git a/src/weld_joint.c b/src/weld_joint.c new file mode 100644 index 00000000..0d5ee4b5 --- /dev/null +++ b/src/weld_joint.c @@ -0,0 +1,191 @@ +// 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 b2PrepareWeld(b2Joint* base, b2StepContext* context) +{ + B2_ASSERT(base->type == b2_weldJoint); + + 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); + + b2WeldJoint* joint = &base->weldJoint; + joint->localCenterA = bodyA->localCenter; + joint->invMassA = bodyA->invMass; + joint->invIA = bodyA->invI; + + joint->localCenterB = bodyB->localCenter; + joint->invMassB = bodyB->invMass; + joint->invIB = bodyB->invI; + + const float h = context->dt; + + float linearHertz = joint->linearHertz; + if (linearHertz == 0.0f) + { + linearHertz = 0.25f * context->velocityIterations * context->inv_dt; + } + + { + const float zeta = joint->linearDampingRatio; + const float omega = 2.0f * b2_pi * linearHertz; + joint->linearBiasCoefficient = omega / (2.0f * zeta + h * omega); + float c = h * omega * (2.0f * zeta + h * omega); + joint->linearImpulseCoefficient = 1.0f / (1.0f + c); + joint->linearMassCoefficient = c * joint->linearImpulseCoefficient; + } + + float angularHertz = joint->angularHertz; + if (angularHertz == 0.0f) + { + angularHertz = 0.25f * context->velocityIterations * context->inv_dt; + } + + { + const float zeta = joint->angularDampingRatio; + const float omega = 2.0f * b2_pi * angularHertz; + joint->angularBiasCoefficient = omega / (2.0f * zeta + h * omega); + float c = h * omega * (2.0f * zeta + h * omega); + joint->angularImpulseCoefficient = 1.0f / (1.0f + c); + joint->angularMassCoefficient = c * joint->angularImpulseCoefficient; + } + + joint->impulse = b2Vec3_zero; +} + +void b2SolveWeldVelocitySoft(b2Joint* base, const b2StepContext* context, bool removeOverlap) +{ + B2_ASSERT(base->type == b2_weldJoint); + + b2WeldJoint* joint = &base->weldJoint; + + b2Body* bodyA = context->bodies + base->edges[0].bodyIndex; + b2Body* bodyB = context->bodies + base->edges[1].bodyIndex; + + b2Vec2 vA = bodyA->linearVelocity; + float wA = bodyA->angularVelocity; + b2Vec2 vB = bodyB->linearVelocity; + float wB = bodyB->angularVelocity; + + const b2Vec2 cA = b2Add(bodyA->position, bodyA->deltaPosition); + const float aA = bodyA->angle + bodyA->deltaAngle; + const b2Vec2 cB = b2Add(bodyB->position, bodyB->deltaPosition); + const float aB = bodyB->angle + bodyB->deltaAngle; + + float mA = joint->invMassA, mB = joint->invMassB; + float iA = joint->invIA, iB = joint->invIB; + + b2Rot qA = b2MakeRot(aA); + b2Rot qB = b2MakeRot(aB); + + b2Vec2 rA = b2RotateVector(qA, b2Sub(base->localAnchorA, joint->localCenterA)); + b2Vec2 rB = b2RotateVector(qB, b2Sub(base->localAnchorB, joint->localCenterB)); + + b2Mat33 K; + K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; + K.cy.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; + K.cz.x = -rA.y * iA - rB.y * iB; + K.cx.y = K.cy.x; + K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; + K.cz.y = rA.x * iA + rB.x * iB; + K.cx.z = K.cz.x; + K.cy.z = K.cz.y; + K.cz.z = iA + iB; + + b2Vec2 Cdot1 = b2Add(b2Sub(vB, vA), b2Sub(b2CrossSV(wB, rB), b2CrossSV(wA, rA))); + float Cdot2 = wB - wA; + + float linearBiasScale = 0.0f; + float linearMassScale = 1.0f; + float linearImpulseScale = 0.0f; + float angularBiasScale = 0.0f; + float angularMassScale = 1.0f; + float angularImpulseScale = 0.0f; + if (removeOverlap) + { + linearBiasScale = joint->linearBiasCoefficient; + linearMassScale = joint->linearMassCoefficient; + linearImpulseScale = joint->linearImpulseCoefficient; + angularBiasScale = joint->angularBiasCoefficient; + angularMassScale = joint->angularMassCoefficient; + angularImpulseScale = joint->angularImpulseCoefficient; + } + + b2Vec2 C1 = b2Add(b2Sub(cB, cA), b2Sub(rB, rA)); + float C2 = aB - aA - joint->referenceAngle; + + b2Vec3 c; + c.x = Cdot1.x + linearBiasScale * C1.x; + c.y = Cdot1.y + linearBiasScale * C1.y; + c.z = Cdot2 + angularBiasScale * C2; + + b2Vec3 b = b2Solve33(K, c); + b2Vec3 impulse; + impulse.x = -linearMassScale * b.x - linearImpulseScale * joint->impulse.x; + impulse.y = -linearMassScale * b.y - linearImpulseScale * joint->impulse.y; + impulse.z = -angularMassScale * b.z - angularImpulseScale * joint->impulse.z; + + joint->impulse.x += impulse.x; + joint->impulse.y += impulse.y; + joint->impulse.z += impulse.z; + + b2Vec2 P = {impulse.x, impulse.y}; + + vA = b2MulSub(vA, mA, P); + wA -= iA * (b2Cross(rA, P) + impulse.z); + + vB = b2MulAdd(vB, mB, P); + wB += iB * (b2Cross(rB, P) + impulse.z); + + bodyA->linearVelocity = vA; + bodyA->angularVelocity = wA; + bodyB->linearVelocity = vB; + bodyB->angularVelocity = wB; +} + +#if 0 +void b2WeldJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2WeldJointDef 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