b2_mouse_joint.cpp

Go to the documentation of this file.

// 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;
}