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

Example 11 with Vec2

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

the class ContactSolver method solvePositionConstraints.

/**
 * Sequential solver.
 */
public boolean solvePositionConstraints() {
    float minSeparation = 0.0f;
    for (int i = 0; i < m_count; ++i) {
        ContactPositionConstraint pc = m_positionConstraints[i];
        int indexA = pc.indexA;
        int indexB = pc.indexB;
        float mA = pc.invMassA;
        float iA = pc.invIA;
        final float localCenterAx = pc.localCenterA.x;
        final float localCenterAy = pc.localCenterA.y;
        float mB = pc.invMassB;
        float iB = pc.invIB;
        final float localCenterBx = pc.localCenterB.x;
        final float localCenterBy = pc.localCenterB.y;
        int pointCount = pc.pointCount;
        Vec2 cA = m_positions[indexA].c;
        float aA = m_positions[indexA].a;
        Vec2 cB = m_positions[indexB].c;
        float aB = m_positions[indexB].a;
        // Solve normal constraints
        for (int j = 0; j < pointCount; ++j) {
            final Rotation xfAq = xfA.q;
            final Rotation xfBq = xfB.q;
            xfAq.set(aA);
            xfBq.set(aB);
            xfA.p.x = cA.x - xfAq.c * localCenterAx + xfAq.s * localCenterAy;
            xfA.p.y = cA.y - xfAq.s * localCenterAx - xfAq.c * localCenterAy;
            xfB.p.x = cB.x - xfBq.c * localCenterBx + xfBq.s * localCenterBy;
            xfB.p.y = cB.y - xfBq.s * localCenterBx - xfBq.c * localCenterBy;
            psolver.initialize(pc, xfA, xfB, j);
            final Vec2 normal = psolver.normal;
            final Vec2 point = psolver.point;
            final float separation = psolver.separation;
            float rAx = point.x - cA.x;
            float rAy = point.y - cA.y;
            float rBx = point.x - cB.x;
            float rBy = point.y - cB.y;
            // Track max constraint error.
            minSeparation = Math.min(minSeparation, separation);
            // Prevent large corrections and allow slop.
            final float C = FXGLMath.clamp(JBoxSettings.baumgarte * (separation + JBoxSettings.linearSlop), -JBoxSettings.maxLinearCorrection, 0.0f);
            // Compute the effective mass.
            final float rnA = rAx * normal.y - rAy * normal.x;
            final float rnB = rBx * normal.y - rBy * normal.x;
            final float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
            // Compute normal impulse
            final float impulse = K > 0.0f ? -C / K : 0.0f;
            float Px = normal.x * impulse;
            float Py = normal.y * impulse;
            cA.x -= Px * mA;
            cA.y -= Py * mA;
            aA -= iA * (rAx * Py - rAy * Px);
            cB.x += Px * mB;
            cB.y += Py * mB;
            aB += iB * (rBx * Py - rBy * Px);
        }
        m_positions[indexA].a = aA;
        m_positions[indexB].a = aB;
    }
    // push the separation above -linearSlop.
    return minSeparation >= -3.0f * JBoxSettings.linearSlop;
}

Example 12 with Vec2

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

the class ContactSolver method warmStart.

public void warmStart() {
    // Warm start.
    for (int i = 0; i < m_count; ++i) {
        final ContactVelocityConstraint vc = m_velocityConstraints[i];
        int indexA = vc.indexA;
        int indexB = vc.indexB;
        float mA = vc.invMassA;
        float iA = vc.invIA;
        float mB = vc.invMassB;
        float iB = vc.invIB;
        int pointCount = vc.pointCount;
        Vec2 vA = m_velocities[indexA].v;
        float wA = m_velocities[indexA].w;
        Vec2 vB = m_velocities[indexB].v;
        float wB = m_velocities[indexB].w;
        Vec2 normal = vc.normal;
        float tangentx = 1.0f * normal.y;
        float tangenty = -1.0f * normal.x;
        for (int j = 0; j < pointCount; ++j) {
            VelocityConstraintPoint vcp = vc.points[j];
            float Px = tangentx * vcp.tangentImpulse + normal.x * vcp.normalImpulse;
            float Py = tangenty * vcp.tangentImpulse + normal.y * vcp.normalImpulse;
            wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);
            vA.x -= Px * mA;
            vA.y -= Py * mA;
            wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
            vB.x += Px * mB;
            vB.y += Py * mB;
        }
        m_velocities[indexA].w = wA;
        m_velocities[indexB].w = wB;
    }
}

Example 13 with Vec2

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

the class ContactSolver method initializeVelocityConstraints.

