Examples with Vec2 com.almasb.fxgl.core.math.Vec2 used on opensource projects

Example 16 with Vec2

use of com.almasb.fxgl.core.math.Vec2 in project FXGL by AlmasB.

the class MouseJoint method solveVelocityConstraints.

@Override
public void solveVelocityConstraints(final SolverData data) {
    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;
    // Cdot = v + cross(w, r)
    final Vec2 Cdot = pool.popVec2();
    Vec2.crossToOutUnsafe(wB, m_rB, Cdot);
    Cdot.addLocal(vB);
    final Vec2 impulse = pool.popVec2();
    final Vec2 temp = pool.popVec2();
    temp.set(m_impulse).mulLocal(m_gamma).addLocal(m_C).addLocal(Cdot).negateLocal();
    Mat22.mulToOutUnsafe(m_mass, temp, impulse);
    Vec2 oldImpulse = temp;
    oldImpulse.set(m_impulse);
    m_impulse.addLocal(impulse);
    float maxImpulse = data.step.dt * m_maxForce;
    if (m_impulse.lengthSquared() > maxImpulse * maxImpulse) {
        m_impulse.mulLocal(maxImpulse / m_impulse.length());
    }
    impulse.set(m_impulse).subLocal(oldImpulse);
    vB.x += m_invMassB * impulse.x;
    vB.y += m_invMassB * impulse.y;
    wB += m_invIB * Vec2.cross(m_rB, impulse);
    // data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;
    pool.pushVec2(3);
}

Example 17 with Vec2

use of com.almasb.fxgl.core.math.Vec2 in project FXGL by AlmasB.

the class PrismaticJoint method initVelocityConstraints.

@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 d = pool.popVec2();
    final Vec2 temp = pool.popVec2();
    final Vec2 rA = pool.popVec2();
    final Vec2 rB = pool.popVec2();
    qA.set(aA);
    qB.set(aB);
    // Compute the effective masses.
    Rotation.mulToOutUnsafe(qA, d.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rotation.mulToOutUnsafe(qB, d.set(m_localAnchorB).subLocal(m_localCenterB), rB);
    d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA);
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    // Compute motor Jacobian and effective mass.
    {
        Rotation.mulToOutUnsafe(qA, m_localXAxisA, m_axis);
        temp.set(d).addLocal(rA);
        m_a1 = Vec2.cross(temp, m_axis);
        m_a2 = Vec2.cross(rB, m_axis);
        m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
        if (m_motorMass > 0.0f) {
            m_motorMass = 1.0f / m_motorMass;
        }
    }
    // Prismatic constraint.
    {
        Rotation.mulToOutUnsafe(qA, m_localYAxisA, m_perp);
        temp.set(d).addLocal(rA);
        m_s1 = Vec2.cross(temp, m_perp);
        m_s2 = Vec2.cross(rB, m_perp);
        float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
        float k12 = iA * m_s1 + iB * m_s2;
        float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
        float k22 = iA + iB;
        if (k22 == 0.0f) {
            // For bodies with fixed rotation.
            k22 = 1.0f;
        }
        float k23 = iA * m_a1 + iB * m_a2;
        float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
        m_K.ex.set(k11, k12, k13);
        m_K.ey.set(k12, k22, k23);
        m_K.ez.set(k13, k23, k33);
    }
    // Compute motor and limit terms.
    if (m_enableLimit) {
        float jointTranslation = Vec2.dot(m_axis, d);
        if (FXGLMath.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * JBoxSettings.linearSlop) {
            m_limitState = LimitState.EQUAL;
        } else if (jointTranslation <= m_lowerTranslation) {
            if (m_limitState != LimitState.AT_LOWER) {
                m_limitState = LimitState.AT_LOWER;
                m_impulse.z = 0.0f;
            }
        } else if (jointTranslation >= m_upperTranslation) {
            if (m_limitState != LimitState.AT_UPPER) {
                m_limitState = LimitState.AT_UPPER;
                m_impulse.z = 0.0f;
            }
        } else {
            m_limitState = LimitState.INACTIVE;
            m_impulse.z = 0.0f;
        }
    } else {
        m_limitState = LimitState.INACTIVE;
        m_impulse.z = 0.0f;
    }
    if (!m_enableMotor) {
        m_motorImpulse = 0.0f;
    }
    if (data.step.warmStarting) {
        // Account for variable time step.
        m_impulse.mulLocal(data.step.dtRatio);
        m_motorImpulse *= data.step.dtRatio;
        final Vec2 P = pool.popVec2();
        temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z);
        P.set(m_perp).mulLocal(m_impulse.x).addLocal(temp);
        float LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
        float LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
        vA.x -= mA * P.x;
        vA.y -= mA * P.y;
        wA -= iA * LA;
        vB.x += mB * P.x;
        vB.y += mB * P.y;
        wB += iB * LB;
        pool.pushVec2(1);
    } else {
        m_impulse.setZero();
        m_motorImpulse = 0.0f;
    }
    // data.velocities[m_indexA].v.set(vA);
    data.velocities[m_indexA].w = wA;
    // data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;
    pool.pushRot(2);
    pool.pushVec2(4);
}

Example 18 with Vec2

use of com.almasb.fxgl.core.math.Vec2 in project FXGL by AlmasB.

the class PrismaticJoint method solveVelocityConstraints.

