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

Example 26 with Rotation

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

the class ParticleSystem method solveRigid.

private 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;
            Rotation rotation = tempRotation;
            rotation.set(step.dt * group.m_angularVelocity);
            Rotation.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 27 with Rotation

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

the class PrismaticJoint 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 d = pool.popVec2();
    final Vec2 axis = pool.popVec2();
    final Vec2 perp = pool.popVec2();
    final Vec2 temp = pool.popVec2();
    final Vec2 C1 = pool.popVec2();
    final Vec3 impulse = pool.popVec3();
    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);
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    // Compute fresh Jacobians
    Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
    d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
    Rotation.mulToOutUnsafe(qA, m_localXAxisA, axis);
    float a1 = Vec2.cross(temp.set(d).addLocal(rA), axis);
    float a2 = Vec2.cross(rB, axis);
    Rotation.mulToOutUnsafe(qA, m_localYAxisA, perp);
    float s1 = Vec2.cross(temp.set(d).addLocal(rA), perp);
    float s2 = Vec2.cross(rB, perp);
    C1.x = Vec2.dot(perp, d);
    C1.y = aB - aA - m_referenceAngle;
    float linearError = FXGLMath.abs(C1.x);
    float angularError = FXGLMath.abs(C1.y);
    boolean active = false;
    float C2 = 0.0f;
    if (m_enableLimit) {
        float translation = Vec2.dot(axis, d);
        if (FXGLMath.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * JBoxSettings.linearSlop) {
            // Prevent large angular corrections
            C2 = FXGLMath.clamp(translation, -JBoxSettings.maxLinearCorrection, JBoxSettings.maxLinearCorrection);
            linearError = Math.max(linearError, FXGLMath.abs(translation));
            active = true;
        } else if (translation <= m_lowerTranslation) {
            // Prevent large linear corrections and allow some slop.
            C2 = FXGLMath.clamp(translation - m_lowerTranslation + JBoxSettings.linearSlop, -JBoxSettings.maxLinearCorrection, 0.0f);
            linearError = Math.max(linearError, m_lowerTranslation - translation);
            active = true;
        } else if (translation >= m_upperTranslation) {
            // Prevent large linear corrections and allow some slop.
            C2 = FXGLMath.clamp(translation - m_upperTranslation - JBoxSettings.linearSlop, 0.0f, JBoxSettings.maxLinearCorrection);
            linearError = Math.max(linearError, translation - m_upperTranslation);
            active = true;
        }
    }
    if (active) {
        float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
        float k12 = iA * s1 + iB * s2;
        float k13 = iA * s1 * a1 + iB * s2 * a2;
        float k22 = iA + iB;
        if (k22 == 0.0f) {
            // For fixed rotation
            k22 = 1.0f;
        }
        float k23 = iA * a1 + iB * a2;
        float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
        final Mat33 K = pool.popMat33();
        K.ex.set(k11, k12, k13);
        K.ey.set(k12, k22, k23);
        K.ez.set(k13, k23, k33);
        final Vec3 C = pool.popVec3();
        C.x = C1.x;
        C.y = C1.y;
        C.z = C2;
        K.solve33ToOut(C.negateLocal(), impulse);
        pool.pushVec3(1);
        pool.pushMat33(1);
    } else {
        float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
        float k12 = iA * s1 + iB * s2;
        float k22 = iA + iB;
        if (k22 == 0.0f) {
            k22 = 1.0f;
        }
        final Mat22 K = pool.popMat22();
        K.ex.set(k11, k12);
        K.ey.set(k12, k22);
        // temp is impulse1
        K.solveToOut(C1.negateLocal(), temp);
        C1.negateLocal();
        impulse.x = temp.x;
        impulse.y = temp.y;
        impulse.z = 0.0f;
        pool.pushMat22(1);
    }
    float Px = impulse.x * perp.x + impulse.z * axis.x;
    float Py = impulse.x * perp.y + impulse.z * axis.y;
    float LA = impulse.x * s1 + impulse.y + impulse.z * a1;
    float LB = impulse.x * s2 + impulse.y + impulse.z * a2;
    cA.x -= mA * Px;
    cA.y -= mA * Py;
    aA -= iA * LA;
    cB.x += mB * Px;
    cB.y += mB * Py;
    aB += iB * LB;
    // 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.pushVec2(7);
    pool.pushVec3(1);
    pool.pushRot(2);
    return linearError <= JBoxSettings.linearSlop && angularError <= JBoxSettings.angularSlop;
}