public void initializeVelocityConstraints() {
    // Warm start.
    for (int i = 0; i < m_count; ++i) {
        ContactVelocityConstraint vc = m_velocityConstraints[i];
        ContactPositionConstraint pc = m_positionConstraints[i];
        float radiusA = pc.radiusA;
        float radiusB = pc.radiusB;
        Manifold manifold = m_contacts[vc.contactIndex].getManifold();
        int indexA = vc.indexA;
        int indexB = vc.indexB;
        float mA = vc.invMassA;
        float mB = vc.invMassB;
        float iA = vc.invIA;
        float iB = vc.invIB;
        Vec2 localCenterA = pc.localCenterA;
        Vec2 localCenterB = pc.localCenterB;
        Vec2 cA = m_positions[indexA].c;
        float aA = m_positions[indexA].a;
        Vec2 vA = m_velocities[indexA].v;
        float wA = m_velocities[indexA].w;
        Vec2 cB = m_positions[indexB].c;
        float aB = m_positions[indexB].a;
        Vec2 vB = m_velocities[indexB].v;
        float wB = m_velocities[indexB].w;
        assert manifold.pointCount > 0;
        final Rotation xfAq = xfA.q;
        final Rotation xfBq = xfB.q;
        xfAq.set(aA);
        xfBq.set(aB);
        xfA.p.x = cA.x - (xfAq.c * localCenterA.x - xfAq.s * localCenterA.y);
        xfA.p.y = cA.y - (xfAq.s * localCenterA.x + xfAq.c * localCenterA.y);
        xfB.p.x = cB.x - (xfBq.c * localCenterB.x - xfBq.s * localCenterB.y);
        xfB.p.y = cB.y - (xfBq.s * localCenterB.x + xfBq.c * localCenterB.y);
        worldManifold.initialize(manifold, xfA, radiusA, xfB, radiusB);
        final Vec2 vcnormal = vc.normal;
        vcnormal.x = worldManifold.normal.x;
        vcnormal.y = worldManifold.normal.y;
        int pointCount = vc.pointCount;
        for (int j = 0; j < pointCount; ++j) {
            VelocityConstraintPoint vcp = vc.points[j];
            Vec2 wmPj = worldManifold.points[j];
            final Vec2 vcprA = vcp.rA;
            final Vec2 vcprB = vcp.rB;
            vcprA.x = wmPj.x - cA.x;
            vcprA.y = wmPj.y - cA.y;
            vcprB.x = wmPj.x - cB.x;
            vcprB.y = wmPj.y - cB.y;
            float rnA = vcprA.x * vcnormal.y - vcprA.y * vcnormal.x;
            float rnB = vcprB.x * vcnormal.y - vcprB.y * vcnormal.x;
            float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
            vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
            float tangentx = 1.0f * vcnormal.y;
            float tangenty = -1.0f * vcnormal.x;
            float rtA = vcprA.x * tangenty - vcprA.y * tangentx;
            float rtB = vcprB.x * tangenty - vcprB.y * tangentx;
            float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
            vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
            // Setup a velocity bias for restitution.
            vcp.velocityBias = 0.0f;
            float tempx = vB.x + -wB * vcprB.y - vA.x - (-wA * vcprA.y);
            float tempy = vB.y + wB * vcprB.x - vA.y - (wA * vcprA.x);
            float vRel = vcnormal.x * tempx + vcnormal.y * tempy;
            if (vRel < -JBoxSettings.velocityThreshold) {
                vcp.velocityBias = -vc.restitution * vRel;
            }
        }
        // If we have two points, then prepare the block solver.
        if (vc.pointCount == 2) {
            VelocityConstraintPoint vcp1 = vc.points[0];
            VelocityConstraintPoint vcp2 = vc.points[1];
            float rn1A = vcp1.rA.x * vcnormal.y - vcp1.rA.y * vcnormal.x;
            float rn1B = vcp1.rB.x * vcnormal.y - vcp1.rB.y * vcnormal.x;
            float rn2A = vcp2.rA.x * vcnormal.y - vcp2.rA.y * vcnormal.x;
            float rn2B = vcp2.rB.x * vcnormal.y - vcp2.rB.y * vcnormal.x;
            float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
            float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
            float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
            if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) {
                // K is safe to invert.
                vc.K.ex.x = k11;
                vc.K.ex.y = k12;
                vc.K.ey.x = k12;
                vc.K.ey.y = k22;
                vc.K.invertToOut(vc.normalMass);
            } else {
                // The constraints are redundant, just use one.
                // TODO_ERIN use deepest?
                vc.pointCount = 1;
            }
        }
    }
}

Example 14 with Vec2

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

the class ContactSolver method solveTOIPositionConstraints.

