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

Example 16 with Rotation

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

the class EdgeShape method computeAABB.

@Override
@SuppressWarnings("PMD.UselessParentheses")
public void computeAABB(AABB aabb, Transform xf, int childIndex) {
    final Vec2 lowerBound = aabb.lowerBound;
    final Vec2 upperBound = aabb.upperBound;
    final Rotation xfq = xf.q;
    final float v1x = (xfq.c * m_vertex1.x - xfq.s * m_vertex1.y) + xf.p.x;
    final float v1y = (xfq.s * m_vertex1.x + xfq.c * m_vertex1.y) + xf.p.y;
    final float v2x = (xfq.c * m_vertex2.x - xfq.s * m_vertex2.y) + xf.p.x;
    final float v2y = (xfq.s * m_vertex2.x + xfq.c * m_vertex2.y) + xf.p.y;
    lowerBound.x = v1x < v2x ? v1x : v2x;
    lowerBound.y = v1y < v2y ? v1y : v2y;
    upperBound.x = v1x > v2x ? v1x : v2x;
    upperBound.y = v1y > v2y ? v1y : v2y;
    lowerBound.x -= getRadius();
    lowerBound.y -= getRadius();
    upperBound.x += getRadius();
    upperBound.y += getRadius();
}

Example 17 with Rotation

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

the class Collision method collidePolygonAndCircle.

/**
 * Compute the collision manifold between a polygon and a circle.
 *
 * @param manifold
 * @param polygon
 * @param xfA
 * @param circle
 * @param xfB
 */
