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

Example 6 with Vec2

use of org.jbox2d.common.Vec2 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 7 with Vec2

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

the class Collision method clipSegmentToLine.

/**
   * Clipping for contact manifolds. Sutherland-Hodgman clipping.
   * 
   * @param vOut
   * @param vIn
   * @param normal
   * @param offset
   * @return
   */
public static final int clipSegmentToLine(final ClipVertex[] vOut, final ClipVertex[] vIn, final Vec2 normal, float offset, int vertexIndexA) {
    // Start with no output points
    int numOut = 0;
    final ClipVertex vIn0 = vIn[0];
    final ClipVertex vIn1 = vIn[1];
    final Vec2 vIn0v = vIn0.v;
    final Vec2 vIn1v = vIn1.v;
    // Calculate the distance of end points to the line
    float distance0 = Vec2.dot(normal, vIn0v) - offset;
    float distance1 = Vec2.dot(normal, vIn1v) - offset;
    // If the points are behind the plane
    if (distance0 <= 0.0f) {
        vOut[numOut++].set(vIn0);
    }
    if (distance1 <= 0.0f) {
        vOut[numOut++].set(vIn1);
    }
    // If the points are on different sides of the plane
    if (distance0 * distance1 < 0.0f) {
        // Find intersection point of edge and plane
        float interp = distance0 / (distance0 - distance1);
        ClipVertex vOutNO = vOut[numOut];
        // vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
        vOutNO.v.x = vIn0v.x + interp * (vIn1v.x - vIn0v.x);
        vOutNO.v.y = vIn0v.y + interp * (vIn1v.y - vIn0v.y);
        // VertexA is hitting edgeB.
        vOutNO.id.indexA = (byte) vertexIndexA;
        vOutNO.id.indexB = vIn0.id.indexB;
        vOutNO.id.typeA = (byte) ContactID.Type.VERTEX.ordinal();
        vOutNO.id.typeB = (byte) ContactID.Type.FACE.ordinal();
        ++numOut;
    }
    return numOut;
}

Example 8 with Vec2

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

the class Collision method collideEdgeAndCircle.

// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
public void collideEdgeAndCircle(Manifold manifold, final EdgeShape edgeA, final Transform xfA, final CircleShape circleB, final Transform xfB) {
    manifold.pointCount = 0;
    // Compute circle in frame of edge
    // Vec2 Q = MulT(xfA, Mul(xfB, circleB.m_p));
    Transform.mulToOutUnsafe(xfB, circleB.m_p, temp);
    Transform.mulTransToOutUnsafe(xfA, temp, Q);
    final Vec2 A = edgeA.m_vertex1;
    final Vec2 B = edgeA.m_vertex2;
    e.set(B).subLocal(A);
    // Barycentric coordinates
    float u = Vec2.dot(e, temp.set(B).subLocal(Q));
    float v = Vec2.dot(e, temp.set(Q).subLocal(A));
    float radius = edgeA.m_radius + circleB.m_radius;
    // ContactFeature cf;
    cf.indexB = 0;
    cf.typeB = (byte) ContactID.Type.VERTEX.ordinal();
    // Region A
    if (v <= 0.0f) {
        final Vec2 P = A;
        d.set(Q).subLocal(P);
        float dd = Vec2.dot(d, d);
        if (dd > radius * radius) {
            return;
        }
        // Is there an edge connected to A?
        if (edgeA.m_hasVertex0) {
            final Vec2 A1 = edgeA.m_vertex0;
            final Vec2 B1 = A;
            e1.set(B1).subLocal(A1);
            float u1 = Vec2.dot(e1, temp.set(B1).subLocal(Q));
            // Is the circle in Region AB of the previous edge?
            if (u1 > 0.0f) {
                return;
            }
        }
        cf.indexA = 0;
        cf.typeA = (byte) ContactID.Type.VERTEX.ordinal();
        manifold.pointCount = 1;
        manifold.type = Manifold.ManifoldType.CIRCLES;
        manifold.localNormal.setZero();
        manifold.localPoint.set(P);
        // manifold.points[0].id.key = 0;
        manifold.points[0].id.set(cf);
        manifold.points[0].localPoint.set(circleB.m_p);
        return;
    }
    // Region B
    if (u <= 0.0f) {
        Vec2 P = B;
        d.set(Q).subLocal(P);
        float dd = Vec2.dot(d, d);
        if (dd > radius * radius) {
            return;
        }
        // Is there an edge connected to B?
        if (edgeA.m_hasVertex3) {
            final Vec2 B2 = edgeA.m_vertex3;
            final Vec2 A2 = B;
            final Vec2 e2 = e1;
            e2.set(B2).subLocal(A2);
            float v2 = Vec2.dot(e2, temp.set(Q).subLocal(A2));
            // Is the circle in Region AB of the next edge?
            if (v2 > 0.0f) {
                return;
            }
        }
        cf.indexA = 1;
        cf.typeA = (byte) ContactID.Type.VERTEX.ordinal();
        manifold.pointCount = 1;
        manifold.type = Manifold.ManifoldType.CIRCLES;
        manifold.localNormal.setZero();
        manifold.localPoint.set(P);
        // manifold.points[0].id.key = 0;
        manifold.points[0].id.set(cf);
        manifold.points[0].localPoint.set(circleB.m_p);
        return;
    }
    // Region AB
    float den = Vec2.dot(e, e);
    assert (den > 0.0f);
    // Vec2 P = (1.0f / den) * (u * A + v * B);
    P.set(A).mulLocal(u).addLocal(temp.set(B).mulLocal(v));
    P.mulLocal(1.0f / den);
    d.set(Q).subLocal(P);
    float dd = Vec2.dot(d, d);
    if (dd > radius * radius) {
        return;
    }
    n.x = -e.y;
    n.y = e.x;
    if (Vec2.dot(n, temp.set(Q).subLocal(A)) < 0.0f) {
        n.set(-n.x, -n.y);
    }
    n.normalize();
    cf.indexA = 0;
    cf.typeA = (byte) ContactID.Type.FACE.ordinal();
    manifold.pointCount = 1;
    manifold.type = Manifold.ManifoldType.FACE_A;
    manifold.localNormal.set(n);
    manifold.localPoint.set(A);
    // manifold.points[0].id.key = 0;
    manifold.points[0].id.set(cf);
    manifold.points[0].localPoint.set(circleB.m_p);
}

Example 9 with Vec2

use of org.jbox2d.common.Vec2 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 10 with Vec2

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

the class ChainShape method createLoop.

/** Create a loop. This automatically adjusts connectivity.
	 * @param vertices an array of vertices, these are copied */
public void createLoop(Vector2[] vertices) {
    Vec2[] v = new Vec2[vertices.length];
    for (int i = 0; i < vertices.length; i++) {
        v[i] = new Vec2(vertices[i].x, vertices[i].y);
    }
    shape.createLoop(v, v.length);
    isLooped = true;
}