Examples with Rot org.jbox2d.common.Rot used on opensource projects

Example 16 with Rot

use of org.jbox2d.common.Rot in project libgdx by libgdx.

the class WheelJoint method initVelocityConstraints.

@Override
public void initVelocityConstraints(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;
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    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 Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Vec2 temp = pool.popVec2();
    qA.set(aA);
    qB.set(aB);
    // Compute the effective masses.
    Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
    d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
    // Point to line constraint
    {
        Rot.mulToOut(qA, m_localYAxisA, m_ay);
        m_sAy = Vec2.cross(temp.set(d).addLocal(rA), m_ay);
        m_sBy = Vec2.cross(rB, m_ay);
        m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;
        if (m_mass > 0.0f) {
            m_mass = 1.0f / m_mass;
        }
    }
    // Spring constraint
    m_springMass = 0.0f;
    m_bias = 0.0f;
    m_gamma = 0.0f;
    if (m_frequencyHz > 0.0f) {
        Rot.mulToOut(qA, m_localXAxisA, m_ax);
        m_sAx = Vec2.cross(temp.set(d).addLocal(rA), m_ax);
        m_sBx = Vec2.cross(rB, m_ax);
        float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
        if (invMass > 0.0f) {
            m_springMass = 1.0f / invMass;
            float C = Vec2.dot(d, m_ax);
            // Frequency
            float omega = 2.0f * MathUtils.PI * m_frequencyHz;
            // Damping coefficient
            float d = 2.0f * m_springMass * m_dampingRatio * omega;
            // Spring stiffness
            float k = m_springMass * omega * omega;
            // magic formulas
            float h = data.step.dt;
            m_gamma = h * (d + h * k);
            if (m_gamma > 0.0f) {
                m_gamma = 1.0f / m_gamma;
            }
            m_bias = C * h * k * m_gamma;
            m_springMass = invMass + m_gamma;
            if (m_springMass > 0.0f) {
                m_springMass = 1.0f / m_springMass;
            }
        }
    } else {
        m_springImpulse = 0.0f;
    }
    // Rotational motor
    if (m_enableMotor) {
        m_motorMass = iA + iB;
        if (m_motorMass > 0.0f) {
            m_motorMass = 1.0f / m_motorMass;
        }
    } else {
        m_motorMass = 0.0f;
        m_motorImpulse = 0.0f;
    }
    if (data.step.warmStarting) {
        final Vec2 P = pool.popVec2();
        // Account for variable time step.
        m_impulse *= data.step.dtRatio;
        m_springImpulse *= data.step.dtRatio;
        m_motorImpulse *= data.step.dtRatio;
        P.x = m_impulse * m_ay.x + m_springImpulse * m_ax.x;
        P.y = m_impulse * m_ay.y + m_springImpulse * m_ax.y;
        float LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
        float LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;
        vA.x -= m_invMassA * P.x;
        vA.y -= m_invMassA * P.y;
        wA -= m_invIA * LA;
        vB.x += m_invMassB * P.x;
        vB.y += m_invMassB * P.y;
        wB += m_invIB * LB;
        pool.pushVec2(1);
    } else {
        m_impulse = 0.0f;
        m_springImpulse = 0.0f;
        m_motorImpulse = 0.0f;
    }
    pool.pushRot(2);
    pool.pushVec2(1);
    // data.velocities[m_indexA].v = vA;
    data.velocities[m_indexA].w = wA;
    // data.velocities[m_indexB].v = vB;
    data.velocities[m_indexB].w = wB;
}

Example 17 with Rot

use of org.jbox2d.common.Rot in project libgdx by libgdx.

the class WheelJoint 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;
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Vec2 temp = pool.popVec2();
    qA.set(aA);
    qB.set(aB);
    Rot.mulToOut(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rot.mulToOut(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
    d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA);
    Vec2 ay = pool.popVec2();
    Rot.mulToOut(qA, m_localYAxisA, ay);
    float sAy = Vec2.cross(temp.set(d).addLocal(rA), ay);
    float sBy = Vec2.cross(rB, ay);
    float C = Vec2.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 Vec2 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 MathUtils.abs(C) <= Settings.linearSlop;
}

