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

Example 91 with Vec2

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

the class GearJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(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;
    Vec2 cC = data.positions[m_indexC].c;
    float aC = data.positions[m_indexC].a;
    Vec2 cD = data.positions[m_indexD].c;
    float aD = data.positions[m_indexD].a;
    Rotation qA = pool.popRot();
    Rotation qB = pool.popRot();
    Rotation qC = pool.popRot();
    Rotation qD = pool.popRot();
    qA.set(aA);
    qB.set(aB);
    qC.set(aC);
    qD.set(aD);
    float linearError = 0.0f;
    float coordinateA, coordinateB;
    Vec2 temp = pool.popVec2();
    Vec2 JvAC = pool.popVec2();
    Vec2 JvBD = pool.popVec2();
    float JwA, JwB, JwC, JwD;
    float mass = 0.0f;
    if (m_typeA.equals(RevoluteJoint.class)) {
        JvAC.setZero();
        JwA = 1.0f;
        JwC = 1.0f;
        mass += m_iA + m_iC;
        coordinateA = aA - aC - m_referenceAngleA;
    } else {
        Vec2 rC = pool.popVec2();
        Vec2 rA = pool.popVec2();
        Vec2 pC = pool.popVec2();
        Vec2 pA = pool.popVec2();
        Rotation.mulToOutUnsafe(qC, m_localAxisC, JvAC);
        Rotation.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC);
        Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA);
        JwC = Vec2.cross(rC, JvAC);
        JwA = Vec2.cross(rA, JvAC);
        mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
        pC.set(m_localAnchorC).subLocal(m_lcC);
        Rotation.mulTransUnsafe(qC, temp.set(rA).addLocal(cA).subLocal(cC), pA);
        coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC);
        pool.pushVec2(4);
    }
    if (m_typeB.equals(RevoluteJoint.class)) {
        JvBD.setZero();
        JwB = m_ratio;
        JwD = m_ratio;
        mass += m_ratio * m_ratio * (m_iB + m_iD);
        coordinateB = aB - aD - m_referenceAngleB;
    } else {
        Vec2 u = pool.popVec2();
        Vec2 rD = pool.popVec2();
        Vec2 rB = pool.popVec2();
        Vec2 pD = pool.popVec2();
        Vec2 pB = pool.popVec2();
        Rotation.mulToOutUnsafe(qD, m_localAxisD, u);
        Rotation.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD);
        Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB);
        JvBD.set(u).mulLocal(m_ratio);
        JwD = Vec2.cross(rD, u);
        JwB = Vec2.cross(rB, u);
        mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
        pD.set(m_localAnchorD).subLocal(m_lcD);
        Rotation.mulTransUnsafe(qD, temp.set(rB).addLocal(cB).subLocal(cD), pB);
        coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD);
        pool.pushVec2(5);
    }
    float C = coordinateA + m_ratio * coordinateB - m_constant;
    float impulse = 0.0f;
    if (mass > 0.0f) {
        impulse = -C / mass;
    }
    pool.pushVec2(3);
    pool.pushRot(4);
    cA.x += (m_mA * impulse) * JvAC.x;
    cA.y += (m_mA * impulse) * JvAC.y;
    aA += m_iA * impulse * JwA;
    cB.x += (m_mB * impulse) * JvBD.x;
    cB.y += (m_mB * impulse) * JvBD.y;
    aB += m_iB * impulse * JwB;
    cC.x -= (m_mC * impulse) * JvAC.x;
    cC.y -= (m_mC * impulse) * JvAC.y;
    aC -= m_iC * impulse * JwC;
    cD.x -= (m_mD * impulse) * JvBD.x;
    cD.y -= (m_mD * impulse) * JvBD.y;
    aD -= m_iD * impulse * JwD;
    // data.positions[m_indexA].c = cA;
    data.positions[m_indexA].a = aA;
    // data.positions[m_indexB].c = cB;
    data.positions[m_indexB].a = aB;
    // data.positions[m_indexC].c = cC;
    data.positions[m_indexC].a = aC;
    // data.positions[m_indexD].c = cD;
    data.positions[m_indexD].a = aD;
    // TODO_ERIN not implemented
    return linearError < JBoxSettings.linearSlop;
}

Example 92 with Vec2

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

the class ParticleSystem method solveDamping.