@SuppressWarnings("PMD.UselessParentheses")
public void collidePolygonAndCircle(Manifold manifold, final PolygonShape polygon, final Transform xfA, final CircleShape circle, final Transform xfB) {
    manifold.pointCount = 0;
    // Vec2 v = circle.m_p;
    // Compute circle position in the frame of the polygon.
    // before inline:
    // Transform.mulToOutUnsafe(xfB, circle.m_p, c);
    // Transform.mulTransToOut(xfA, c, cLocal);
    // final float cLocalx = cLocal.x;
    // final float cLocaly = cLocal.y;
    // after inline:
    final Vec2 circlep = circle.center;
    final Rotation xfBq = xfB.q;
    final Rotation xfAq = xfA.q;
    final float cx = (xfBq.c * circlep.x - xfBq.s * circlep.y) + xfB.p.x;
    final float cy = (xfBq.s * circlep.x + xfBq.c * circlep.y) + xfB.p.y;
    final float px = cx - xfA.p.x;
    final float py = cy - xfA.p.y;
    final float cLocalx = (xfAq.c * px + xfAq.s * py);
    final float cLocaly = (-xfAq.s * px + xfAq.c * py);
    // end inline
    // Find the min separating edge.
    int normalIndex = 0;
    float separation = -Float.MAX_VALUE;
    final float radius = polygon.getRadius() + circle.getRadius();
    final int vertexCount = polygon.getVertexCount();
    final Vec2[] vertices = polygon.m_vertices;
    final Vec2[] normals = polygon.m_normals;
    for (int i = 0; i < vertexCount; i++) {
        // before inline
        // temp.set(cLocal).subLocal(vertices[i]);
        // float s = Vec2.dot(normals[i], temp);
        // after inline
        final Vec2 vertex = vertices[i];
        final float tempx = cLocalx - vertex.x;
        final float tempy = cLocaly - vertex.y;
        float s = normals[i].x * tempx + normals[i].y * tempy;
        if (s > radius) {
            // early out
            return;
        }
        if (s > separation) {
            separation = s;
            normalIndex = i;
        }
    }
    // Vertices that subtend the incident face.
    final int vertIndex1 = normalIndex;
    final int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
    final Vec2 v1 = vertices[vertIndex1];
    final Vec2 v2 = vertices[vertIndex2];
    // If the center is inside the polygon ...
    if (separation < JBoxSettings.EPSILON) {
        manifold.pointCount = 1;
        manifold.type = ManifoldType.FACE_A;
        // before inline:
        // manifold.localNormal.set(normals[normalIndex]);
        // manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f);
        // manifold.points[0].localPoint.set(circle.m_p);
        // after inline:
        final Vec2 normal = normals[normalIndex];
        manifold.localNormal.x = normal.x;
        manifold.localNormal.y = normal.y;
        manifold.localPoint.x = (v1.x + v2.x) * .5f;
        manifold.localPoint.y = (v1.y + v2.y) * .5f;
        final ManifoldPoint mpoint = manifold.points[0];
        mpoint.localPoint.x = circlep.x;
        mpoint.localPoint.y = circlep.y;
        mpoint.id.zero();
        return;
    }
    // Compute barycentric coordinates
    // before inline:
    // temp.set(cLocal).subLocal(v1);
    // temp2.set(v2).subLocal(v1);
    // float u1 = Vec2.dot(temp, temp2);
    // temp.set(cLocal).subLocal(v2);
    // temp2.set(v1).subLocal(v2);
    // float u2 = Vec2.dot(temp, temp2);
    // after inline:
    final float tempX = cLocalx - v1.x;
    final float tempY = cLocaly - v1.y;
    final float temp2X = v2.x - v1.x;
    final float temp2Y = v2.y - v1.y;
    final float u1 = tempX * temp2X + tempY * temp2Y;
    final float temp3X = cLocalx - v2.x;
    final float temp3Y = cLocaly - v2.y;
    final float temp4X = v1.x - v2.x;
    final float temp4Y = v1.y - v2.y;
    final float u2 = temp3X * temp4X + temp3Y * temp4Y;
    if (u1 <= 0f) {
        // inlined
        final float dx = cLocalx - v1.x;
        final float dy = cLocaly - v1.y;
        if (dx * dx + dy * dy > radius * radius) {
            return;
        }
        manifold.pointCount = 1;
        manifold.type = ManifoldType.FACE_A;
        // before inline:
        // manifold.localNormal.set(cLocal).subLocal(v1);
        // after inline:
        manifold.localNormal.x = cLocalx - v1.x;
        manifold.localNormal.y = cLocaly - v1.y;
        // end inline
        manifold.localNormal.getLengthAndNormalize();
        manifold.localPoint.set(v1);
        manifold.points[0].localPoint.set(circlep);
        manifold.points[0].id.zero();
    } else if (u2 <= 0.0f) {
        // inlined
        final float dx = cLocalx - v2.x;
        final float dy = cLocaly - v2.y;
        if (dx * dx + dy * dy > radius * radius) {
            return;
        }
        manifold.pointCount = 1;
        manifold.type = ManifoldType.FACE_A;
        // before inline:
        // manifold.localNormal.set(cLocal).subLocal(v2);
        // after inline:
        manifold.localNormal.x = cLocalx - v2.x;
        manifold.localNormal.y = cLocaly - v2.y;
        // end inline
        manifold.localNormal.getLengthAndNormalize();
        manifold.localPoint.set(v2);
        manifold.points[0].localPoint.set(circlep);
        manifold.points[0].id.zero();
    } else {
        // Vec2 faceCenter = 0.5f * (v1 + v2);
        // (temp is faceCenter)
        // before inline:
        // temp.set(v1).addLocal(v2).mulLocal(.5f);
        // 
        // temp2.set(cLocal).subLocal(temp);
        // separation = Vec2.dot(temp2, normals[vertIndex1]);
        // if (separation > radius) {
        // return;
        // }
        // after inline:
        final float fcx = (v1.x + v2.x) * .5f;
        final float fcy = (v1.y + v2.y) * .5f;
        final float tx = cLocalx - fcx;
        final float ty = cLocaly - fcy;
        final Vec2 normal = normals[vertIndex1];
        separation = tx * normal.x + ty * normal.y;
        if (separation > radius) {
            return;
        }
        // end inline
        manifold.pointCount = 1;
        manifold.type = ManifoldType.FACE_A;
        manifold.localNormal.set(normals[vertIndex1]);
        // (faceCenter)
        manifold.localPoint.x = fcx;
        manifold.localPoint.y = fcy;
        manifold.points[0].localPoint.set(circlep);
        manifold.points[0].id.zero();
    }
}

