All Downloads are FREE. Search and download functionalities are using the official Maven repository.

org.jbox2d.dynamics.joints.RopeJoint Maven / Gradle / Ivy

The newest version!
/*
 * The MIT License (MIT)
 *
 * FXGL - JavaFX Game Library
 *
 * Copyright (c) 2015-2017 AlmasB ([email protected])
 *
 * 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.
 */

package org.jbox2d.dynamics.joints;

import com.almasb.fxgl.core.math.Vec2;
import org.jbox2d.common.JBoxSettings;
import org.jbox2d.common.JBoxUtils;
import org.jbox2d.common.Rotation;
import org.jbox2d.dynamics.SolverData;
import org.jbox2d.pooling.IWorldPool;

/**
 * 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 DistanceJoint if you want to
 * dynamically control length.
 *
 * @author Daniel Murphy
 */
public class RopeJoint extends Joint {
    // Solver shared
    private final Vec2 m_localAnchorA = new Vec2();
    private final Vec2 m_localAnchorB = new Vec2();
    private float m_maxLength;
    private float m_length;
    private float m_impulse;

    // Solver temp
    private int m_indexA;
    private int m_indexB;
    private final Vec2 m_u = new Vec2();
    private final Vec2 m_rA = new Vec2();
    private final Vec2 m_rB = new Vec2();
    private final Vec2 m_localCenterA = new Vec2();
    private final Vec2 m_localCenterB = new Vec2();
    private float m_invMassA;
    private float m_invMassB;
    private float m_invIA;
    private float m_invIB;
    private float m_mass;
    private LimitState m_state;

    protected RopeJoint(IWorldPool worldPool, RopeJointDef def) {
        super(worldPool, def);
        m_localAnchorA.set(def.localAnchorA);
        m_localAnchorB.set(def.localAnchorB);

        m_maxLength = def.maxLength;

        m_mass = 0.0f;
        m_impulse = 0.0f;
        m_state = LimitState.INACTIVE;
        m_length = 0.0f;
    }

    @Override
    public void initVelocityConstraints(final SolverData data) {
        m_indexA = m_bodyA.m_islandIndex;
        m_indexB = m_bodyB.m_islandIndex;
        m_localCenterA.set(m_bodyA.m_sweep.localCenter);
        m_localCenterB.set(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;

        Vec2 cA = data.positions[m_indexA].c;
        float aA = data.positions[m_indexA].a;
        Vec2 vA = data.velocities[m_indexA].v;
        float wA = data.velocities[m_indexA].w;

        Vec2 cB = data.positions[m_indexB].c;
        float aB = data.positions[m_indexB].a;
        Vec2 vB = data.velocities[m_indexB].v;
        float wB = data.velocities[m_indexB].w;

        final Rotation qA = pool.popRot();
        final Rotation qB = pool.popRot();
        final Vec2 temp = pool.popVec2();

        qA.set(aA);
        qB.set(aB);

        // Compute the effective masses.
        Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
        Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);

        m_u.set(cB).addLocal(m_rB).subLocal(cA).subLocal(m_rA);

        m_length = m_u.length();

        float C = m_length - m_maxLength;
        if (C > 0.0f) {
            m_state = LimitState.AT_UPPER;
        } else {
            m_state = LimitState.INACTIVE;
        }

        if (m_length > JBoxSettings.linearSlop) {
            m_u.mulLocal(1.0f / m_length);
        } else {
            m_u.setZero();
            m_mass = 0.0f;
            m_impulse = 0.0f;
            return;
        }

        // Compute effective mass.
        float crA = Vec2.cross(m_rA, m_u);
        float crB = Vec2.cross(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;

            float Px = m_impulse * m_u.x;
            float Py = m_impulse * m_u.y;
            vA.x -= m_invMassA * Px;
            vA.y -= m_invMassA * Py;
            wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px);

            vB.x += m_invMassB * Px;
            vB.y += m_invMassB * Py;
            wB += m_invIB * (m_rB.x * Py - m_rB.y * Px);
        } else {
            m_impulse = 0.0f;
        }

        pool.pushRot(2);
        pool.pushVec2(1);

