diff options
Diffstat (limited to 'Client/ThirdParty/Box2D/src/dynamics/b2_mouse_joint.cpp')
| -rw-r--r-- | Client/ThirdParty/Box2D/src/dynamics/b2_mouse_joint.cpp | 192 |
1 files changed, 192 insertions, 0 deletions
diff --git a/Client/ThirdParty/Box2D/src/dynamics/b2_mouse_joint.cpp b/Client/ThirdParty/Box2D/src/dynamics/b2_mouse_joint.cpp new file mode 100644 index 0000000..1ea6dd4 --- /dev/null +++ b/Client/ThirdParty/Box2D/src/dynamics/b2_mouse_joint.cpp @@ -0,0 +1,192 @@ +// 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_mouse_joint.h" +#include "box2d/b2_time_step.h" + +// p = attached point, m = mouse point +// C = p - m +// Cdot = v +// = v + cross(w, r) +// J = [I r_skew] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def) +: b2Joint(def) +{ + m_targetA = def->target; + m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA); + m_maxForce = def->maxForce; + m_stiffness = def->stiffness; + m_damping = def->damping; + + m_impulse.SetZero(); + m_beta = 0.0f; + m_gamma = 0.0f; +} + +void b2MouseJoint::SetTarget(const b2Vec2& target) +{ + if (target != m_targetA) + { + m_bodyB->SetAwake(true); + m_targetA = target; + } +} + +const b2Vec2& b2MouseJoint::GetTarget() const +{ + return m_targetA; +} + +void b2MouseJoint::SetMaxForce(float force) +{ + m_maxForce = force; +} + +float b2MouseJoint::GetMaxForce() const +{ + return m_maxForce; +} + +void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data) +{ + m_indexB = m_bodyB->m_islandIndex; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassB = m_bodyB->m_invMass; + m_invIB = m_bodyB->m_invI; + + 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 qB(aB); + + float mass = m_bodyB->GetMass(); + + float d = m_damping; + float k = m_stiffness; + + // magic formulas + // gamma has units of inverse mass. + // beta has units of inverse time. + float h = data.step.dt; + m_gamma = h * (d + h * k); + if (m_gamma != 0.0f) + { + m_gamma = 1.0f / m_gamma; + } + m_beta = h * k * m_gamma; + + // Compute the effective mass matrix. + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] + // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] + b2Mat22 K; + K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma; + K.ex.y = -m_invIB * m_rB.x * m_rB.y; + K.ey.x = K.ex.y; + K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma; + + m_mass = K.GetInverse(); + + m_C = cB + m_rB - m_targetA; + m_C *= m_beta; + + // Cheat with some damping + wB *= 0.98f; + + if (data.step.warmStarting) + { + m_impulse *= data.step.dtRatio; + vB += m_invMassB * m_impulse; + wB += m_invIB * b2Cross(m_rB, m_impulse); + } + else + { + m_impulse.SetZero(); + } + + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; +} + +void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data) +{ + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + + // Cdot = v + cross(w, r) + b2Vec2 Cdot = vB + b2Cross(wB, m_rB); + b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse)); + + b2Vec2 oldImpulse = m_impulse; + m_impulse += impulse; + float maxImpulse = data.step.dt * m_maxForce; + if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) + { + m_impulse *= maxImpulse / m_impulse.Length(); + } + impulse = m_impulse - oldImpulse; + + vB += m_invMassB * impulse; + wB += m_invIB * b2Cross(m_rB, impulse); + + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; +} + +bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data) +{ + B2_NOT_USED(data); + return true; +} + +b2Vec2 b2MouseJoint::GetAnchorA() const +{ + return m_targetA; +} + +b2Vec2 b2MouseJoint::GetAnchorB() const +{ + return m_bodyB->GetWorldPoint(m_localAnchorB); +} + +b2Vec2 b2MouseJoint::GetReactionForce(float inv_dt) const +{ + return inv_dt * m_impulse; +} + +float b2MouseJoint::GetReactionTorque(float inv_dt) const +{ + return inv_dt * 0.0f; +} + +void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin) +{ + m_targetA -= newOrigin; +} |