mirror of
https://github.com/love2d/love.git
synced 2026-08-15 15:51:12 +02:00
Fix RopeJoints not working.
This commit is contained in:
Alex Szpakowski
@@ -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"
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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()
|
||||
|
||||
@@ -56,7 +56,7 @@ public:
|
||||
|
||||
private:
|
||||
// The Box2D RopeJoint object.
|
||||
b2RopeJoint *joint;
|
||||
b2DistanceJoint *joint;
|
||||
};
|
||||
|
||||
} // box2d
|
||||
|
||||
Reference in New Issue
Block a user