|
|
@@ -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);
|
|
|
-}
|
Alex Szpakowski