Example 28 with Rotation

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

the class RevoluteJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(final SolverData data) {
    final Rotation qA = pool.popRot();
    final Rotation qB = pool.popRot();
    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);
    float angularError = 0.0f;
    float positionError = 0.0f;
    boolean fixedRotation = m_invIA + m_invIB == 0.0f;
    // Solve angular limit constraint.
    if (m_enableLimit && m_limitState != LimitState.INACTIVE && !fixedRotation) {
        float angle = aB - aA - m_referenceAngle;
        float limitImpulse = 0.0f;
        if (m_limitState == LimitState.EQUAL) {
            // Prevent large angular corrections
            float C = FXGLMath.clamp(angle - m_lowerAngle, -JBoxSettings.maxAngularCorrection, JBoxSettings.maxAngularCorrection);
            limitImpulse = -m_motorMass * C;
            angularError = FXGLMath.abs(C);
        } else if (m_limitState == LimitState.AT_LOWER) {
            float C = angle - m_lowerAngle;
            angularError = -C;
            // Prevent large angular corrections and allow some slop.
            C = FXGLMath.clamp(C + JBoxSettings.angularSlop, -JBoxSettings.maxAngularCorrection, 0.0f);
            limitImpulse = -m_motorMass * C;
        } else if (m_limitState == LimitState.AT_UPPER) {
            float C = angle - m_upperAngle;
            angularError = C;
            // Prevent large angular corrections and allow some slop.
            C = FXGLMath.clamp(C - JBoxSettings.angularSlop, 0.0f, JBoxSettings.maxAngularCorrection);
            limitImpulse = -m_motorMass * C;
        }
        aA -= m_invIA * limitImpulse;
        aB += m_invIB * limitImpulse;
    }
    // Solve point-to-point constraint.
    {
        qA.set(aA);
        qB.set(aB);
        final Vec2 rA = pool.popVec2();
        final Vec2 rB = pool.popVec2();
        final Vec2 C = pool.popVec2();
        final Vec2 impulse = pool.popVec2();
        Rotation.mulToOutUnsafe(qA, C.set(m_localAnchorA).subLocal(m_localCenterA), rA);
        Rotation.mulToOutUnsafe(qB, C.set(m_localAnchorB).subLocal(m_localCenterB), rB);
        C.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
        positionError = C.length();
        float mA = m_invMassA, mB = m_invMassB;
        float iA = m_invIA, iB = m_invIB;
        final Mat22 K = pool.popMat22();
        K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
        K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
        K.ey.x = K.ex.y;
        K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
        K.solveToOut(C, impulse);
        impulse.negateLocal();
        cA.x -= mA * impulse.x;
        cA.y -= mA * impulse.y;
        aA -= iA * Vec2.cross(rA, impulse);
        cB.x += mB * impulse.x;
        cB.y += mB * impulse.y;
        aB += iB * Vec2.cross(rB, impulse);
        pool.pushVec2(4);
        pool.pushMat22(1);
    }
    // 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);
    return positionError <= JBoxSettings.linearSlop && angularError <= JBoxSettings.angularSlop;
}

Example 29 with Rotation

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

the class Body method synchronizeTransform.

void synchronizeTransform() {
    // 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.set(m_sweep.a);
    Rotation 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;
}

Example 30 with Rotation

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

the class DistanceJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(final SolverData data) {
    if (m_frequencyHz > 0.0f) {
        return true;
    }
    final Rotation qA = pool.popRot();
    final Rotation qB = pool.popRot();
    final Vec2 rA = pool.popVec2();
    final Vec2 rB = pool.popVec2();
    final Vec2 u = 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, u.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rotation.mulToOutUnsafe(qB, u.set(m_localAnchorB).subLocal(m_localCenterB), rB);
    u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
    float length = u.getLengthAndNormalize();
    float C = length - m_length;
    C = FXGLMath.clamp(C, -JBoxSettings.maxLinearCorrection, JBoxSettings.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);
    // 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.pushVec2(3);
    pool.pushRot(2);
    return FXGLMath.abs(C) < JBoxSettings.linearSlop;
}