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

Example 31 with Rotation

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

the class MotorJoint 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;
    final Vec2 cA = data.positions[m_indexA].c;
    float aA = data.positions[m_indexA].a;
    final Vec2 vA = data.velocities[m_indexA].v;
    float wA = data.velocities[m_indexA].w;
    final Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;
    final 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();
    Mat22 K = pool.popMat22();
    qA.set(aA);
    qB.set(aB);
    // Compute the effective mass matrix.
    // m_rA = b2Mul(qA, -m_localCenterA);
    // m_rB = b2Mul(qB, -m_localCenterB);
    m_rA.x = qA.c * -m_localCenterA.x - qA.s * -m_localCenterA.y;
    m_rA.y = qA.s * -m_localCenterA.x + qA.c * -m_localCenterA.y;
    m_rB.x = qB.c * -m_localCenterB.x - qB.s * -m_localCenterB.y;
    m_rB.y = qB.s * -m_localCenterB.x + qB.c * -m_localCenterB.y;
    // 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;
    K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
    K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
    K.ey.x = K.ex.y;
    K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
    K.invertToOut(m_linearMass);
    m_angularMass = iA + iB;
    if (m_angularMass > 0.0f) {
        m_angularMass = 1.0f / m_angularMass;
    }
    // m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset);
    Rotation.mulToOutUnsafe(qA, linearOffset, temp);
    m_linearError.x = cB.x + m_rB.x - cA.x - m_rA.x - temp.x;
    m_linearError.y = cB.y + m_rB.y - cA.y - m_rA.y - temp.y;
    m_angularError = aB - aA - angularOffset;
    if (data.step.warmStarting) {
        // Scale impulses to support a variable time step.
        linearImpulse.x *= data.step.dtRatio;
        linearImpulse.y *= data.step.dtRatio;
        angularImpulse *= data.step.dtRatio;
        final Vec2 P = linearImpulse;
        vA.x -= mA * P.x;
        vA.y -= mA * P.y;
        wA -= iA * (m_rA.x * P.y - m_rA.y * P.x + angularImpulse);
        vB.x += mB * P.x;
        vB.y += mB * P.y;
        wB += iB * (m_rB.x * P.y - m_rB.y * P.x + angularImpulse);
    } else {
        linearImpulse.setZero();
        angularImpulse = 0.0f;
    }
    pool.pushVec2(1);
    pool.pushMat22(1);
    pool.pushRot(2);
    // 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 32 with Rotation

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

the class EdgeShape method raycast.

// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
@Override
public boolean raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex) {
    float tempx, tempy;
    final Vec2 v1 = m_vertex1;
    final Vec2 v2 = m_vertex2;
    final Rotation xfq = xf.q;
    final Vec2 xfp = xf.p;
    // Put the ray into the edge's frame of reference.
    // b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
    // b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
    tempx = input.p1.x - xfp.x;
    tempy = input.p1.y - xfp.y;
    final float p1x = xfq.c * tempx + xfq.s * tempy;
    final float p1y = -xfq.s * tempx + xfq.c * tempy;
    tempx = input.p2.x - xfp.x;
    tempy = input.p2.y - xfp.y;
    final float p2x = xfq.c * tempx + xfq.s * tempy;
    final float p2y = -xfq.s * tempx + xfq.c * tempy;
    final float dx = p2x - p1x;
    final float dy = p2y - p1y;
    // final Vec2 normal = pool2.set(v2).subLocal(v1);
    // normal.set(normal.y, -normal.x);
    normal.x = v2.y - v1.y;
    normal.y = v1.x - v2.x;
    normal.getLengthAndNormalize();
    final float normalx = normal.x;
    final float normaly = normal.y;
    // q = p1 + t * d
    // dot(normal, q - v1) = 0
    // dot(normal, p1 - v1) + t * dot(normal, d) = 0
    tempx = v1.x - p1x;
    tempy = v1.y - p1y;
    float numerator = normalx * tempx + normaly * tempy;
    float denominator = normalx * dx + normaly * dy;
    if (denominator == 0.0f) {
        return false;
    }
    float t = numerator / denominator;
    if (t < 0.0f || 1.0f < t) {
        return false;
    }
    // Vec2 q = p1 + t * d;
    final float qx = p1x + t * dx;
    final float qy = p1y + t * dy;
    // q = v1 + s * r
    // s = dot(q - v1, r) / dot(r, r)
    // Vec2 r = v2 - v1;
    final float rx = v2.x - v1.x;
    final float ry = v2.y - v1.y;
    final float rr = rx * rx + ry * ry;
    if (rr == 0.0f) {
        return false;
    }
    tempx = qx - v1.x;
    tempy = qy - v1.y;
    // float s = Vec2.dot(pool5, r) / rr;
    float s = (tempx * rx + tempy * ry) / rr;
    if (s < 0.0f || 1.0f < s) {
        return false;
    }
    output.fraction = t;
    if (numerator > 0.0f) {
        // output.normal = -b2Mul(xf.q, normal);
        output.normal.x = -xfq.c * normal.x + xfq.s * normal.y;
        output.normal.y = -xfq.s * normal.x - xfq.c * normal.y;
    } else {
        // output->normal = b2Mul(xf.q, normal);
        output.normal.x = xfq.c * normal.x - xfq.s * normal.y;
        output.normal.y = xfq.s * normal.x + xfq.c * normal.y;
    }
    return true;
}