@SuppressWarnings("PMD.UnusedFormalParameter")
private void solveDamping(TimeStep step) {
    // reduces normal velocity of each contact
    float damping = m_dampingStrength;
    for (int k = 0; k < m_bodyContactCount; k++) {
        final ParticleBodyContact contact = m_bodyContactBuffer[k];
        int a = contact.index;
        Body b = contact.body;
        float w = contact.weight;
        float m = contact.mass;
        Vec2 n = contact.normal;
        Vec2 p = m_positionBuffer.data[a];
        final float tempX = p.x - b.m_sweep.c.x;
        final float tempY = p.y - b.m_sweep.c.y;
        final Vec2 velA = m_velocityBuffer.data[a];
        // getLinearVelocityFromWorldPointToOut, with -= velA
        float vx = -b.getAngularVelocity() * tempY + b.getLinearVelocity().x - velA.x;
        float vy = b.getAngularVelocity() * tempX + b.getLinearVelocity().y - velA.y;
        // done
        float vn = vx * n.x + vy * n.y;
        if (vn < 0) {
            final Vec2 f = tempVec;
            f.x = damping * w * m * vn * n.x;
            f.y = damping * w * m * vn * n.y;
            final float invMass = getParticleInvMass();
            velA.x += invMass * f.x;
            velA.y += invMass * f.y;
            f.x = -f.x;
            f.y = -f.y;
            b.applyLinearImpulse(f, p, true);
        }
    }
    for (int k = 0; k < m_contactCount; k++) {
        final ParticleContact contact = m_contactBuffer[k];
        int a = contact.indexA;
        int b = contact.indexB;
        float w = contact.weight;
        Vec2 n = contact.normal;
        final Vec2 velA = m_velocityBuffer.data[a];
        final Vec2 velB = m_velocityBuffer.data[b];
        final float vx = velB.x - velA.x;
        final float vy = velB.y - velA.y;
        float vn = vx * n.x + vy * n.y;
        if (vn < 0) {
            float fx = damping * w * vn * n.x;
            float fy = damping * w * vn * n.y;
            velA.x += fx;
            velA.y += fy;
            velB.x -= fx;
            velB.y -= fy;
        }
    }
}

Example 93 with Vec2

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

the class ParticleSystem method solvePowder.

private void solvePowder(final TimeStep step) {
    float powderStrength = m_powderStrength * getCriticalVelocity(step);
    float minWeight = 1.0f - JBoxSettings.particleStride;
    for (int k = 0; k < m_bodyContactCount; k++) {
        final ParticleBodyContact contact = m_bodyContactBuffer[k];
        int a = contact.index;
        if ((m_flagsBuffer.data[a] & ParticleTypeInternal.b2_powderParticle) != 0) {
            float w = contact.weight;
            if (w > minWeight) {
                Body b = contact.body;
                float m = contact.mass;
                Vec2 p = m_positionBuffer.data[a];
                Vec2 n = contact.normal;
                final Vec2 f = tempVec;
                final Vec2 va = m_velocityBuffer.data[a];
                final float inter = powderStrength * m * (w - minWeight);
                final float pInvMass = getParticleInvMass();
                f.x = inter * n.x;
                f.y = inter * n.y;
                va.x -= pInvMass * f.x;
                va.y -= pInvMass * f.y;
                b.applyLinearImpulse(f, p, true);
            }
        }
    }
    for (int k = 0; k < m_contactCount; k++) {
        final ParticleContact contact = m_contactBuffer[k];
        if ((contact.flags & ParticleTypeInternal.b2_powderParticle) != 0) {
            float w = contact.weight;
            if (w > minWeight) {
                int a = contact.indexA;
                int b = contact.indexB;
                Vec2 n = contact.normal;
                final Vec2 va = m_velocityBuffer.data[a];
                final Vec2 vb = m_velocityBuffer.data[b];
                final float inter = powderStrength * (w - minWeight);
                final float fx = inter * n.x;
                final float fy = inter * n.y;
                va.x -= fx;
                va.y -= fy;
                vb.x += fx;
                vb.y += fy;
            }
        }
    }
}

Example 94 with Vec2

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

the class ParticleSystem method solvePressure.

