From 8815e0963d2c042309ab302fadcea47034f352cb Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sun, 6 Sep 2020 10:57:23 -0700 Subject: [PATCH] Fix for issue 632 (#642) - Fixes issue #632 - Originally the idea was to rename rope to cloth to avoid confusion with the rope joint. However, I realized the distance joint could be extended to cover the rope joint functionality. - Extended distance joint to have a minimum and maximum limit - Removed rope joint - This is API breaking: 0 stiffness now means the spring is turned off rather than making the joint rigid. --- include/box2d/b2_distance_joint.h | 59 ++++- include/box2d/b2_rope_joint.h | 118 --------- include/box2d/box2d.h | 1 - src/CMakeLists.txt | 2 - src/dynamics/b2_distance_joint.cpp | 223 ++++++++++++++--- src/dynamics/b2_joint.cpp | 12 - src/dynamics/b2_prismatic_joint.cpp | 1 - src/dynamics/b2_rope_joint.cpp | 234 ------------------ testbed/CMakeLists.txt | 3 +- testbed/settings.h | 2 +- testbed/test.cpp | 8 +- testbed/tests/distance_joint.cpp | 207 ++++------------ testbed/tests/dominos.cpp | 2 + testbed/tests/web.cpp | 218 ++++++++++++++++ .../{rope_joint.cpp => wrecking_ball.cpp} | 83 ++++--- 15 files changed, 571 insertions(+), 602 deletions(-) delete mode 100644 include/box2d/b2_rope_joint.h delete mode 100644 src/dynamics/b2_rope_joint.cpp create mode 100644 testbed/tests/web.cpp rename testbed/tests/{rope_joint.cpp => wrecking_ball.cpp} (59%) diff --git a/include/box2d/b2_distance_joint.h b/include/box2d/b2_distance_joint.h index b339e9239..ad3054ad0 100644 --- a/include/box2d/b2_distance_joint.h +++ b/include/box2d/b2_distance_joint.h @@ -26,10 +26,9 @@ #include "b2_joint.h" /// Distance joint definition. This requires defining an anchor point on both -/// bodies and the non-zero length of the distance joint. The definition uses local anchor points -/// so that the initial configuration can violate the constraint -/// slightly. This helps when saving and loading a game. -/// @warning Do not use a zero or short length. +/// bodies and the non-zero distance of the distance joint. The definition uses +/// local anchor points so that the initial configuration can violate the +/// constraint slightly. This helps when saving and loading a game. struct b2DistanceJointDef : public b2JointDef { b2DistanceJointDef() @@ -38,12 +37,14 @@ struct b2DistanceJointDef : public b2JointDef localAnchorA.Set(0.0f, 0.0f); localAnchorB.Set(0.0f, 0.0f); length = 1.0f; + minLength = 0.0f; + maxLength = FLT_MAX; stiffness = 0.0f; damping = 0.0f; } - /// Initialize the bodies, anchors, and length using the world - /// anchors. + /// Initialize the bodies, anchors, and rest length using world space anchors. + /// The minimum and maximum lengths are set to the rest length. void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchorA, const b2Vec2& anchorB); @@ -53,10 +54,16 @@ struct b2DistanceJointDef : public b2JointDef /// The local anchor point relative to bodyB's origin. b2Vec2 localAnchorB; - /// The natural length between the anchor points. + /// The rest length of this joint. Clamped to a stable minimum value. float length; - /// The linear stiffness in N/m. A value of 0 disables softness. + /// Minimum length. Clamped to a stable minimum value. + float minLength; + + /// Maximum length. Must be greater than or equal to the minimum length. + float maxLength; + + /// The linear stiffness in N/m. float stiffness; /// The linear damping in N*s/m. @@ -86,11 +93,30 @@ class b2DistanceJoint : public b2Joint /// The local anchor point relative to bodyB's origin. const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - /// Set/get the natural length. - /// Manipulating the length can lead to non-physical behavior when the frequency is zero. - void SetLength(float length) { m_length = length; } + /// Get the rest length float GetLength() const { return m_length; } + /// Set the rest length + /// @returns clamped rest length + float SetLength(float length); + + /// Get the minimum length + float GetMinLength() const { return m_minLength; } + + /// Set the minimum length + /// @returns the clamped minimum length + float SetMinLength(float minLength); + + /// Get the maximum length + float GetMaxLength() const { return m_maxLength; } + + /// Set the maximum length + /// @returns the clamped maximum length + float SetMaxLength(float maxLength); + + /// Get the current length + float GetCurrentLength() const; + /// Set/get the linear stiffness in N/m void SetStiffness(float stiffness) { m_stiffness = stiffness; } float GetStiffness() const { return m_stiffness; } @@ -102,6 +128,9 @@ class b2DistanceJoint : public b2Joint /// Dump joint to dmLog void Dump() override; + /// + void Draw(b2Draw* draw) const override; + protected: friend class b2Joint; @@ -114,13 +143,17 @@ class b2DistanceJoint : public b2Joint float m_stiffness; float m_damping; float m_bias; + float m_length; + float m_minLength; + float m_maxLength; // Solver shared b2Vec2 m_localAnchorA; b2Vec2 m_localAnchorB; float m_gamma; float m_impulse; - float m_length; + float m_lowerImpulse; + float m_upperImpulse; // Solver temp int32 m_indexA; @@ -130,10 +163,12 @@ class b2DistanceJoint : public b2Joint b2Vec2 m_rB; b2Vec2 m_localCenterA; b2Vec2 m_localCenterB; + float m_currentLength; float m_invMassA; float m_invMassB; float m_invIA; float m_invIB; + float m_softMass; float m_mass; }; diff --git a/include/box2d/b2_rope_joint.h b/include/box2d/b2_rope_joint.h deleted file mode 100644 index b254b3a22..000000000 --- a/include/box2d/b2_rope_joint.h +++ /dev/null @@ -1,118 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_ROPE_JOINT_H -#define B2_ROPE_JOINT_H - -#include "b2_joint.h" - -/// Rope joint definition. This requires two body anchor points and -/// a maximum lengths. -/// Note: by default the connected objects will not collide. -/// see collideConnected in b2JointDef. -struct b2RopeJointDef : public b2JointDef -{ - b2RopeJointDef() - { - type = e_ropeJoint; - localAnchorA.Set(-1.0f, 0.0f); - localAnchorB.Set(1.0f, 0.0f); - maxLength = 0.0f; - } - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The maximum length of the rope. - /// Warning: this must be larger than b2_linearSlop or - /// the joint will have no effect. - float maxLength; -}; - -/// A rope joint enforces a maximum distance between two points -/// on two bodies. It has no other effect. -/// Warning: if you attempt to change the maximum length during -/// the simulation you will get some non-physical behavior. -/// A model that would allow you to dynamically modify the length -/// would have some sponginess, so I chose not to implement it -/// that way. See b2DistanceJoint if you want to dynamically -/// control length. -class b2RopeJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; - - b2Vec2 GetReactionForce(float inv_dt) const override; - float GetReactionTorque(float inv_dt) const override; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Set/Get the maximum length of the rope. - void SetMaxLength(float length) { m_maxLength = length; } - float GetMaxLength() const; - - // Get current length - float GetLength() const; - - /// Dump joint to dmLog - void Dump() override; - -protected: - - friend class b2Joint; - b2RopeJoint(const b2RopeJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float m_maxLength; - float m_length; - float m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_u; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float m_invMassA; - float m_invMassB; - float m_invIA; - float m_invIB; - float m_mass; -}; - -#endif diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index 71db86c58..55c695822 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -52,7 +52,6 @@ #include "b2_prismatic_joint.h" #include "b2_pulley_joint.h" #include "b2_revolute_joint.h" -#include "b2_rope_joint.h" #include "b2_weld_joint.h" #include "b2_wheel_joint.h" diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a68e91435..dac94f4e9 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -48,7 +48,6 @@ set(BOX2D_SOURCE_FILES dynamics/b2_prismatic_joint.cpp dynamics/b2_pulley_joint.cpp dynamics/b2_revolute_joint.cpp - dynamics/b2_rope_joint.cpp dynamics/b2_weld_joint.cpp dynamics/b2_wheel_joint.cpp dynamics/b2_world.cpp @@ -82,7 +81,6 @@ set(BOX2D_HEADER_FILES ../include/box2d/b2_pulley_joint.h ../include/box2d/b2_revolute_joint.h ../include/box2d/b2_rope.h - ../include/box2d/b2_rope_joint.h ../include/box2d/b2_settings.h ../include/box2d/b2_shape.h ../include/box2d/b2_stack_allocator.h diff --git a/src/dynamics/b2_distance_joint.cpp b/src/dynamics/b2_distance_joint.cpp index 9eea08559..8a0735d06 100644 --- a/src/dynamics/b2_distance_joint.cpp +++ b/src/dynamics/b2_distance_joint.cpp @@ -21,6 +21,7 @@ // SOFTWARE. #include "box2d/b2_body.h" +#include "box2d/b2_draw.h" #include "box2d/b2_distance_joint.h" #include "box2d/b2_time_step.h" @@ -48,7 +49,9 @@ void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2, localAnchorA = bodyA->GetLocalPoint(anchor1); localAnchorB = bodyB->GetLocalPoint(anchor2); b2Vec2 d = anchor2 - anchor1; - length = d.Length(); + length = b2Max(d.Length(), b2_linearSlop); + minLength = length; + maxLength = length; } b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def) @@ -56,13 +59,18 @@ b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def) { m_localAnchorA = def->localAnchorA; m_localAnchorB = def->localAnchorB; - m_length = def->length; + m_length = b2Max(def->length, b2_linearSlop); + m_minLength = b2Max(def->minLength, b2_linearSlop); + m_maxLength = b2Max(def->maxLength, m_minLength); m_stiffness = def->stiffness; m_damping = def->damping; - m_impulse = 0.0f; m_gamma = 0.0f; m_bias = 0.0f; + m_impulse = 0.0f; + m_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + m_currentLength = 0.0f; } void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) @@ -93,23 +101,29 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) m_u = cB + m_rB - cA - m_rA; // Handle singularity. - float length = m_u.Length(); - if (length > b2_linearSlop) + m_currentLength = m_u.Length(); + if (m_currentLength > b2_linearSlop) { - m_u *= 1.0f / length; + m_u *= 1.0f / m_currentLength; } else { m_u.Set(0.0f, 0.0f); + m_mass = 0.0f; + m_impulse = 0.0f; + m_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; } float crAu = b2Cross(m_rA, m_u); float crBu = b2Cross(m_rB, m_u); float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu; + m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - if (m_stiffness > 0.0f) + if (m_stiffness > 0.0f && m_minLength < m_maxLength) { - float C = length - m_length; + // soft + float C = m_currentLength - m_length; float d = m_damping; float k = m_stiffness; @@ -117,27 +131,31 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) // magic formulas float h = data.step.dt; - // gamma = 1 / (h * (d + h * k)), the extra factor of h in the denominator is since the lambda is an impulse, not a force + // gamma = 1 / (h * (d + h * k)) + // the extra factor of h in the denominator is since the lambda is an impulse, not a force m_gamma = h * (d + h * k); m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; m_bias = C * h * k * m_gamma; invMass += m_gamma; - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + m_softMass = invMass != 0.0f ? 1.0f / invMass : 0.0f; } else { + // rigid m_gamma = 0.0f; m_bias = 0.0f; - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + m_softMass = m_mass; } if (data.step.warmStarting) { // Scale the impulse to support a variable time step. m_impulse *= data.step.dtRatio; + m_lowerImpulse *= data.step.dtRatio; + m_upperImpulse *= data.step.dtRatio; - b2Vec2 P = m_impulse * m_u; + b2Vec2 P = (m_impulse + m_lowerImpulse - m_upperImpulse) * m_u; vA -= m_invMassA * P; wA -= m_invIA * b2Cross(m_rA, P); vB += m_invMassB * P; @@ -161,19 +179,85 @@ void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data) b2Vec2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; - // Cdot = dot(u, v + cross(w, r)) - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float Cdot = b2Dot(m_u, vpB - vpA); + if (m_minLength < m_maxLength) + { + if (m_stiffness > 0.0f) + { + // Cdot = dot(u, v + cross(w, r)) + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float Cdot = b2Dot(m_u, vpB - vpA); + + float impulse = -m_softMass * (Cdot + m_bias + m_gamma * m_impulse); + m_impulse += impulse; + + b2Vec2 P = impulse * m_u; + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); + } + + // lower + { + float C = m_currentLength - m_minLength; + float bias = b2Max(0.0f, C) * data.step.inv_dt; + + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float Cdot = b2Dot(m_u, vpB - vpA); + + float impulse = -m_mass * (Cdot + bias); + float oldImpulse = m_lowerImpulse; + m_lowerImpulse = b2Max(0.0f, m_lowerImpulse + impulse); + impulse = m_lowerImpulse - oldImpulse; + b2Vec2 P = impulse * m_u; + + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); + } + + // upper + { + float C = m_maxLength - m_currentLength; + float bias = b2Max(0.0f, C) * data.step.inv_dt; + + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float Cdot = b2Dot(m_u, vpA - vpB); + + float impulse = -m_mass * (Cdot + bias); + float oldImpulse = m_upperImpulse; + m_upperImpulse = b2Max(0.0f, m_upperImpulse + impulse); + impulse = m_upperImpulse - oldImpulse; + b2Vec2 P = -impulse * m_u; + + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); + } + } + else + { + // Equal limits - float impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse); - m_impulse += impulse; + // Cdot = dot(u, v + cross(w, r)) + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float Cdot = b2Dot(m_u, vpB - vpA); - b2Vec2 P = impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); + float impulse = -m_mass * Cdot; + m_impulse += impulse; + + b2Vec2 P = impulse * m_u; + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); + } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; @@ -183,12 +267,6 @@ void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data) bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data) { - if (m_stiffness > 0.0f) - { - // There is no position correction for soft distance constraints. - return true; - } - b2Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; @@ -201,8 +279,23 @@ bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data) b2Vec2 u = cB + rB - cA - rA; float length = u.Normalize(); - float C = length - m_length; - C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection); + float C; + if (m_minLength == m_maxLength) + { + C = length - m_minLength; + } + else if (length < m_minLength) + { + C = length - m_minLength; + } + else if (m_maxLength < length) + { + C = length - m_maxLength; + } + else + { + return true; + } float impulse = -m_mass * C; b2Vec2 P = impulse * u; @@ -242,6 +335,36 @@ float b2DistanceJoint::GetReactionTorque(float inv_dt) const return 0.0f; } +float b2DistanceJoint::SetLength(float length) +{ + m_impulse = 0.0f; + m_length = b2Max(b2_linearSlop, length); + return m_length; +} + +float b2DistanceJoint::SetMinLength(float minLength) +{ + m_lowerImpulse = 0.0f; + m_minLength = b2Clamp(minLength, b2_linearSlop, m_maxLength); + return m_minLength; +} + +float b2DistanceJoint::SetMaxLength(float maxLength) +{ + m_upperImpulse = 0.0f; + m_maxLength = b2Max(maxLength, m_minLength); + return m_maxLength; +} + +float b2DistanceJoint::GetCurrentLength() const +{ + b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA); + b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB); + b2Vec2 d = pB - pA; + float length = d.Length(); + return length; +} + void b2DistanceJoint::Dump() { int32 indexA = m_bodyA->m_islandIndex; @@ -254,7 +377,45 @@ void b2DistanceJoint::Dump() 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.length = %.9g;\n", m_length); + b2Dump(" jd.minLength = %.9g;\n", m_minLength); + b2Dump(" jd.maxLength = %.9g;\n", m_maxLength); b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); b2Dump(" jd.damping = %.9g;\n", m_damping); b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } + +void b2DistanceJoint::Draw(b2Draw* draw) const +{ + const b2Transform& xfA = m_bodyA->GetTransform(); + const b2Transform& xfB = m_bodyB->GetTransform(); + b2Vec2 pA = b2Mul(xfA, m_localAnchorA); + b2Vec2 pB = b2Mul(xfB, m_localAnchorB); + + b2Vec2 axis = pB - pA; + float length = axis.Normalize(); + + b2Color c1(0.7f, 0.7f, 0.7f); + b2Color c2(0.3f, 0.9f, 0.3f); + b2Color c3(0.9f, 0.3f, 0.3f); + b2Color c4(0.4f, 0.4f, 0.4f); + + draw->DrawSegment(pA, pB, c4); + + b2Vec2 pRest = pA + m_length * axis; + draw->DrawPoint(pRest, 8.0f, c1); + + if (m_minLength != m_maxLength) + { + if (m_minLength > b2_linearSlop) + { + b2Vec2 pMin = pA + m_minLength * axis; + draw->DrawPoint(pMin, 4.0f, c2); + } + + if (m_maxLength < FLT_MAX) + { + b2Vec2 pMax = pA + m_maxLength * axis; + draw->DrawPoint(pMax, 4.0f, c3); + } + } +} diff --git a/src/dynamics/b2_joint.cpp b/src/dynamics/b2_joint.cpp index 1f8ac2644..41addbbb2 100644 --- a/src/dynamics/b2_joint.cpp +++ b/src/dynamics/b2_joint.cpp @@ -31,7 +31,6 @@ #include "box2d/b2_prismatic_joint.h" #include "box2d/b2_pulley_joint.h" #include "box2d/b2_revolute_joint.h" -#include "box2d/b2_rope_joint.h" #include "box2d/b2_weld_joint.h" #include "box2d/b2_wheel_joint.h" #include "box2d/b2_world.h" @@ -157,13 +156,6 @@ b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) } break; - case e_ropeJoint: - { - void* mem = allocator->Allocate(sizeof(b2RopeJoint)); - joint = new (mem) b2RopeJoint(static_cast(def)); - } - break; - case e_motorJoint: { void* mem = allocator->Allocate(sizeof(b2MotorJoint)); @@ -220,10 +212,6 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) allocator->Free(joint, sizeof(b2FrictionJoint)); break; - case e_ropeJoint: - allocator->Free(joint, sizeof(b2RopeJoint)); - break; - case e_motorJoint: allocator->Free(joint, sizeof(b2MotorJoint)); break; diff --git a/src/dynamics/b2_prismatic_joint.cpp b/src/dynamics/b2_prismatic_joint.cpp index cb550170f..eb73997aa 100644 --- a/src/dynamics/b2_prismatic_joint.cpp +++ b/src/dynamics/b2_prismatic_joint.cpp @@ -607,7 +607,6 @@ void b2PrismaticJoint::Dump() b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } -/// void b2PrismaticJoint::Draw(b2Draw* draw) const { const b2Transform& xfA = m_bodyA->GetTransform(); diff --git a/src/dynamics/b2_rope_joint.cpp b/src/dynamics/b2_rope_joint.cpp deleted file mode 100644 index ebe800f82..000000000 --- a/src/dynamics/b2_rope_joint.cpp +++ /dev/null @@ -1,234 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_body.h" -#include "box2d/b2_rope_joint.h" -#include "box2d/b2_time_step.h" - - -// Limit: -// C = norm(pB - pA) - L -// u = (pB - pA) / norm(pB - pA) -// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA)) -// J = [-u -cross(rA, u) u cross(rB, u)] -// K = J * invM * JT -// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2 - -b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - - m_maxLength = def->maxLength; - - m_mass = 0.0f; - m_impulse = 0.0f; - m_length = 0.0f; -} - -void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - m_u = cB + m_rB - cA - m_rA; - - m_length = m_u.Length(); - - if (m_length > b2_linearSlop) - { - m_u *= 1.0f / m_length; - } - else - { - m_u.SetZero(); - m_mass = 0.0f; - m_impulse = 0.0f; - return; - } - - // Compute effective mass. - float crA = b2Cross(m_rA, m_u); - float crB = b2Cross(m_rB, m_u); - float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB; - - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - - if (data.step.warmStarting) - { - // Scale the impulse to support a variable time step. - m_impulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - // Cdot = dot(u, v + cross(w, r)) - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float C = m_length - m_maxLength; - float Cdot = b2Dot(m_u, vpB - vpA); - - // Predictive constraint. - if (C < 0.0f) - { - Cdot += data.step.inv_dt * C; - } - - float impulse = -m_mass * Cdot; - float oldImpulse = m_impulse; - m_impulse = b2Min(0.0f, m_impulse + impulse); - impulse = m_impulse - oldImpulse; - - b2Vec2 P = impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 u = cB + rB - cA - rA; - - m_length = u.Normalize(); - float C = m_length - m_maxLength; - - C = b2Clamp(C, 0.0f, b2_maxLinearCorrection); - - float impulse = -m_mass * C; - b2Vec2 P = impulse * u; - - cA -= m_invMassA * P; - aA -= m_invIA * b2Cross(rA, P); - cB += m_invMassB * P; - aB += m_invIB * b2Cross(rB, P); - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return m_length - m_maxLength < b2_linearSlop; -} - -b2Vec2 b2RopeJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2RopeJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2RopeJoint::GetReactionForce(float inv_dt) const -{ - b2Vec2 F = (inv_dt * m_impulse) * m_u; - return F; -} - -float b2RopeJoint::GetReactionTorque(float inv_dt) const -{ - B2_NOT_USED(inv_dt); - return 0.0f; -} - -float b2RopeJoint::GetMaxLength() const -{ - return m_maxLength; -} - -float b2RopeJoint::GetLength() const -{ - return m_length; -} - -void b2RopeJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Dump(" b2RopeJointDef 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.maxLength = %.9g;\n", m_maxLength); - b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/testbed/CMakeLists.txt b/testbed/CMakeLists.txt index 1827c109c..5ef305e09 100644 --- a/testbed/CMakeLists.txt +++ b/testbed/CMakeLists.txt @@ -55,7 +55,6 @@ set (TESTBED_SOURCE_FILES tests/restitution.cpp tests/revolute_joint.cpp tests/rope.cpp - tests/rope_joint.cpp tests/sensor.cpp tests/shape_cast.cpp tests/shape_editing.cpp @@ -66,7 +65,9 @@ set (TESTBED_SOURCE_FILES tests/tiles.cpp tests/time_of_impact.cpp tests/tumbler.cpp + tests/web.cpp tests/wheel_joint.cpp + tests/wrecking_ball.cpp ) add_executable(testbed ${TESTBED_SOURCE_FILES}) diff --git a/testbed/settings.h b/testbed/settings.h index 0813c074c..dcb8426a2 100644 --- a/testbed/settings.h +++ b/testbed/settings.h @@ -38,7 +38,7 @@ struct Settings m_velocityIterations = 8; m_positionIterations = 3; m_drawShapes = true; - m_drawJoints = false; + m_drawJoints = true; m_drawAABBs = false; m_drawContactPoints = false; m_drawContactNormals = false; diff --git a/testbed/test.cpp b/testbed/test.cpp index a6625fd19..b871fa06f 100644 --- a/testbed/test.cpp +++ b/testbed/test.cpp @@ -295,10 +295,10 @@ void Test::Step(Settings& settings) } uint32 flags = 0; - flags += settings.m_drawShapes * b2Draw::e_shapeBit; - flags += settings.m_drawJoints * b2Draw::e_jointBit; - flags += settings.m_drawAABBs * b2Draw::e_aabbBit; - flags += settings.m_drawCOMs * b2Draw::e_centerOfMassBit; + flags += settings.m_drawShapes * b2Draw::e_shapeBit; + flags += settings.m_drawJoints * b2Draw::e_jointBit; + flags += settings.m_drawAABBs * b2Draw::e_aabbBit; + flags += settings.m_drawCOMs * b2Draw::e_centerOfMassBit; g_debugDraw.SetFlags(flags); m_world->SetAllowSleeping(settings.m_enableSleep); diff --git a/testbed/tests/distance_joint.cpp b/testbed/tests/distance_joint.cpp index f24ddbc3d..389f403fa 100644 --- a/testbed/tests/distance_joint.cpp +++ b/testbed/tests/distance_joint.cpp @@ -21,6 +21,7 @@ // SOFTWARE. #include "test.h" +#include "imgui/imgui.h" // This tests distance joints, body destruction, and joint destruction. class DistanceJoint : public Test @@ -39,173 +40,71 @@ class DistanceJoint : public Test } { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.5f); - b2BodyDef bd; bd.type = b2_dynamicBody; + bd.angularDamping = 0.1f; - bd.position.Set(-5.0f, 5.0f); - m_bodies[0] = m_world->CreateBody(&bd); - m_bodies[0]->CreateFixture(&shape, 5.0f); + bd.position.Set(0.0f, 5.0f); + b2Body* body = m_world->CreateBody(&bd); - bd.position.Set(5.0f, 5.0f); - m_bodies[1] = m_world->CreateBody(&bd); - m_bodies[1]->CreateFixture(&shape, 5.0f); - - bd.position.Set(5.0f, 15.0f); - m_bodies[2] = m_world->CreateBody(&bd); - m_bodies[2]->CreateFixture(&shape, 5.0f); + b2PolygonShape shape; + shape.SetAsBox(0.5f, 0.5f); + body->CreateFixture(&shape, 5.0f); - bd.position.Set(-5.0f, 15.0f); - m_bodies[3] = m_world->CreateBody(&bd); - m_bodies[3]->CreateFixture(&shape, 5.0f); + m_hertz = 1.0f; + m_dampingRatio = 0.7f; b2DistanceJointDef jd; - b2Vec2 p1, p2, d; - - float frequencyHz = 2.0f; - float dampingRatio = 0.0f; - - jd.bodyA = ground; - jd.bodyB = m_bodies[0]; - jd.localAnchorA.Set(-10.0f, 0.0f); - jd.localAnchorB.Set(-0.5f, -0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[0] = m_world->CreateJoint(&jd); - - jd.bodyA = ground; - jd.bodyB = m_bodies[1]; - jd.localAnchorA.Set(10.0f, 0.0f); - jd.localAnchorB.Set(0.5f, -0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[1] = m_world->CreateJoint(&jd); - - jd.bodyA = ground; - jd.bodyB = m_bodies[2]; - jd.localAnchorA.Set(10.0f, 20.0f); - jd.localAnchorB.Set(0.5f, 0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[2] = m_world->CreateJoint(&jd); - - jd.bodyA = ground; - jd.bodyB = m_bodies[3]; - jd.localAnchorA.Set(-10.0f, 20.0f); - jd.localAnchorB.Set(-0.5f, 0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[3] = m_world->CreateJoint(&jd); - - jd.bodyA = m_bodies[0]; - jd.bodyB = m_bodies[1]; - jd.localAnchorA.Set(0.5f, 0.0f); - jd.localAnchorB.Set(-0.5f, 0.0f);; - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[4] = m_world->CreateJoint(&jd); - - jd.bodyA = m_bodies[1]; - jd.bodyB = m_bodies[2]; - jd.localAnchorA.Set(0.0f, 0.5f); - jd.localAnchorB.Set(0.0f, -0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[5] = m_world->CreateJoint(&jd); - - jd.bodyA = m_bodies[2]; - jd.bodyB = m_bodies[3]; - jd.localAnchorA.Set(-0.5f, 0.0f); - jd.localAnchorB.Set(0.5f, 0.0f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[6] = m_world->CreateJoint(&jd); - - jd.bodyA = m_bodies[3]; - jd.bodyB = m_bodies[0]; - jd.localAnchorA.Set(0.0f, -0.5f); - jd.localAnchorB.Set(0.0f, 0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[7] = m_world->CreateJoint(&jd); + jd.Initialize(ground, body, b2Vec2(0.0f, 15.0f), bd.position); + jd.collideConnected = true; + m_length = jd.length; + m_minLength = m_length; + m_maxLength = m_length; + b2LinearStiffness(jd.stiffness, jd.damping, m_hertz, m_dampingRatio, jd.bodyA, jd.bodyB); + m_joint = (b2DistanceJoint*)m_world->CreateJoint(&jd); } } - void Keyboard(int key) override + void UpdateUI() override { - switch (key) + ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); + ImGui::SetNextWindowSize(ImVec2(260.0f, 150.0f)); + ImGui::Begin("Joint Controls", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize); + + if (ImGui::SliderFloat("Length", &m_length, 0.0f, 20.0f, "%.0f")) { - case GLFW_KEY_B: - for (int32 i = 0; i < 4; ++i) - { - if (m_bodies[i]) - { - m_world->DestroyBody(m_bodies[i]); - m_bodies[i] = NULL; - break; - } - } - break; - - case GLFW_KEY_J: - for (int32 i = 0; i < 8; ++i) - { - if (m_joints[i]) - { - m_world->DestroyJoint(m_joints[i]); - m_joints[i] = NULL; - break; - } - } - break; + m_length = m_joint->SetLength(m_length); } - } - void Step(Settings& settings) override - { - Test::Step(settings); - g_debugDraw.DrawString(5, m_textLine, "This demonstrates a soft distance joint."); - m_textLine += m_textIncrement; - g_debugDraw.DrawString(5, m_textLine, "Press: (b) to delete a body, (j) to delete a joint"); - m_textLine += m_textIncrement; - } + if (ImGui::SliderFloat("Min Length", &m_minLength, 0.0f, 20.0f, "%.0f")) + { + m_minLength = m_joint->SetMinLength(m_minLength); + } - void JointDestroyed(b2Joint* joint) override - { - for (int32 i = 0; i < 8; ++i) + if (ImGui::SliderFloat("Max Length", &m_maxLength, 0.0f, 20.0f, "%.0f")) + { + m_maxLength = m_joint->SetMaxLength(m_maxLength); + } + + if (ImGui::SliderFloat("Hertz", &m_hertz, 0.0f, 10.0f, "%.1f")) { - if (m_joints[i] == joint) - { - m_joints[i] = NULL; - break; - } + float stiffness; + float damping; + b2LinearStiffness(stiffness, damping, m_hertz, m_dampingRatio, m_joint->GetBodyA(), m_joint->GetBodyB()); + m_joint->SetStiffness(stiffness); + m_joint->SetDamping(damping); } + + if (ImGui::SliderFloat("Damping Ratio", &m_dampingRatio, 0.0f, 2.0f, "%.1f")) + { + float stiffness; + float damping; + b2LinearStiffness(stiffness, damping, m_hertz, m_dampingRatio, m_joint->GetBodyA(), m_joint->GetBodyB()); + m_joint->SetStiffness(stiffness); + m_joint->SetDamping(damping); + } + + ImGui::End(); } static Test* Create() @@ -213,8 +112,12 @@ class DistanceJoint : public Test return new DistanceJoint; } - b2Body* m_bodies[4]; - b2Joint* m_joints[8]; + b2DistanceJoint* m_joint; + float m_length; + float m_minLength; + float m_maxLength; + float m_hertz; + float m_dampingRatio; }; static int testIndex = RegisterTest("Joints", "Distance Joint", DistanceJoint::Create); diff --git a/testbed/tests/dominos.cpp b/testbed/tests/dominos.cpp index 84942ee41..739876bf5 100644 --- a/testbed/tests/dominos.cpp +++ b/testbed/tests/dominos.cpp @@ -190,6 +190,8 @@ class Dominos : public Test djd.localAnchorB.Set(0.0f, -1.0f); b2Vec2 d = djd.bodyB->GetWorldPoint(djd.localAnchorB) - djd.bodyA->GetWorldPoint(djd.localAnchorA); djd.length = d.Length(); + + b2LinearStiffness(djd.stiffness, djd.damping, 1.0f, 1.0f, djd.bodyA, djd.bodyB); m_world->CreateJoint(&djd); { diff --git a/testbed/tests/web.cpp b/testbed/tests/web.cpp new file mode 100644 index 000000000..aefa46ac9 --- /dev/null +++ b/testbed/tests/web.cpp @@ -0,0 +1,218 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "test.h" + +// Test distance joints, body destruction, and joint destruction. +class Web : public Test +{ +public: + Web() + { + b2Body* ground = NULL; + { + b2BodyDef bd; + ground = m_world->CreateBody(&bd); + + b2EdgeShape shape; + shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); + ground->CreateFixture(&shape, 0.0f); + } + + { + b2PolygonShape shape; + shape.SetAsBox(0.5f, 0.5f); + + b2BodyDef bd; + bd.type = b2_dynamicBody; + + bd.position.Set(-5.0f, 5.0f); + m_bodies[0] = m_world->CreateBody(&bd); + m_bodies[0]->CreateFixture(&shape, 5.0f); + + bd.position.Set(5.0f, 5.0f); + m_bodies[1] = m_world->CreateBody(&bd); + m_bodies[1]->CreateFixture(&shape, 5.0f); + + bd.position.Set(5.0f, 15.0f); + m_bodies[2] = m_world->CreateBody(&bd); + m_bodies[2]->CreateFixture(&shape, 5.0f); + + bd.position.Set(-5.0f, 15.0f); + m_bodies[3] = m_world->CreateBody(&bd); + m_bodies[3]->CreateFixture(&shape, 5.0f); + + b2DistanceJointDef jd; + b2Vec2 p1, p2, d; + + float frequencyHz = 2.0f; + float dampingRatio = 0.0f; + + jd.bodyA = ground; + jd.bodyB = m_bodies[0]; + jd.localAnchorA.Set(-10.0f, 0.0f); + jd.localAnchorB.Set(-0.5f, -0.5f); + p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); + p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); + d = p2 - p1; + jd.length = d.Length(); + b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); + m_joints[0] = m_world->CreateJoint(&jd); + + jd.bodyA = ground; + jd.bodyB = m_bodies[1]; + jd.localAnchorA.Set(10.0f, 0.0f); + jd.localAnchorB.Set(0.5f, -0.5f); + p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); + p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); + d = p2 - p1; + jd.length = d.Length(); + b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); + m_joints[1] = m_world->CreateJoint(&jd); + + jd.bodyA = ground; + jd.bodyB = m_bodies[2]; + jd.localAnchorA.Set(10.0f, 20.0f); + jd.localAnchorB.Set(0.5f, 0.5f); + p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); + p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); + d = p2 - p1; + jd.length = d.Length(); + b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); + m_joints[2] = m_world->CreateJoint(&jd); + + jd.bodyA = ground; + jd.bodyB = m_bodies[3]; + jd.localAnchorA.Set(-10.0f, 20.0f); + jd.localAnchorB.Set(-0.5f, 0.5f); + p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); + p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); + d = p2 - p1; + jd.length = d.Length(); + b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); + m_joints[3] = m_world->CreateJoint(&jd); + + jd.bodyA = m_bodies[0]; + jd.bodyB = m_bodies[1]; + jd.localAnchorA.Set(0.5f, 0.0f); + jd.localAnchorB.Set(-0.5f, 0.0f);; + p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); + p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); + d = p2 - p1; + jd.length = d.Length(); + b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); + m_joints[4] = m_world->CreateJoint(&jd); + + jd.bodyA = m_bodies[1]; + jd.bodyB = m_bodies[2]; + jd.localAnchorA.Set(0.0f, 0.5f); + jd.localAnchorB.Set(0.0f, -0.5f); + p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); + p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); + d = p2 - p1; + jd.length = d.Length(); + b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); + m_joints[5] = m_world->CreateJoint(&jd); + + jd.bodyA = m_bodies[2]; + jd.bodyB = m_bodies[3]; + jd.localAnchorA.Set(-0.5f, 0.0f); + jd.localAnchorB.Set(0.5f, 0.0f); + p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); + p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); + d = p2 - p1; + jd.length = d.Length(); + b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); + m_joints[6] = m_world->CreateJoint(&jd); + + jd.bodyA = m_bodies[3]; + jd.bodyB = m_bodies[0]; + jd.localAnchorA.Set(0.0f, -0.5f); + jd.localAnchorB.Set(0.0f, 0.5f); + p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); + p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); + d = p2 - p1; + jd.length = d.Length(); + b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); + m_joints[7] = m_world->CreateJoint(&jd); + } + } + + void Keyboard(int key) override + { + switch (key) + { + case GLFW_KEY_B: + for (int32 i = 0; i < 4; ++i) + { + if (m_bodies[i]) + { + m_world->DestroyBody(m_bodies[i]); + m_bodies[i] = NULL; + break; + } + } + break; + + case GLFW_KEY_J: + for (int32 i = 0; i < 8; ++i) + { + if (m_joints[i]) + { + m_world->DestroyJoint(m_joints[i]); + m_joints[i] = NULL; + break; + } + } + break; + } + } + + void Step(Settings& settings) override + { + Test::Step(settings); + g_debugDraw.DrawString(5, m_textLine, "Press: (b) to delete a body, (j) to delete a joint"); + m_textLine += m_textIncrement; + } + + void JointDestroyed(b2Joint* joint) override + { + for (int32 i = 0; i < 8; ++i) + { + if (m_joints[i] == joint) + { + m_joints[i] = NULL; + break; + } + } + } + + static Test* Create() + { + return new Web; + } + + b2Body* m_bodies[4]; + b2Joint* m_joints[8]; +}; + +static int testIndex = RegisterTest("Examples", "Web", Web::Create); diff --git a/testbed/tests/rope_joint.cpp b/testbed/tests/wrecking_ball.cpp similarity index 59% rename from testbed/tests/rope_joint.cpp rename to testbed/tests/wrecking_ball.cpp index 0d7bb965a..74a61b474 100644 --- a/testbed/tests/rope_joint.cpp +++ b/testbed/tests/wrecking_ball.cpp @@ -21,19 +21,20 @@ // SOFTWARE. #include "test.h" +#include "imgui/imgui.h" -/// This test shows how a rope joint can be used to stabilize a chain of -/// bodies with a heavy payload. Notice that the rope joint just prevents +/// This test shows how a distance joint can be used to stabilize a chain of +/// bodies with a heavy payload. Notice that the distance joint just prevents /// excessive stretching and has no other effect. -/// By disabling the rope joint you can see that the Box2D solver has trouble +/// By disabling the distance joint you can see that the Box2D solver has trouble /// supporting heavy bodies with light bodies. Try playing around with the /// densities, time step, and iterations to see how they affect stability. /// This test also shows how to use contact filtering. Filtering is configured /// so that the payload does not collide with the chain. -class RopeJoint : public Test +class WreckingBall : public Test { public: - RopeJoint() + WreckingBall() { b2Body* ground = NULL; { @@ -61,7 +62,7 @@ class RopeJoint : public Test const int32 N = 10; const float y = 15.0f; - m_ropeDef.localAnchorA.Set(0.0f, y); + m_distanceJointDef.localAnchorA.Set(0.0f, y); b2Body* prevBody = ground; for (int32 i = 0; i < N; ++i) @@ -71,16 +72,26 @@ class RopeJoint : public Test bd.position.Set(0.5f + 1.0f * i, y); if (i == N - 1) { - shape.SetAsBox(1.5f, 1.5f); - fd.density = 100.0f; - fd.filter.categoryBits = 0x0002; bd.position.Set(1.0f * i, y); bd.angularDamping = 0.4f; } b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); + if (i == N - 1) + { + b2CircleShape circleShape; + circleShape.m_radius = 1.5f; + b2FixtureDef sfd; + sfd.shape = &circleShape; + sfd.density = 100.0f; + sfd.filter.categoryBits = 0x0002; + body->CreateFixture(&sfd); + } + else + { + body->CreateFixture(&fd); + } b2Vec2 anchor(float(i), y); jd.Initialize(prevBody, body, anchor); @@ -89,60 +100,66 @@ class RopeJoint : public Test prevBody = body; } - m_ropeDef.localAnchorB.SetZero(); + m_distanceJointDef.localAnchorB.SetZero(); float extraLength = 0.01f; - m_ropeDef.maxLength = N - 1.0f + extraLength; - m_ropeDef.bodyB = prevBody; + m_distanceJointDef.minLength = 0.0f; + m_distanceJointDef.maxLength = N - 1.0f + extraLength; + m_distanceJointDef.bodyB = prevBody; } { - m_ropeDef.bodyA = ground; - m_rope = m_world->CreateJoint(&m_ropeDef); + m_distanceJointDef.bodyA = ground; + m_distanceJoint = m_world->CreateJoint(&m_distanceJointDef); + m_stabilize = true; } } - void Keyboard(int key) override + void UpdateUI() override { - switch (key) + ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); + ImGui::SetNextWindowSize(ImVec2(200.0f, 100.0f)); + ImGui::Begin("Wrecking Ball Controls", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize); + + if (ImGui::Checkbox("Stabilize", &m_stabilize)) { - case GLFW_KEY_J: - if (m_rope) + if (m_stabilize == true && m_distanceJoint == nullptr) { - m_world->DestroyJoint(m_rope); - m_rope = NULL; + m_distanceJoint = m_world->CreateJoint(&m_distanceJointDef); } - else + else if (m_stabilize == false && m_distanceJoint != nullptr) { - m_rope = m_world->CreateJoint(&m_ropeDef); + m_world->DestroyJoint(m_distanceJoint); + m_distanceJoint = nullptr; } - break; } + + ImGui::End(); } void Step(Settings& settings) override { Test::Step(settings); - g_debugDraw.DrawString(5, m_textLine, "Press (j) to toggle the rope joint."); - m_textLine += m_textIncrement; - if (m_rope) + + if (m_distanceJoint) { - g_debugDraw.DrawString(5, m_textLine, "Rope ON"); + g_debugDraw.DrawString(5, m_textLine, "Distance Joint ON"); } else { - g_debugDraw.DrawString(5, m_textLine, "Rope OFF"); + g_debugDraw.DrawString(5, m_textLine, "Distance Joint OFF"); } m_textLine += m_textIncrement; } static Test* Create() { - return new RopeJoint; + return new WreckingBall; } - b2RopeJointDef m_ropeDef; - b2Joint* m_rope; + b2DistanceJointDef m_distanceJointDef; + b2Joint* m_distanceJoint; + bool m_stabilize; }; -static int testIndex = RegisterTest("Joints", "Rope", RopeJoint::Create); +static int testIndex = RegisterTest("Examples", "Wrecking Ball", WreckingBall::Create);