Example 18 with Rotation

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

the class Collision method findIncidentEdge.

@SuppressWarnings("PMD.UselessParentheses")
public void findIncidentEdge(final ClipVertex[] c, final PolygonShape poly1, final Transform xf1, int edge1, final PolygonShape poly2, final Transform xf2) {
    int count1 = poly1.getVertexCount();
    final Vec2[] normals1 = poly1.m_normals;
    int count2 = poly2.getVertexCount();
    final Vec2[] vertices2 = poly2.m_vertices;
    final Vec2[] normals2 = poly2.m_normals;
    assert 0 <= edge1 && edge1 < count1;
    final ClipVertex c0 = c[0];
    final ClipVertex c1 = c[1];
    final Rotation xf1q = xf1.q;
    final Rotation xf2q = xf2.q;
    // Get the normal of the reference edge in poly2's frame.
    // Vec2 normal1 = MulT(xf2.R, Mul(xf1.R, normals1[edge1]));
    // before inline:
    // Rot.mulToOutUnsafe(xf1.q, normals1[edge1], normal1); // temporary
    // Rot.mulTrans(xf2.q, normal1, normal1);
    // after inline:
    final Vec2 v = normals1[edge1];
    final float tempx = xf1q.c * v.x - xf1q.s * v.y;
    final float tempy = xf1q.s * v.x + xf1q.c * v.y;
    final float normal1x = xf2q.c * tempx + xf2q.s * tempy;
    final float normal1y = -xf2q.s * tempx + xf2q.c * tempy;
    // end inline
    // Find the incident edge on poly2.
    int index = 0;
    float minDot = Float.MAX_VALUE;
    for (int i = 0; i < count2; ++i) {
        Vec2 b = normals2[i];
        float dot = normal1x * b.x + normal1y * b.y;
        if (dot < minDot) {
            minDot = dot;
            index = i;
        }
    }
    // Build the clip vertices for the incident edge.
    int i1 = index;
    int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
    // c0.v = Mul(xf2, vertices2[i1]);
    Vec2 v1 = vertices2[i1];
    Vec2 out = c0.v;
    out.x = (xf2q.c * v1.x - xf2q.s * v1.y) + xf2.p.x;
    out.y = (xf2q.s * v1.x + xf2q.c * v1.y) + xf2.p.y;
    c0.id.indexA = (byte) edge1;
    c0.id.indexB = (byte) i1;
    c0.id.typeA = (byte) ContactID.Type.FACE.ordinal();
    c0.id.typeB = (byte) ContactID.Type.VERTEX.ordinal();
    // c1.v = Mul(xf2, vertices2[i2]);
    Vec2 v2 = vertices2[i2];
    Vec2 out1 = c1.v;
    out1.x = (xf2q.c * v2.x - xf2q.s * v2.y) + xf2.p.x;
    out1.y = (xf2q.s * v2.x + xf2q.c * v2.y) + xf2.p.y;
    c1.id.indexA = (byte) edge1;
    c1.id.indexB = (byte) i2;
    c1.id.typeA = (byte) ContactID.Type.FACE.ordinal();
    c1.id.typeB = (byte) ContactID.Type.VERTEX.ordinal();
}

Example 19 with Rotation

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

