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

Example 1 with Rotation

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

the class CircleShape method testPoint.

@Override
public boolean testPoint(final Transform transform, final Vec2 p) {
    // Rot.mulToOutUnsafe(transform.q, m_p, center);
    // center.addLocal(transform.p);
    // 
    // final Vec2 d = center.subLocal(p).negateLocal();
    // return Vec2.dot(d, d) <= radius * radius;
    final Rotation q = transform.q;
    final Vec2 tp = transform.p;
    float centerx = -(q.c * m_p.x - q.s * m_p.y + tp.x - p.x);
    float centery = -(q.s * m_p.x + q.c * m_p.y + tp.y - p.y);
    return centerx * centerx + centery * centery <= getRadius() * getRadius();
}

Example 2 with Rotation

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

the class CircleShape method computeAABB.

@Override
public void computeAABB(final AABB aabb, final Transform transform, int childIndex) {
    final Rotation tq = transform.q;
    final Vec2 tp = transform.p;
    final float px = tq.c * m_p.x - tq.s * m_p.y + tp.x;
    final float py = tq.s * m_p.x + tq.c * m_p.y + tp.y;
    aabb.lowerBound.x = px - getRadius();
    aabb.lowerBound.y = py - getRadius();
    aabb.upperBound.x = px + getRadius();
    aabb.upperBound.y = py + getRadius();
}

Example 3 with Rotation

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

the class PolygonShape method testPoint.

@Override
public boolean testPoint(final Transform xf, final Vec2 p) {
    float tempx, tempy;
    final Rotation 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;
    for (int i = 0; i < vertexCount; ++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 4 with Rotation

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

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