Example 33 with Rotation

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

the class PolygonShape method containsPoint.

@Override
public boolean containsPoint(final Transform xf, final Vec2 point) {
    final Rotation xfq = xf.q;
    float tempx = point.x - xf.p.x;
    float tempy = point.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 34 with Rotation

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

the class Collision method findMaxSeparation.

/**
 * Find the max separation between poly1 and poly2 using edge normals from poly1.
 */
public void findMaxSeparation(EdgeResults results, PolygonShape poly1, Transform xf1, PolygonShape poly2, Transform xf2) {
    int count1 = poly1.getVertexCount();
    int count2 = poly2.getVertexCount();
    Vec2[] n1s = poly1.m_normals;
    Vec2[] v1s = poly1.m_vertices;
    Vec2[] v2s = poly2.m_vertices;
    Transform.mulTransToOutUnsafe(xf2, xf1, xf);
    final Rotation xfq = xf.q;
    int bestIndex = 0;
    float maxSeparation = -Float.MAX_VALUE;
    for (int i = 0; i < count1; i++) {
        // Get poly1 normal in frame2.
        Rotation.mulToOutUnsafe(xfq, n1s[i], n);
        Transform.mulToOutUnsafe(xf, v1s[i], v1);
        // Find deepest point for normal i.
        float si = Float.MAX_VALUE;
        for (int j = 0; j < count2; ++j) {
            Vec2 v2sj = v2s[j];
            float sij = n.x * (v2sj.x - v1.x) + n.y * (v2sj.y - v1.y);
            if (sij < si) {
                si = sij;
            }
        }
        if (si > maxSeparation) {
            maxSeparation = si;
            bestIndex = i;
        }
    }
    results.edgeIndex = bestIndex;
    results.separation = maxSeparation;
}

Example 35 with Rotation

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

the class Collision method collidePolygons.

/**
 * Compute the collision manifold between two polygons.
 */
@SuppressWarnings("PMD.UselessParentheses")
public void collidePolygons(Manifold manifold, final PolygonShape polyA, final Transform xfA, final PolygonShape polyB, final Transform xfB) {
    // Find edge normal of max separation on A - return if separating axis is found
    // Find edge normal of max separation on B - return if separation axis is found
    // Choose reference edge as min(minA, minB)
    // Find incident edge
    // Clip
    // The normal points from 1 to 2
    manifold.pointCount = 0;
    float totalRadius = polyA.getRadius() + polyB.getRadius();
    findMaxSeparation(results1, polyA, xfA, polyB, xfB);
    if (results1.separation > totalRadius) {
        return;
    }
    findMaxSeparation(results2, polyB, xfB, polyA, xfA);
    if (results2.separation > totalRadius) {
        return;
    }
    // reference polygon
    final PolygonShape poly1;
    // incident polygon
    final PolygonShape poly2;
    Transform xf1, xf2;
    // reference edge
    int edge1;
    boolean flip;
    final float k_tol = 0.1f * JBoxSettings.linearSlop;
    if (results2.separation > results1.separation + k_tol) {
        poly1 = polyB;
        poly2 = polyA;
        xf1 = xfB;
        xf2 = xfA;
        edge1 = results2.edgeIndex;
        manifold.type = ManifoldType.FACE_B;
        flip = true;
    } else {
        poly1 = polyA;
        poly2 = polyB;
        xf1 = xfA;
        xf2 = xfB;
        edge1 = results1.edgeIndex;
        manifold.type = ManifoldType.FACE_A;
        flip = false;
    }
    final Rotation xf1q = xf1.q;
    findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
    int count1 = poly1.getVertexCount();
    final Vec2[] vertices1 = poly1.m_vertices;
    final int iv1 = edge1;
    final int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
    v11.set(vertices1[iv1]);
    v12.set(vertices1[iv2]);
    localTangent.x = v12.x - v11.x;
    localTangent.y = v12.y - v11.y;
    localTangent.getLengthAndNormalize();
    // Vec2 localNormal = Vec2.cross(dv, 1.0f);
    localNormal.x = 1f * localTangent.y;
    localNormal.y = -1f * localTangent.x;
    // Vec2 planePoint = 0.5f * (v11+ v12);
    planePoint.x = (v11.x + v12.x) * .5f;
    planePoint.y = (v11.y + v12.y) * .5f;
    // Rot.mulToOutUnsafe(xf1.q, localTangent, tangent);
    tangent.x = xf1q.c * localTangent.x - xf1q.s * localTangent.y;
    tangent.y = xf1q.s * localTangent.x + xf1q.c * localTangent.y;
    // Vec2.crossToOutUnsafe(tangent, 1f, normal);
    final float normalx = 1f * tangent.y;
    final float normaly = -1f * tangent.x;
    Transform.mulToOut(xf1, v11, v11);
    Transform.mulToOut(xf1, v12, v12);
    // v11 = Mul(xf1, v11);
    // v12 = Mul(xf1, v12);
    // Face offset
    // float frontOffset = Vec2.dot(normal, v11);
    float frontOffset = normalx * v11.x + normaly * v11.y;
    // Side offsets, extended by polytope skin thickness.
    // float sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius;
    // float sideOffset2 = Vec2.dot(tangent, v12) + totalRadius;
    float sideOffset1 = -(tangent.x * v11.x + tangent.y * v11.y) + totalRadius;
    float sideOffset2 = tangent.x * v12.x + tangent.y * v12.y + totalRadius;
    // Clip incident edge against extruded edge1 side edges.
    // ClipVertex clipPoints1[2];
    // ClipVertex clipPoints2[2];
    int np;
    // Clip to box side 1
    // np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
    tangent.negateLocal();
    np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1, iv1);
    tangent.negateLocal();
    if (np < 2) {
        return;
    }
    // Clip to negative box side 1
    np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
    if (np < 2) {
        return;
    }
    // Now clipPoints2 contains the clipped points.
    manifold.localNormal.set(localNormal);
    manifold.localPoint.set(planePoint);
    int pointCount = 0;
    for (int i = 0; i < JBoxSettings.maxManifoldPoints; ++i) {
        // float separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset;
        float separation = normalx * clipPoints2[i].v.x + normaly * clipPoints2[i].v.y - frontOffset;
        if (separation <= totalRadius) {
            ManifoldPoint cp = manifold.points[pointCount];
            // cp.m_localPoint = MulT(xf2, clipPoints2[i].v);
            Vec2 out = cp.localPoint;
            final float px = clipPoints2[i].v.x - xf2.p.x;
            final float py = clipPoints2[i].v.y - xf2.p.y;
            out.x = (xf2.q.c * px + xf2.q.s * py);
            out.y = (-xf2.q.s * px + xf2.q.c * py);
            cp.id.set(clipPoints2[i].id);
            if (flip) {
                // Swap features
                cp.id.flip();
            }
            ++pointCount;
        }
    }
    manifold.pointCount = pointCount;
}