Examples with Tuple2f spacegraph.util.math.Tuple2f used on opensource projects

Example 56 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class MotorJoint method solveVelocityConstraints.

@Override
public void solveVelocityConstraints(SolverData data) {
    final Tuple2f vA = data.velocities[m_indexA];
    float wA = data.velocities[m_indexA].w;
    final Tuple2f vB = data.velocities[m_indexB];
    float wB = data.velocities[m_indexB].w;
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    float h = data.step.dt;
    float inv_h = data.step.inv_dt;
    final Tuple2f temp = new v2();
    // Solve angular friction
    {
        float Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError;
        float impulse = -m_angularMass * Cdot;
        float oldImpulse = m_angularImpulse;
        float maxImpulse = h * m_maxTorque;
        m_angularImpulse = MathUtils.clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
        impulse = m_angularImpulse - oldImpulse;
        wA -= iA * impulse;
        wB += iB * impulse;
    }
    final Tuple2f Cdot = new v2();
    // Solve linear friction
    {
        // Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor *
        // m_linearError;
        Cdot.x = vB.x + -wB * m_rB.y - vA.x - -wA * m_rA.y + inv_h * m_correctionFactor * m_linearError.x;
        Cdot.y = vB.y + wB * m_rB.x - vA.y - wA * m_rA.x + inv_h * m_correctionFactor * m_linearError.y;
        final Tuple2f impulse = temp;
        Mat22.mulToOutUnsafe(m_linearMass, Cdot, impulse);
        impulse.negated();
        final Tuple2f oldImpulse = new v2();
        oldImpulse.set(m_linearImpulse);
        m_linearImpulse.added(impulse);
        float maxImpulse = h * m_maxForce;
        if (m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse) {
            m_linearImpulse.normalize();
            m_linearImpulse.scaled(maxImpulse);
        }
        impulse.x = m_linearImpulse.x - oldImpulse.x;
        impulse.y = m_linearImpulse.y - oldImpulse.y;
        vA.x -= mA * impulse.x;
        vA.y -= mA * impulse.y;
        wA -= iA * (m_rA.x * impulse.y - m_rA.y * impulse.x);
        vB.x += mB * impulse.x;
        vB.y += mB * impulse.y;
        wB += iB * (m_rB.x * impulse.y - m_rB.y * impulse.x);
    }
    // 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;
}

Example 57 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class PrismaticJoint method getReactionForce.

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

Example 58 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class PrismaticJoint method solveVelocityConstraints.

@Override
public void solveVelocityConstraints(final SolverData data) {
    Tuple2f vA = data.velocities[m_indexA];
    float wA = data.velocities[m_indexA].w;
    Tuple2f vB = data.velocities[m_indexB];
    float wB = data.velocities[m_indexB].w;
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    final Tuple2f temp = pool.popVec2();
    // Solve linear motor constraint.
    if (m_enableMotor && m_limitState != LimitState.EQUAL) {
        temp.set(vB).subbed(vA);
        float Cdot = Tuple2f.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 = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
        impulse = m_motorImpulse - oldImpulse;
        final Tuple2f P = pool.popVec2();
        P.set(m_axis).scaled(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 Tuple2f Cdot1 = pool.popVec2();
    temp.set(vB).subbed(vA);
    Cdot1.x = Tuple2f.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).subbed(vA);
        Cdot2 = Tuple2f.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 = MathUtils.max(m_impulse.z, 0.0f);
        } else if (m_limitState == LimitState.AT_UPPER) {
            m_impulse.z = MathUtils.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 Tuple2f b = pool.popVec2();
        final Tuple2f f2r = pool.popVec2();
        temp.set(m_K.ez.x, m_K.ez.y).scaled(m_impulse.z - f1.z);
        b.set(Cdot1).negated().subbed(temp);
        m_K.solve22ToOut(b, f2r);
        f2r.added(f1.x, f1.y);
        m_impulse.x = f2r.x;
        m_impulse.y = f2r.y;
        df.set(m_impulse).subLocal(f1);
        final Tuple2f P = pool.popVec2();
        temp.set(m_axis).scaled(df.z);
        P.set(m_perp).scaled(df.x).added(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 Tuple2f df = pool.popVec2();
        m_K.solve22ToOut(Cdot1.negated(), df);
        Cdot1.negated();
        m_impulse.x += df.x;
        m_impulse.y += df.y;
        final Tuple2f P = pool.popVec2();
        P.set(m_perp).scaled(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 59 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class PulleyJoint method solveVelocityConstraints.

@Override
public void solveVelocityConstraints(final SolverData data) {
    Tuple2f vA = data.velocities[m_indexA];
    float wA = data.velocities[m_indexA].w;
    Tuple2f vB = data.velocities[m_indexB];
    float wB = data.velocities[m_indexB].w;
    final Tuple2f vpA = pool.popVec2();
    final Tuple2f vpB = pool.popVec2();
    final Tuple2f PA = pool.popVec2();
    final Tuple2f PB = pool.popVec2();
    Tuple2f.crossToOutUnsafe(wA, m_rA, vpA);
    vpA.added(vA);
    Tuple2f.crossToOutUnsafe(wB, m_rB, vpB);
    vpB.added(vB);
    float Cdot = -Tuple2f.dot(m_uA, vpA) - m_ratio * Tuple2f.dot(m_uB, vpB);
    float impulse = -m_mass * Cdot;
    m_impulse += impulse;
    PA.set(m_uA).scaled(-impulse);
    PB.set(m_uB).scaled(-m_ratio * impulse);
    vA.x += m_invMassA * PA.x;
    vA.y += m_invMassA * PA.y;
    wA += m_invIA * Tuple2f.cross(m_rA, PA);
    vB.x += m_invMassB * PB.x;
    vB.y += m_invMassB * PB.y;
    wB += m_invIB * Tuple2f.cross(m_rB, PB);
    // 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(4);
}

Example 60 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class PulleyJoint method getLength1.

public float getLength1() {
    final Tuple2f p = pool.popVec2();
    A.getWorldPointToOut(m_localAnchorA, p);
    p.subbed(m_groundAnchorA);
    float len = p.length();
    pool.pushVec2(1);
    return len;
}