        // data.velocities[m_indexA].v = vA;
        data.velocities[m_indexA].w = wA;
        // data.velocities[m_indexB].v = vB;
        data.velocities[m_indexB].w = wB;
    }

    @Override
    public void solveVelocityConstraints(final SolverData data) {
        Vec2 vA = data.velocities[m_indexA].v;
        float wA = data.velocities[m_indexA].w;
        Vec2 vB = data.velocities[m_indexB].v;
        float wB = data.velocities[m_indexB].w;

        // Cdot = dot(u, v + cross(w, r))
        Vec2 vpA = pool.popVec2();
        Vec2 vpB = pool.popVec2();
        Vec2 temp = pool.popVec2();

        Vec2.crossToOutUnsafe(wA, m_rA, vpA);
        vpA.addLocal(vA);
        Vec2.crossToOutUnsafe(wB, m_rB, vpB);
        vpB.addLocal(vB);

        float C = m_length - m_maxLength;
        float Cdot = Vec2.dot(m_u, temp.set(vpB).subLocal(vpA));

        // Predictive constraint.
        if (C < 0.0f) {
            Cdot += data.step.inv_dt * C;
        }

        float impulse = -m_mass * Cdot;
        float oldImpulse = m_impulse;
        m_impulse = JBoxUtils.min(0.0f, m_impulse + impulse);
        impulse = m_impulse - oldImpulse;

        float Px = impulse * m_u.x;
        float Py = impulse * m_u.y;
        vA.x -= m_invMassA * Px;
        vA.y -= m_invMassA * Py;
        wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px);
        vB.x += m_invMassB * Px;
        vB.y += m_invMassB * Py;
        wB += m_invIB * (m_rB.x * Py - m_rB.y * Px);

        pool.pushVec2(3);

        // data.velocities[m_indexA].v = vA;
        data.velocities[m_indexA].w = wA;
        // data.velocities[m_indexB].v = vB;
        data.velocities[m_indexB].w = wB;
    }

    @Override
    public boolean solvePositionConstraints(final SolverData data) {
        Vec2 cA = data.positions[m_indexA].c;
        float aA = data.positions[m_indexA].a;
        Vec2 cB = data.positions[m_indexB].c;
        float aB = data.positions[m_indexB].a;

        final Rotation qA = pool.popRot();
        final Rotation qB = pool.popRot();
        final Vec2 u = pool.popVec2();
        final Vec2 rA = pool.popVec2();
        final Vec2 rB = pool.popVec2();
        final Vec2 temp = pool.popVec2();

        qA.set(aA);
        qB.set(aB);

        // Compute the effective masses.
        Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
        Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
        u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);

        float length = u.normalize();
        float C = length - m_maxLength;

        C = JBoxUtils.clamp(C, 0.0f, JBoxSettings.maxLinearCorrection);

        float impulse = -m_mass * C;
        float Px = impulse * u.x;
        float Py = impulse * u.y;

        cA.x -= m_invMassA * Px;
        cA.y -= m_invMassA * Py;
        aA -= m_invIA * (rA.x * Py - rA.y * Px);
        cB.x += m_invMassB * Px;
        cB.y += m_invMassB * Py;
        aB += m_invIB * (rB.x * Py - rB.y * Px);

        pool.pushRot(2);
        pool.pushVec2(4);

        // 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 length - m_maxLength < JBoxSettings.linearSlop;
    }

    @Override
    public void getAnchorA(Vec2 argOut) {
        m_bodyA.getWorldPointToOut(m_localAnchorA, argOut);
    }

    @Override
    public void getAnchorB(Vec2 argOut) {
        m_bodyB.getWorldPointToOut(m_localAnchorB, argOut);
    }

    @Override
    public void getReactionForce(float inv_dt, Vec2 argOut) {
        argOut.set(m_u).mulLocal(inv_dt).mulLocal(m_impulse);
    }

    @Override
    public float getReactionTorque(float inv_dt) {
        return 0f;
    }

    public Vec2 getLocalAnchorA() {
        return m_localAnchorA;
    }

    public Vec2 getLocalAnchorB() {
        return m_localAnchorB;
    }

    public float getMaxLength() {
        return m_maxLength;
    }

    public void setMaxLength(float maxLength) {
        this.m_maxLength = maxLength;
    }

    public LimitState getLimitState() {
        return m_state;
    }

}




© 2015 - 2024 Weber Informatics LLC | Privacy Policy