the class WeldJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(final 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 Rotation qA = pool.popRot();
    final Rotation qB = pool.popRot();
    final Vec2 temp = pool.popVec2();
    final Vec2 rA = pool.popVec2();
    final Vec2 rB = pool.popVec2();
    qA.set(aA);
    qB.set(aB);
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
    float positionError, angularError;
    final Mat33 K = pool.popMat33();
    final Vec2 C1 = pool.popVec2();
    final Vec2 P = pool.popVec2();
    K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
    K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
    K.ez.x = -rA.y * iA - rB.y * iB;
    K.ex.y = K.ey.x;
    K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
    K.ez.y = rA.x * iA + rB.x * iB;
    K.ex.z = K.ez.x;
    K.ey.z = K.ez.y;
    K.ez.z = iA + iB;
    if (m_frequencyHz > 0.0f) {
        C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
        positionError = C1.length();
        angularError = 0.0f;
        K.solve22ToOut(C1, P);
        P.negateLocal();
        cA.x -= mA * P.x;
        cA.y -= mA * P.y;
        aA -= iA * Vec2.cross(rA, P);
        cB.x += mB * P.x;
        cB.y += mB * P.y;
        aB += iB * Vec2.cross(rB, P);
    } else {
        C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
        float C2 = aB - aA - m_referenceAngle;
        positionError = C1.length();
        angularError = FXGLMath.abs(C2);
        final Vec3 C = pool.popVec3();
        final Vec3 impulse = pool.popVec3();
        C.set(C1.x, C1.y, C2);
        K.solve33ToOut(C, impulse);
        impulse.negateLocal();
        P.set(impulse.x, impulse.y);
        cA.x -= mA * P.x;
        cA.y -= mA * P.y;
        aA -= iA * (Vec2.cross(rA, P) + impulse.z);
        cB.x += mB * P.x;
        cB.y += mB * P.y;
        aB += iB * (Vec2.cross(rB, P) + impulse.z);
        pool.pushVec3(2);
    }
    // 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(5);
    pool.pushRot(2);
    pool.pushMat33(1);
    return positionError <= JBoxSettings.linearSlop && angularError <= JBoxSettings.angularSlop;
}

Example 20 with Rotation

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

the class WeldJoint 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);
    // J = [-I -r1_skew I r2_skew]
    // [ 0 -1 0 1]
    // r_skew = [-ry; rx]
    // Matlab
    // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
    // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
    // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    final Mat33 K = pool.popMat33();
    K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
    K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
    K.ez.x = -m_rA.y * iA - m_rB.y * iB;
    K.ex.y = K.ey.x;
    K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
    K.ez.y = m_rA.x * iA + m_rB.x * iB;
    K.ex.z = K.ez.x;
    K.ey.z = K.ez.y;
    K.ez.z = iA + iB;
    if (m_frequencyHz > 0.0f) {
        K.getInverse22(m_mass);
        float invM = iA + iB;
        float m = invM > 0.0f ? 1.0f / invM : 0.0f;
        float C = aB - aA - m_referenceAngle;
        // Frequency
        float omega = 2.0f * (float) FXGLMath.PI * m_frequencyHz;
        // Damping coefficient
        float d = 2.0f * m * m_dampingRatio * omega;
        // Spring stiffness
        float k = m * omega * omega;
        // magic formulas
        float h = data.step.dt;
        m_gamma = h * (d + h * k);
        m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
        m_bias = C * h * k * m_gamma;
        invM += m_gamma;
        m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
    } else {
        K.getSymInverse33(m_mass);
        m_gamma = 0.0f;
        m_bias = 0.0f;
    }
    if (data.step.warmStarting) {
        final Vec2 P = pool.popVec2();
        // Scale impulses to support a variable time step.
        m_impulse.mulLocal(data.step.dtRatio);
        P.set(m_impulse.x, m_impulse.y);
        vA.x -= mA * P.x;
        vA.y -= mA * P.y;
        wA -= iA * (Vec2.cross(m_rA, P) + m_impulse.z);
        vB.x += mB * P.x;
        vB.y += mB * P.y;
        wB += iB * (Vec2.cross(m_rB, P) + m_impulse.z);
        pool.pushVec2(1);
    } else {
        m_impulse.setZero();
    }
    // 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);
    pool.pushMat33(1);
}