Examples with Rotation com.almasb.fxgl.physics.box2d.common.Rotation used on opensource projects

Example 6 with Rotation

use of com.almasb.fxgl.physics.box2d.common.Rotation 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 7 with Rotation

use of com.almasb.fxgl.physics.box2d.common.Rotation 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);
}

Example 8 with Rotation

use of com.almasb.fxgl.physics.box2d.common.Rotation 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 9 with Rotation

use of com.almasb.fxgl.physics.box2d.common.Rotation in project FXGL by AlmasB.

the class PulleyJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(final SolverData data) {
    final Rotation qA = pool.popRot();
    final Rotation qB = pool.popRot();
    final Vec2 rA = pool.popVec2();
    final Vec2 rB = pool.popVec2();
    final Vec2 uA = pool.popVec2();
    final Vec2 uB = pool.popVec2();
    final Vec2 temp = pool.popVec2();
    final Vec2 PA = pool.popVec2();
    final Vec2 PB = pool.popVec2();
    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;
    qA.set(aA);
    qB.set(aB);
    Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
    uA.set(cA).addLocal(rA).subLocal(m_groundAnchorA);
    uB.set(cB).addLocal(rB).subLocal(m_groundAnchorB);
    float lengthA = uA.length();
    float lengthB = uB.length();
    if (lengthA > 10.0f * JBoxSettings.linearSlop) {
        uA.mulLocal(1.0f / lengthA);
    } else {
        uA.setZero();
    }
    if (lengthB > 10.0f * JBoxSettings.linearSlop) {
        uB.mulLocal(1.0f / lengthB);
    } else {
        uB.setZero();
    }
    // Compute effective mass.
    float ruA = Vec2.cross(rA, uA);
    float ruB = Vec2.cross(rB, uB);
    float mA = m_invMassA + m_invIA * ruA * ruA;
    float mB = m_invMassB + m_invIB * ruB * ruB;
    float mass = mA + m_ratio * m_ratio * mB;
    if (mass > 0.0f) {
        mass = 1.0f / mass;
    }
    float C = m_constant - lengthA - m_ratio * lengthB;
    float linearError = FXGLMath.abs(C);
    float impulse = -mass * C;
    PA.set(uA).mulLocal(-impulse);
    PB.set(uB).mulLocal(-m_ratio * impulse);
    cA.x += m_invMassA * PA.x;
    cA.y += m_invMassA * PA.y;
    aA += m_invIA * Vec2.cross(rA, PA);
    cB.x += m_invMassB * PB.x;
    cB.y += m_invMassB * PB.y;
    aB += m_invIB * Vec2.cross(rB, PB);
    // data.positions[m_indexA].c.set(cA);
    data.positions[m_indexA].a = aA;
    // data.positions[m_indexB].c.set(cB);
    data.positions[m_indexB].a = aB;
    pool.pushRot(2);
    pool.pushVec2(7);
    return linearError < JBoxSettings.linearSlop;
}

Example 10 with Rotation

use of com.almasb.fxgl.physics.box2d.common.Rotation in project FXGL by AlmasB.

the class PulleyJoint 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 temp = pool.popVec2();
    qA.set(aA);
    qB.set(aB);
    // Compute the effective masses.
    Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
    Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
    m_uA.set(cA).addLocal(m_rA).subLocal(m_groundAnchorA);
    m_uB.set(cB).addLocal(m_rB).subLocal(m_groundAnchorB);
    float lengthA = m_uA.length();
    float lengthB = m_uB.length();
    if (lengthA > 10f * JBoxSettings.linearSlop) {
        m_uA.mulLocal(1.0f / lengthA);
    } else {
        m_uA.setZero();
    }
    if (lengthB > 10f * JBoxSettings.linearSlop) {
        m_uB.mulLocal(1.0f / lengthB);
    } else {
        m_uB.setZero();
    }
    // Compute effective mass.
    float ruA = Vec2.cross(m_rA, m_uA);
    float ruB = Vec2.cross(m_rB, m_uB);
    float mA = m_invMassA + m_invIA * ruA * ruA;
    float mB = m_invMassB + m_invIB * ruB * ruB;
    m_mass = mA + m_ratio * m_ratio * mB;
    if (m_mass > 0.0f) {
        m_mass = 1.0f / m_mass;
    }
    if (data.step.warmStarting) {
        // Scale impulses to support variable time steps.
        m_impulse *= data.step.dtRatio;
        // Warm starting.
        final Vec2 PA = pool.popVec2();
        final Vec2 PB = pool.popVec2();
        PA.set(m_uA).mulLocal(-m_impulse);
        PB.set(m_uB).mulLocal(-m_ratio * m_impulse);
        vA.x += m_invMassA * PA.x;
        vA.y += m_invMassA * PA.y;
        wA += m_invIA * Vec2.cross(m_rA, PA);
        vB.x += m_invMassB * PB.x;
        vB.y += m_invMassB * PB.y;
        wB += m_invIB * Vec2.cross(m_rB, PB);
        pool.pushVec2(2);
    } else {
        m_impulse = 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.pushVec2(1);
    pool.pushRot(2);
}