Example 18 with Rot

use of org.jbox2d.common.Rot in project libgdx by libgdx.

the class ParticleSystem method solveRigid.

void solveRigid(final TimeStep step) {
    for (ParticleGroup group = m_groupList; group != null; group = group.getNext()) {
        if ((group.m_groupFlags & ParticleGroupType.b2_rigidParticleGroup) != 0) {
            group.updateStatistics();
            Vec2 temp = tempVec;
            Vec2 cross = tempVec2;
            Rot rotation = tempRot;
            rotation.set(step.dt * group.m_angularVelocity);
            Rot.mulToOutUnsafe(rotation, group.m_center, cross);
            temp.set(group.m_linearVelocity).mulLocal(step.dt).addLocal(group.m_center).subLocal(cross);
            tempXf.p.set(temp);
            tempXf.q.set(rotation);
            Transform.mulToOut(tempXf, group.m_transform, group.m_transform);
            final Transform velocityTransform = tempXf2;
            velocityTransform.p.x = step.inv_dt * tempXf.p.x;
            velocityTransform.p.y = step.inv_dt * tempXf.p.y;
            velocityTransform.q.s = step.inv_dt * tempXf.q.s;
            velocityTransform.q.c = step.inv_dt * (tempXf.q.c - 1);
            for (int i = group.m_firstIndex; i < group.m_lastIndex; i++) {
                Transform.mulToOutUnsafe(velocityTransform, m_positionBuffer.data[i], m_velocityBuffer.data[i]);
            }
        }
    }
}

Example 19 with Rot

use of org.jbox2d.common.Rot in project libgdx by libgdx.

the class PolygonShape method testPoint.

@Override
public final boolean testPoint(final Transform xf, final Vec2 p) {
    float tempx, tempy;
    final Rot xfq = xf.q;
    tempx = p.x - xf.p.x;
    tempy = p.y - xf.p.y;
    final float pLocalx = xfq.c * tempx + xfq.s * tempy;
    final float pLocaly = -xfq.s * tempx + xfq.c * tempy;
    if (m_debug) {
        System.out.println("--testPoint debug--");
        System.out.println("Vertices: ");
        for (int i = 0; i < m_count; ++i) {
            System.out.println(m_vertices[i]);
        }
        System.out.println("pLocal: " + pLocalx + ", " + pLocaly);
    }
    for (int i = 0; i < m_count; ++i) {
        Vec2 vertex = m_vertices[i];
        Vec2 normal = m_normals[i];
        tempx = pLocalx - vertex.x;
        tempy = pLocaly - vertex.y;
        final float dot = normal.x * tempx + normal.y * tempy;
        if (dot > 0.0f) {
            return false;
        }
    }
    return true;
}

Example 20 with Rot

use of org.jbox2d.common.Rot in project libgdx by libgdx.

the class Body method synchronizeTransform.

public final void synchronizeTransform() {
    // m_xf.q.set(m_sweep.a);
    //
    // // m_xf.position = m_sweep.c - Mul(m_xf.R, m_sweep.localCenter);
    // Rot.mulToOutUnsafe(m_xf.q, m_sweep.localCenter, m_xf.p);
    // m_xf.p.mulLocal(-1).addLocal(m_sweep.c);
    //
    m_xf.q.s = MathUtils.sin(m_sweep.a);
    m_xf.q.c = MathUtils.cos(m_sweep.a);
    Rot q = m_xf.q;
    Vec2 v = m_sweep.localCenter;
    m_xf.p.x = m_sweep.c.x - q.c * v.x + q.s * v.y;
    m_xf.p.y = m_sweep.c.y - q.s * v.x - q.c * v.y;
}