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

Example 1 with Rot

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

the class Collision method findMaxSeparation.

/**
   * Find the max separation between poly1 and poly2 using edge normals from poly1.
   * 
   * @param edgeIndex
   * @param poly1
   * @param xf1
   * @param poly2
   * @param xf2
   * @return
   */
public final void findMaxSeparation(EdgeResults results, final PolygonShape poly1, final Transform xf1, final PolygonShape poly2, final Transform xf2) {
    int count1 = poly1.m_count;
    int count2 = poly2.m_count;
    Vec2[] n1s = poly1.m_normals;
    Vec2[] v1s = poly1.m_vertices;
    Vec2[] v2s = poly2.m_vertices;
    Transform.mulTransToOutUnsafe(xf2, xf1, xf);
    final Rot xfq = xf.q;
    int bestIndex = 0;
    float maxSeparation = -Float.MAX_VALUE;
    for (int i = 0; i < count1; i++) {
        // Get poly1 normal in frame2.
        Rot.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 2 with Rot

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

the class Collision method findIncidentEdge.

public final void findIncidentEdge(final ClipVertex[] c, final PolygonShape poly1, final Transform xf1, int edge1, final PolygonShape poly2, final Transform xf2) {
    int count1 = poly1.m_count;
    final Vec2[] normals1 = poly1.m_normals;
    int count2 = poly2.m_count;
    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 Rot xf1q = xf1.q;
    final Rot 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 3 with Rot

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

the class Collision method collidePolygons.

/**
   * Compute the collision manifold between two polygons.
   * 
   * @param manifold
   * @param polygon1
   * @param xf1
   * @param polygon2
   * @param xf2
   */
public final 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.m_radius + polyB.m_radius;
    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 * Settings.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 Rot xf1q = xf1.q;
    findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
    int count1 = poly1.m_count;
    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.normalize();
    // 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 < Settings.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;
}

Example 4 with Rot

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

the class Collision method collidePolygonAndCircle.

// djm pooling, and from above
/**
   * Compute the collision manifold between a polygon and a circle.
   * 
   * @param manifold
   * @param polygon
   * @param xfA
   * @param circle
   * @param xfB
   */
public final 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.m_p;
    final Rot xfBq = xfB.q;
    final Rot 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.m_radius + circle.m_radius;
    final int vertexCount = polygon.m_count;
    float s;
    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;
        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 < Settings.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.normalize();
        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.normalize();
        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 5 with Rot

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

the class CircleShape method computeAABB.

@Override
public final void computeAABB(final AABB aabb, final Transform transform, int childIndex) {
    final Rot 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 - m_radius;
    aabb.lowerBound.y = py - m_radius;
    aabb.upperBound.x = px + m_radius;
    aabb.upperBound.y = py + m_radius;
}