private void solvePressure(TimeStep step) {
    // that means dimensionless density
    for (int i = 0; i < m_count; i++) {
        m_accumulationBuffer[i] = 0;
    }
    for (int k = 0; k < m_bodyContactCount; k++) {
        ParticleBodyContact contact = m_bodyContactBuffer[k];
        int a = contact.index;
        float w = contact.weight;
        m_accumulationBuffer[a] += w;
    }
    for (int k = 0; k < m_contactCount; k++) {
        ParticleContact contact = m_contactBuffer[k];
        int a = contact.indexA;
        int b = contact.indexB;
        float w = contact.weight;
        m_accumulationBuffer[a] += w;
        m_accumulationBuffer[b] += w;
    }
    // ignores powder particles
    if ((m_allParticleFlags & k_noPressureFlags) != 0) {
        for (int i = 0; i < m_count; i++) {
            if ((m_flagsBuffer.data[i] & k_noPressureFlags) != 0) {
                m_accumulationBuffer[i] = 0;
            }
        }
    }
    // calculates pressure as a linear function of density
    float pressurePerWeight = m_pressureStrength * getCriticalPressure(step);
    for (int i = 0; i < m_count; i++) {
        float w = m_accumulationBuffer[i];
        float h = pressurePerWeight * Math.max(0.0f, Math.min(w, JBoxSettings.maxParticleWeight) - JBoxSettings.minParticleWeight);
        m_accumulationBuffer[i] = h;
    }
    // applies pressure between each particles in contact
    float velocityPerPressure = step.dt / (m_density * m_particleDiameter);
    for (int k = 0; k < m_bodyContactCount; k++) {
        ParticleBodyContact contact = m_bodyContactBuffer[k];
        int a = contact.index;
        Body b = contact.body;
        float w = contact.weight;
        float m = contact.mass;
        Vec2 n = contact.normal;
        Vec2 p = m_positionBuffer.data[a];
        float h = m_accumulationBuffer[a] + pressurePerWeight * w;
        final Vec2 f = tempVec;
        final float coef = velocityPerPressure * w * m * h;
        f.x = coef * n.x;
        f.y = coef * n.y;
        final Vec2 velData = m_velocityBuffer.data[a];
        final float particleInvMass = getParticleInvMass();
        velData.x -= particleInvMass * f.x;
        velData.y -= particleInvMass * f.y;
        b.applyLinearImpulse(f, p, true);
    }
    for (int k = 0; k < m_contactCount; k++) {
        ParticleContact contact = m_contactBuffer[k];
        int a = contact.indexA;
        int b = contact.indexB;
        float w = contact.weight;
        Vec2 n = contact.normal;
        float h = m_accumulationBuffer[a] + m_accumulationBuffer[b];
        final float fx = velocityPerPressure * w * h * n.x;
        final float fy = velocityPerPressure * w * h * n.y;
        final Vec2 velDataA = m_velocityBuffer.data[a];
        final Vec2 velDataB = m_velocityBuffer.data[b];
        velDataA.x -= fx;
        velDataA.y -= fy;
        velDataB.x += fx;
        velDataB.y += fy;
    }
}

Example 95 with Vec2

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

the class ParticleSystem method solveElastic.

private void solveElastic(final TimeStep step) {
    float elasticStrength = step.inv_dt * m_elasticStrength;
    for (int k = 0; k < m_triadCount; k++) {
        final Triad triad = m_triadBuffer[k];
        if ((triad.flags & ParticleTypeInternal.b2_elasticParticle) != 0) {
            int a = triad.indexA;
            int b = triad.indexB;
            int c = triad.indexC;
            final Vec2 oa = triad.pa;
            final Vec2 ob = triad.pb;
            final Vec2 oc = triad.pc;
            final Vec2 pa = m_positionBuffer.data[a];
            final Vec2 pb = m_positionBuffer.data[b];
            final Vec2 pc = m_positionBuffer.data[c];
            final float px = 1f / 3 * (pa.x + pb.x + pc.x);
            final float py = 1f / 3 * (pa.y + pb.y + pc.y);
            float rs = Vec2.cross(oa, pa) + Vec2.cross(ob, pb) + Vec2.cross(oc, pc);
            float rc = Vec2.dot(oa, pa) + Vec2.dot(ob, pb) + Vec2.dot(oc, pc);
            float r2 = rs * rs + rc * rc;
            float invR = r2 == 0 ? Float.MAX_VALUE : FXGLMath.sqrtF(1f / r2);
            rs *= invR;
            rc *= invR;
            final float strength = elasticStrength * triad.strength;
            final float roax = rc * oa.x - rs * oa.y;
            final float roay = rs * oa.x + rc * oa.y;
            final float robx = rc * ob.x - rs * ob.y;
            final float roby = rs * ob.x + rc * ob.y;
            final float rocx = rc * oc.x - rs * oc.y;
            final float rocy = rs * oc.x + rc * oc.y;
            final Vec2 va = m_velocityBuffer.data[a];
            final Vec2 vb = m_velocityBuffer.data[b];
            final Vec2 vc = m_velocityBuffer.data[c];
            va.x += strength * (roax - (pa.x - px));
            va.y += strength * (roay - (pa.y - py));
            vb.x += strength * (robx - (pb.x - px));
            vb.y += strength * (roby - (pb.y - py));
            vc.x += strength * (rocx - (pc.x - px));
            vc.y += strength * (rocy - (pc.y - py));
        }
    }
}