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

Example 61 with Tuple2f

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

the class RevoluteJoint method initVelocityConstraints.

@Override
public void initVelocityConstraints(final SolverData data) {
    m_indexA = A.island;
    m_indexB = B.island;
    m_localCenterA.set(A.sweep.localCenter);
    m_localCenterB.set(B.sweep.localCenter);
    m_invMassA = A.m_invMass;
    m_invMassB = B.m_invMass;
    m_invIA = A.m_invI;
    m_invIB = B.m_invI;
    // Vec2 cA = data.positions[m_indexA].c;
    float aA = data.positions[m_indexA].a;
    Tuple2f vA = data.velocities[m_indexA];
    float wA = data.velocities[m_indexA].w;
    // Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;
    Tuple2f vB = data.velocities[m_indexB];
    float wB = data.velocities[m_indexB].w;
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Tuple2f temp = new v2();
    qA.set(aA);
    qB.set(aB);
    // Compute the effective masses.
    Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subbed(m_localCenterA), m_rA);
    Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subbed(m_localCenterB), m_rB);
    // J = [-I -r1_skew I r2_skew]
    // [ 0 -1 0 1]
    // r_skew = [-ry; rx]
    // Matlab
    // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
    // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
    // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    boolean fixedRotation = (iA + iB == 0.0f);
    m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
    m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
    m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
    m_mass.ex.y = m_mass.ey.x;
    m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
    m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
    m_mass.ex.z = m_mass.ez.x;
    m_mass.ey.z = m_mass.ez.y;
    m_mass.ez.z = iA + iB;
    m_motorMass = iA + iB;
    if (m_motorMass > 0.0f) {
        m_motorMass = 1.0f / m_motorMass;
    }
    if (!m_enableMotor || fixedRotation) {
        m_motorImpulse = 0.0f;
    }
    if (m_enableLimit && !fixedRotation) {
        float jointAngle = aB - aA - m_referenceAngle;
        if (Math.abs(m_upperAngle - m_lowerAngle) < 2.0f * Settings.angularSlop) {
            m_limitState = LimitState.EQUAL;
        } else if (jointAngle <= m_lowerAngle) {
            if (m_limitState != LimitState.AT_LOWER) {
                m_impulse.z = 0.0f;
            }
            m_limitState = LimitState.AT_LOWER;
        } else if (jointAngle >= m_upperAngle) {
            if (m_limitState != LimitState.AT_UPPER) {
                m_impulse.z = 0.0f;
            }
            m_limitState = LimitState.AT_UPPER;
        } else {
            m_limitState = LimitState.INACTIVE;
            m_impulse.z = 0.0f;
        }
    } else {
        m_limitState = LimitState.INACTIVE;
    }
    if (data.step.warmStarting) {
        final Tuple2f P = new v2();
        // Scale impulses to support a variable time step.
        m_impulse.x *= data.step.dtRatio;
        m_impulse.y *= data.step.dtRatio;
        m_motorImpulse *= data.step.dtRatio;
        P.x = m_impulse.x;
        P.y = m_impulse.y;
        vA.x -= mA * P.x;
        vA.y -= mA * P.y;
        wA -= iA * (Tuple2f.cross(m_rA, P) + m_motorImpulse + m_impulse.z);
        vB.x += mB * P.x;
        vB.y += mB * P.y;
        wB += iB * (Tuple2f.cross(m_rB, P) + m_motorImpulse + m_impulse.z);
    } 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);
}

Example 62 with Tuple2f

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

the class RopeJoint method initVelocityConstraints.

@Override
public void initVelocityConstraints(final SolverData data) {
    indexA = A.island;
    indexB = B.island;
    m_localCenterA.set(A.sweep.localCenter);
    m_localCenterB.set(B.sweep.localCenter);
    m_invMassA = A.m_invMass;
    m_invMassB = B.m_invMass;
    m_invIA = A.m_invI;
    m_invIB = B.m_invI;
    Tuple2f cA = data.positions[indexA];
    float aA = data.positions[indexA].a;
    Tuple2f vA = data.velocities[indexA];
    float wA = data.velocities[indexA].w;
    Tuple2f cB = data.positions[indexB];
    float aB = data.positions[indexB].a;
    Tuple2f vB = data.velocities[indexB];
    float wB = data.velocities[indexB].w;
    final Rot qA = new Rot();
    final Rot qB = new Rot();
    final Tuple2f temp = new v2();
    qA.set(aA);
    qB.set(aB);
    // Compute the effective masses.
    Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subbed(m_localCenterA), m_rA);
    Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subbed(m_localCenterB), m_rB);
    m_u.set(cB).added(m_rB).subbed(cA).subbed(m_rA);
    length = m_u.length();
    float C = length - targetLength();
    float ca = Math.abs(C);
    if (length > Settings.linearSlop) {
        m_u.scaled(1.0f / length);
        // if (C > Settings.linearSlop) {
        state = LimitState.AT_UPPER;
    // }
    // /*else if (C < -Settings.linearSlop) {
    // state = LimitState.AT_LOWER;
    // } */ else {
    // state = LimitState.INACTIVE;
    // m_u.setZero();
    // m_mass = 0.0f;
    // m_impulse = 0.0f;
    // length = 0;
    // return;
    // }
    } else {
        state = LimitState.INACTIVE;
        m_u.setZero();
        m_mass = 0.0f;
        m_impulse = 0.0f;
        length = 0;
        return;
    }
    // Compute effective mass.
    float crA = Tuple2f.cross(m_rA, m_u);
    float crB = Tuple2f.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 * positionFactor;
    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;
    // }
    // data.velocities[m_indexA].v = vA;
    data.velocities[indexA].w = wA;
    // data.velocities[m_indexB].v = vB;
    data.velocities[indexB].w = wB;
}