@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;
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    final Vec2 temp = pool.popVec2();
    // Solve linear motor constraint.
    if (m_enableMotor && m_limitState != LimitState.EQUAL) {
        temp.set(vB).subLocal(vA);
        float Cdot = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA;
        float impulse = m_motorMass * (m_motorSpeed - Cdot);
        float oldImpulse = m_motorImpulse;
        float maxImpulse = data.step.dt * m_maxMotorForce;
        m_motorImpulse = FXGLMath.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
        impulse = m_motorImpulse - oldImpulse;
        final Vec2 P = pool.popVec2();
        P.set(m_axis).mulLocal(impulse);
        float LA = impulse * m_a1;
        float LB = impulse * m_a2;
        vA.x -= mA * P.x;
        vA.y -= mA * P.y;
        wA -= iA * LA;
        vB.x += mB * P.x;
        vB.y += mB * P.y;
        wB += iB * LB;
        pool.pushVec2(1);
    }
    final Vec2 Cdot1 = pool.popVec2();
    temp.set(vB).subLocal(vA);
    Cdot1.x = Vec2.dot(m_perp, temp) + m_s2 * wB - m_s1 * wA;
    Cdot1.y = wB - wA;
    if (m_enableLimit && m_limitState != LimitState.INACTIVE) {
        // Solve prismatic and limit constraint in block form.
        float Cdot2;
        temp.set(vB).subLocal(vA);
        Cdot2 = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA;
        final Vec3 Cdot = pool.popVec3();
        Cdot.set(Cdot1.x, Cdot1.y, Cdot2);
        final Vec3 f1 = pool.popVec3();
        final Vec3 df = pool.popVec3();
        f1.set(m_impulse);
        m_K.solve33ToOut(Cdot.negateLocal(), df);
        // Cdot.negateLocal(); not used anymore
        m_impulse.addLocal(df);
        if (m_limitState == LimitState.AT_LOWER) {
            m_impulse.z = Math.max(m_impulse.z, 0.0f);
        } else if (m_limitState == LimitState.AT_UPPER) {
            m_impulse.z = Math.min(m_impulse.z, 0.0f);
        }
        // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) +
        // f1(1:2)
        final Vec2 b = pool.popVec2();
        final Vec2 f2r = pool.popVec2();
        temp.set(m_K.ez.x, m_K.ez.y).mulLocal(m_impulse.z - f1.z);
        b.set(Cdot1).negateLocal().subLocal(temp);
        m_K.solve22ToOut(b, f2r);
        f2r.addLocal(f1.x, f1.y);
        m_impulse.x = f2r.x;
        m_impulse.y = f2r.y;
        df.set(m_impulse).subLocal(f1);
        final Vec2 P = pool.popVec2();
        temp.set(m_axis).mulLocal(df.z);
        P.set(m_perp).mulLocal(df.x).addLocal(temp);
        float LA = df.x * m_s1 + df.y + df.z * m_a1;
        float LB = df.x * m_s2 + df.y + df.z * m_a2;
        vA.x -= mA * P.x;
        vA.y -= mA * P.y;
        wA -= iA * LA;
        vB.x += mB * P.x;
        vB.y += mB * P.y;
        wB += iB * LB;
        pool.pushVec2(3);
        pool.pushVec3(3);
    } else {
        // Limit is inactive, just solve the prismatic constraint in block form.
        final Vec2 df = pool.popVec2();
        m_K.solve22ToOut(Cdot1.negateLocal(), df);
        Cdot1.negateLocal();
        m_impulse.x += df.x;
        m_impulse.y += df.y;
        final Vec2 P = pool.popVec2();
        P.set(m_perp).mulLocal(df.x);
        float LA = df.x * m_s1 + df.y;
        float LB = df.x * m_s2 + df.y;
        vA.x -= mA * P.x;
        vA.y -= mA * P.y;
        wA -= iA * LA;
        vB.x += mB * P.x;
        vB.y += mB * P.y;
        wB += iB * LB;
        pool.pushVec2(2);
    }
    // data.velocities[m_indexA].v.set(vA);
    data.velocities[m_indexA].w = wA;
    // data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;
    pool.pushVec2(2);
}

Example 19 with Vec2

use of com.almasb.fxgl.core.math.Vec2 in project FXGL by AlmasB.

the class PrismaticJoint method getJointTranslation.

public float getJointTranslation() {
    Vec2 pA = pool.popVec2(), pB = pool.popVec2(), axis = pool.popVec2();
    m_bodyA.getWorldPointToOut(m_localAnchorA, pA);
    m_bodyB.getWorldPointToOut(m_localAnchorB, pB);
    m_bodyA.getWorldVectorToOutUnsafe(m_localXAxisA, axis);
    pB.subLocal(pA);
    float translation = Vec2.dot(pB, axis);
    pool.pushVec2(3);
    return translation;
}

Example 20 with Vec2

use of com.almasb.fxgl.core.math.Vec2 in project FXGL by AlmasB.

the class PrismaticJoint method getReactionForce.

@Override
public void getReactionForce(float inv_dt, Vec2 argOut) {
    Vec2 temp = pool.popVec2();
    temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z);
    argOut.set(m_perp).mulLocal(m_impulse.x).addLocal(temp).mulLocal(inv_dt);
    pool.pushVec2(1);
}