// Sequential position solver for position constraints.
public boolean solveTOIPositionConstraints(int toiIndexA, int toiIndexB) {
    float minSeparation = 0.0f;
    for (int i = 0; i < m_count; ++i) {
        ContactPositionConstraint pc = m_positionConstraints[i];
        int indexA = pc.indexA;
        int indexB = pc.indexB;
        Vec2 localCenterA = pc.localCenterA;
        Vec2 localCenterB = pc.localCenterB;
        final float localCenterAx = localCenterA.x;
        final float localCenterAy = localCenterA.y;
        final float localCenterBx = localCenterB.x;
        final float localCenterBy = localCenterB.y;
        int pointCount = pc.pointCount;
        float mA = 0.0f;
        float iA = 0.0f;
        if (indexA == toiIndexA || indexA == toiIndexB) {
            mA = pc.invMassA;
            iA = pc.invIA;
        }
        float mB = 0f;
        float iB = 0f;
        if (indexB == toiIndexA || indexB == toiIndexB) {
            mB = pc.invMassB;
            iB = pc.invIB;
        }
        Vec2 cA = m_positions[indexA].c;
        float aA = m_positions[indexA].a;
        Vec2 cB = m_positions[indexB].c;
        float aB = m_positions[indexB].a;
        // Solve normal constraints
        for (int j = 0; j < pointCount; ++j) {
            final Rotation xfAq = xfA.q;
            final Rotation xfBq = xfB.q;
            xfAq.set(aA);
            xfBq.set(aB);
            xfA.p.x = cA.x - xfAq.c * localCenterAx + xfAq.s * localCenterAy;
            xfA.p.y = cA.y - xfAq.s * localCenterAx - xfAq.c * localCenterAy;
            xfB.p.x = cB.x - xfBq.c * localCenterBx + xfBq.s * localCenterBy;
            xfB.p.y = cB.y - xfBq.s * localCenterBx - xfBq.c * localCenterBy;
            final PositionSolverManifold psm = psolver;
            psm.initialize(pc, xfA, xfB, j);
            Vec2 normal = psm.normal;
            Vec2 point = psm.point;
            float separation = psm.separation;
            float rAx = point.x - cA.x;
            float rAy = point.y - cA.y;
            float rBx = point.x - cB.x;
            float rBy = point.y - cB.y;
            // Track max constraint error.
            minSeparation = Math.min(minSeparation, separation);
            // Prevent large corrections and allow slop.
            float C = FXGLMath.clamp(JBoxSettings.toiBaugarte * (separation + JBoxSettings.linearSlop), -JBoxSettings.maxLinearCorrection, 0.0f);
            // Compute the effective mass.
            float rnA = rAx * normal.y - rAy * normal.x;
            float rnB = rBx * normal.y - rBy * normal.x;
            float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
            // Compute normal impulse
            float impulse = K > 0.0f ? -C / K : 0.0f;
            float Px = normal.x * impulse;
            float Py = normal.y * impulse;
            cA.x -= Px * mA;
            cA.y -= Py * mA;
            aA -= iA * (rAx * Py - rAy * Px);
            cB.x += Px * mB;
            cB.y += Py * mB;
            aB += iB * (rBx * Py - rBy * Px);
        }
        m_positions[indexA].a = aA;
        m_positions[indexB].a = aB;
    }
    // push the separation above -_linearSlop.
    return minSeparation >= -1.5f * JBoxSettings.linearSlop;
}

Example 15 with Vec2

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

the class MouseJoint method initVelocityConstraints.

@Override
public void initVelocityConstraints(final SolverData data) {
    m_indexB = m_bodyB.m_islandIndex;
    m_localCenterB.set(m_bodyB.m_sweep.localCenter);
    m_invMassB = m_bodyB.m_invMass;
    m_invIB = m_bodyB.m_invI;
    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 qB = pool.popRot();
    qB.set(aB);
    float mass = m_bodyB.getMass();
    // Frequency
    float omega = 2.0f * (float) FXGLMath.PI * m_frequencyHz;
    // Damping coefficient
    float d = 2.0f * mass * m_dampingRatio * omega;
    // Spring stiffness
    float k = mass * (omega * omega);
    // magic formulas
    // gamma has units of inverse mass.
    // beta has units of inverse time.
    float h = data.step.dt;
    assert d + h * k > JBoxSettings.EPSILON;
    m_gamma = h * (d + h * k);
    if (m_gamma != 0.0f) {
        m_gamma = 1.0f / m_gamma;
    }
    m_beta = h * k * m_gamma;
    Vec2 temp = pool.popVec2();
    // Compute the effective mass matrix.
    Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
    // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
    // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
    // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
    final Mat22 K = pool.popMat22();
    K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
    K.ex.y = -m_invIB * m_rB.x * m_rB.y;
    K.ey.x = K.ex.y;
    K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
    K.invertToOut(m_mass);
    m_C.set(cB).addLocal(m_rB).subLocal(m_targetA);
    m_C.mulLocal(m_beta);
    // Cheat with some damping
    wB *= 0.98f;
    if (data.step.warmStarting) {
        m_impulse.mulLocal(data.step.dtRatio);
        vB.x += m_invMassB * m_impulse.x;
        vB.y += m_invMassB * m_impulse.y;
        wB += m_invIB * Vec2.cross(m_rB, m_impulse);
    } else {
        m_impulse.setZero();
    }
    // data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;
    pool.pushVec2(1);
    pool.pushMat22(1);
    pool.pushRot(1);
}