Example 63 with Tuple2f

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

the class RopeJoint method solveVelocityConstraints.

@Override
public void solveVelocityConstraints(final SolverData data) {
    float targetLength = targetLength();
    Velocity VA = data.velocities[indexA];
    Tuple2f vA = VA;
    float wA = VA.w;
    Velocity VB = data.velocities[indexB];
    Tuple2f vB = VB;
    float wB = VB.w;
    // Cdot = dot(u, v + cross(w, r))
    Tuple2f vpA = pool.popVec2();
    Tuple2f vpB = pool.popVec2();
    Tuple2f temp = pool.popVec2();
    Tuple2f.crossToOutUnsafe(wA, m_rA, vpA);
    vpA.added(vA);
    Tuple2f.crossToOutUnsafe(wB, m_rB, vpB);
    vpB.added(vB);
    float dLen = length - targetLength;
    float Cdot = Tuple2f.dot(m_u, temp.set(vpB).subbed(vpA));
    // Predictive constraint.
    // if (dLen < 0.0f) {
    Cdot += data.step.inv_dt * Math.abs(dLen) * positionFactor;
    // }
    float impulse = -m_mass * Cdot;
    float oldImpulse = m_impulse;
    m_impulse = MathUtils.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;
    VA.w = wA - m_invIA * (m_rA.x * Py - m_rA.y * Px);
    vB.x += m_invMassB * Px;
    vB.y += m_invMassB * Py;
    VB.w = wB + m_invIB * (m_rB.x * Py - m_rB.y * Px);
    pool.pushVec2(3);
// data.velocities[m_indexA].v = vA;
// data.velocities[m_indexB].v = vB;
}

Example 64 with Tuple2f

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

the class RopeJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(final SolverData data) {
    final float targetLength = targetLength();
    Tuple2f cA = data.positions[indexA];
    float aA = data.positions[indexA].a;
    Tuple2f cB = data.positions[indexB];
    float aB = data.positions[indexB].a;
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Tuple2f u = pool.popVec2();
    final Tuple2f rA = pool.popVec2();
    final Tuple2f rB = pool.popVec2();
    final Tuple2f temp = pool.popVec2();
    qA.set(aA);
    qB.set(aB);
    // Compute the effective masses.
    Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subbed(m_localCenterA), rA);
    Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subbed(m_localCenterB), rB);
    u.set(cB).added(rB).subbed(cA).subbed(rA);
    float length = u.normalize();
    float C = length - targetLength;
    // C = MathUtils.clamp(C, 0.0f, Settings.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[indexA].a = aA;
    // data.positions[m_indexB].c = cB;
    data.positions[indexB].a = aB;
    return Math.abs(length - targetLength) < Settings.linearSlop;
}

Example 65 with Tuple2f

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

the class WheelJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(SolverData data) {
    Tuple2f cA = data.positions[m_indexA];
    float aA = data.positions[m_indexA].a;
    Tuple2f cB = data.positions[m_indexB];
    float aB = data.positions[m_indexB].a;
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Tuple2f temp = pool.popVec2();
    qA.set(aA);
    qB.set(aB);
    Rot.mulToOut(qA, temp.set(m_localAnchorA).subbed(m_localCenterA), rA);
    Rot.mulToOut(qB, temp.set(m_localAnchorB).subbed(m_localCenterB), rB);
    d.set(cB).subbed(cA).added(rB).subbed(rA);
    Tuple2f ay = pool.popVec2();
    Rot.mulToOut(qA, m_localYAxisA, ay);
    float sAy = Tuple2f.cross(temp.set(d).added(rA), ay);
    float sBy = Tuple2f.cross(rB, ay);
    float C = Tuple2f.dot(d, ay);
    float k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
    float impulse;
    if (k != 0.0f) {
        impulse = -C / k;
    } else {
        impulse = 0.0f;
    }
    final Tuple2f P = pool.popVec2();
    P.x = impulse * ay.x;
    P.y = impulse * ay.y;
    float LA = impulse * sAy;
    float LB = impulse * sBy;
    cA.x -= m_invMassA * P.x;
    cA.y -= m_invMassA * P.y;
    aA -= m_invIA * LA;
    cB.x += m_invMassB * P.x;
    cB.y += m_invMassB * P.y;
    aB += m_invIB * LB;
    pool.pushVec2(3);
    pool.pushRot(2);
    // 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 Math.abs(C) <= Settings.linearSlop;
}