Fix RopeJoints not working.

This commit is contained in:
Alex Szpakowski
2020-11-12 23:46:49 -04:00
parent 6a1f4ecabe
commit fc2cf602f6
7 changed files with 3 additions and 367 deletions
-1
View File
@@ -1123,7 +1123,6 @@ set(LOVE_SRC_3P_BOX2D_DYNAMICS
src/libraries/box2d/dynamics/b2_prismatic_joint.cpp
src/libraries/box2d/dynamics/b2_pulley_joint.cpp
src/libraries/box2d/dynamics/b2_revolute_joint.cpp
src/libraries/box2d/dynamics/b2_rope_joint.cpp
src/libraries/box2d/dynamics/b2_weld_joint.cpp
src/libraries/box2d/dynamics/b2_wheel_joint.cpp
src/libraries/box2d/dynamics/b2_world.cpp
@@ -880,8 +880,6 @@
FABDA9A62552448300B5C523 /* b2_polygon_contact.h in Headers */ = {isa = PBXBuildFile; fileRef = FABDA92F2552448200B5C523 /* b2_polygon_contact.h */; };
FABDA9A72552448300B5C523 /* b2_motor_joint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = FABDA9302552448200B5C523 /* b2_motor_joint.cpp */; };
FABDA9A82552448300B5C523 /* b2_motor_joint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = FABDA9302552448200B5C523 /* b2_motor_joint.cpp */; };
FABDA9A92552448300B5C523 /* b2_rope_joint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = FABDA9312552448200B5C523 /* b2_rope_joint.cpp */; };
FABDA9AA2552448300B5C523 /* b2_rope_joint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = FABDA9312552448200B5C523 /* b2_rope_joint.cpp */; };
FABDA9AB2552448300B5C523 /* b2_world.cpp in Sources */ = {isa = PBXBuildFile; fileRef = FABDA9322552448200B5C523 /* b2_world.cpp */; };
FABDA9AC2552448300B5C523 /* b2_world.cpp in Sources */ = {isa = PBXBuildFile; fileRef = FABDA9322552448200B5C523 /* b2_world.cpp */; };
FABDA9AD2552448300B5C523 /* b2_island.cpp in Sources */ = {isa = PBXBuildFile; fileRef = FABDA9332552448200B5C523 /* b2_island.cpp */; };
@@ -940,7 +938,6 @@
FABDA9E22552448300B5C523 /* b2_growable_stack.h in Headers */ = {isa = PBXBuildFile; fileRef = FABDA95E2552448200B5C523 /* b2_growable_stack.h */; };
FABDA9E32552448300B5C523 /* b2_draw.h in Headers */ = {isa = PBXBuildFile; fileRef = FABDA95F2552448200B5C523 /* b2_draw.h */; };
FABDA9E42552448300B5C523 /* b2_collision.h in Headers */ = {isa = PBXBuildFile; fileRef = FABDA9602552448200B5C523 /* b2_collision.h */; };
FABDA9E52552448300B5C523 /* b2_rope_joint.h in Headers */ = {isa = PBXBuildFile; fileRef = FABDA9612552448200B5C523 /* b2_rope_joint.h */; };
FABDA9E62552448300B5C523 /* b2_api.h in Headers */ = {isa = PBXBuildFile; fileRef = FABDA9622552448200B5C523 /* b2_api.h */; };
FABDA9E72552448300B5C523 /* b2_chain_shape.h in Headers */ = {isa = PBXBuildFile; fileRef = FABDA9632552448200B5C523 /* b2_chain_shape.h */; };
FABDA9E82552448300B5C523 /* b2_gear_joint.h in Headers */ = {isa = PBXBuildFile; fileRef = FABDA9642552448200B5C523 /* b2_gear_joint.h */; };
@@ -1862,7 +1859,6 @@
FABDA92E2552448200B5C523 /* b2_wheel_joint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2_wheel_joint.cpp; sourceTree = "<group>"; };
FABDA92F2552448200B5C523 /* b2_polygon_contact.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2_polygon_contact.h; sourceTree = "<group>"; };
FABDA9302552448200B5C523 /* b2_motor_joint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2_motor_joint.cpp; sourceTree = "<group>"; };
FABDA9312552448200B5C523 /* b2_rope_joint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2_rope_joint.cpp; sourceTree = "<group>"; };
FABDA9322552448200B5C523 /* b2_world.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2_world.cpp; sourceTree = "<group>"; };
FABDA9332552448200B5C523 /* b2_island.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2_island.cpp; sourceTree = "<group>"; };
FABDA9342552448200B5C523 /* b2_mouse_joint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2_mouse_joint.cpp; sourceTree = "<group>"; };
@@ -1908,7 +1904,6 @@
FABDA95E2552448200B5C523 /* b2_growable_stack.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2_growable_stack.h; sourceTree = "<group>"; };
FABDA95F2552448200B5C523 /* b2_draw.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2_draw.h; sourceTree = "<group>"; };
FABDA9602552448200B5C523 /* b2_collision.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2_collision.h; sourceTree = "<group>"; };
FABDA9612552448200B5C523 /* b2_rope_joint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2_rope_joint.h; sourceTree = "<group>"; };
FABDA9622552448200B5C523 /* b2_api.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2_api.h; sourceTree = "<group>"; };
FABDA9632552448200B5C523 /* b2_chain_shape.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2_chain_shape.h; sourceTree = "<group>"; };
FABDA9642552448200B5C523 /* b2_gear_joint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2_gear_joint.h; sourceTree = "<group>"; };
@@ -3393,7 +3388,6 @@
FABDA95A2552448200B5C523 /* b2_prismatic_joint.h */,
FABDA94C2552448200B5C523 /* b2_pulley_joint.h */,
FABDA9452552448200B5C523 /* b2_revolute_joint.h */,
FABDA9612552448200B5C523 /* b2_rope_joint.h */,
FABDA9472552448200B5C523 /* b2_rope.h */,
FABDA9442552448200B5C523 /* b2_settings.h */,
FABDA9122552448200B5C523 /* b2_shape.h */,
@@ -3450,7 +3444,6 @@
FABDA9362552448200B5C523 /* b2_prismatic_joint.cpp */,
FABDA9242552448200B5C523 /* b2_pulley_joint.cpp */,
FABDA9202552448200B5C523 /* b2_revolute_joint.cpp */,
FABDA9312552448200B5C523 /* b2_rope_joint.cpp */,
FABDA91B2552448200B5C523 /* b2_weld_joint.cpp */,
FABDA92E2552448200B5C523 /* b2_wheel_joint.cpp */,
FABDA9232552448200B5C523 /* b2_world_callbacks.cpp */,
@@ -4164,7 +4157,6 @@
FABDA9E42552448300B5C523 /* b2_collision.h in Headers */,
FA41A3CA1C0A1F950084430C /* ASTCHandler.h in Headers */,
FADF54041E3D77B500012CC0 /* wrap_Text.h in Headers */,
FABDA9E52552448300B5C523 /* b2_rope_joint.h in Headers */,
FA0B7ED31A95902C000E1D17 /* wrap_ThreadModule.h in Headers */,
FAC756F61E4F99B400B91289 /* Effect.h in Headers */,
FA0B7ADD1A958EA3000E1D17 /* gladfuncs.hpp in Headers */,
@@ -4517,7 +4509,6 @@
FA0B7E981A95902C000E1D17 /* Sound.cpp in Sources */,
FA0B7E371A95902C000E1D17 /* WheelJoint.cpp in Sources */,
FA4F2C0C1DE936ED00CA37D7 /* select.c in Sources */,
FABDA9AA2552448300B5C523 /* b2_rope_joint.cpp in Sources */,
FA0B7D8E1A95902C000E1D17 /* ddsHandler.cpp in Sources */,
FA0B7DFE1A95902C000E1D17 /* ChainShape.cpp in Sources */,
FAE64A892071363100BC7981 /* physfs_archiver_vdf.c in Sources */,
@@ -4912,7 +4903,6 @@
FA0B7E361A95902C000E1D17 /* WheelJoint.cpp in Sources */,
FADF542F1E3DABF600012CC0 /* SpriteBatch.cpp in Sources */,
FA0B7D8D1A95902C000E1D17 /* ddsHandler.cpp in Sources */,
FABDA9A92552448300B5C523 /* b2_rope_joint.cpp in Sources */,
FAAA3FD91F64B3AD00F89E99 /* lstrlib.c in Sources */,
FA0B7DFD1A95902C000E1D17 /* ChainShape.cpp in Sources */,
FA0B79201A958E3B000E1D17 /* delay.cpp in Sources */,
-1
View File
@@ -53,7 +53,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"
-118
View File
@@ -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
@@ -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);
}
+2 -2
View File
@@ -38,7 +38,7 @@ RopeJoint::RopeJoint(Body *body1, Body *body2, float x1, float y1, float x2, flo
: Joint(body1, body2)
, joint(NULL)
{
b2RopeJointDef def;
b2DistanceJointDef def;
def.bodyA = body1->body;
def.bodyB = body2->body;
body1->getLocalPoint(x1, y1, x1, y1);
@@ -49,7 +49,7 @@ RopeJoint::RopeJoint(Body *body1, Body *body2, float x1, float y1, float x2, flo
def.localAnchorB.y = Physics::scaleDown(y2);
def.maxLength = Physics::scaleDown(maxLength);
def.collideConnected = collideConnected;
joint = (b2RopeJoint *)createJoint(&def);
joint = (b2DistanceJoint *)createJoint(&def);
}
RopeJoint::~RopeJoint()
+1 -1
View File
@@ -56,7 +56,7 @@ public:
private:
// The Box2D RopeJoint object.
b2RopeJoint *joint;
b2DistanceJoint *joint;
};
} // box2d