Examples with Tuple2f spacegraph.util.math.Tuple2f used on opensource projects

Example 76 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class Pacman method accept.

@Override
public void accept(GL2 gl) {
    gl.glColor3f(1, 1, 0);
    Draw.poly(this, gl, (PolygonShape) fixtures.shape);
    float a = angle();
    gl.glColor3f(0, 0, 0);
    Tuple2f center = getWorldCenter();
    Draw.rect(gl, center.x + 0.01f * (float) Math.cos(a), center.y + 0.01f * (float) Math.sin(a), 0.25f, 0.25f);
}

Example 77 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

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.radius + polyB.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;
    findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
    int count1 = poly1.vertices;
    final Tuple2f[] vertices1 = poly1.vertex;
    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.negated();
    np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1, iv1);
    tangent.negated();
    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);
            Tuple2f out = cp.localPoint;
            final float px = clipPoints2[i].v.x - xf2.pos.x;
            final float py = clipPoints2[i].v.y - xf2.pos.y;
            out.x = (xf2.c * px + xf2.s * py);
            out.y = (-xf2.s * px + xf2.c * py);
            cp.id.set(clipPoints2[i].id);
            if (flip) {
                // Swap features
                cp.id.flip();
            }
            ++pointCount;
        }
    }
    manifold.pointCount = pointCount;
}

Example 78 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

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 Tuple2f normal, float offset, int vertexIndexA) {
    // Start with no output points
    int numOut = 0;
    final ClipVertex vIn0 = vIn[0];
    final ClipVertex vIn1 = vIn[1];
    final Tuple2f vIn0v = vIn0.v;
    final Tuple2f vIn1v = vIn1.v;
    // Calculate the distance of end points to the line
    float distance0 = Tuple2f.dot(normal, vIn0v) - offset;
    float distance1 = Tuple2f.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 79 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

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.vertices;
    int count2 = poly2.vertices;
    Tuple2f[] n1s = poly1.normals;
    Tuple2f[] v1s = poly1.vertex;
    Tuple2f[] v2s = poly2.vertex;
    Transform.mulTransToOutUnsafe(xf2, xf1, xf);
    final Rot xfq = xf;
    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) {
            Tuple2f 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 80 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class DynamicTree method raycast.

@Override
public void raycast(TreeRayCastCallback callback, RayCastInput input) {
    final Tuple2f p1 = input.p1;
    final Tuple2f p2 = input.p2;
    float p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
    float vx, vy;
    float rx, ry;
    float absVx, absVy;
    float cx, cy;
    float hx, hy;
    float tempx, tempy;
    r.x = p2x - p1x;
    r.y = p2y - p1y;
    assert ((r.x * r.x + r.y * r.y) > 0f);
    r.normalize();
    rx = r.x;
    ry = r.y;
    // v is perpendicular to the segment.
    vx = -1f * ry;
    vy = 1f * rx;
    absVx = Math.abs(vx);
    absVy = Math.abs(vy);
    // Separating axis for segment (Gino, p80).
    // |dot(v, p1 - c)| > dot(|v|, h)
    float maxFraction = input.maxFraction;
    // Build a bounding box for the segment.
    final AABB segAABB = aabb;
    // Vec2 t = p1 + maxFraction * (p2 - p1);
    // before inline
    // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
    // Vec2.minToOut(p1, temp, segAABB.lowerBound);
    // Vec2.maxToOut(p1, temp, segAABB.upperBound);
    tempx = (p2x - p1x) * maxFraction + p1x;
    tempy = (p2y - p1y) * maxFraction + p1y;
    segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
    segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
    segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
    segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
    // end inline
    stackPtr = 0;
    stack[stackPtr++] = m_root;
    while (stackPtr > 0) {
        final DynamicTreeNode node = stack[--stackPtr];
        if (node == null) {
            continue;
        }
        final AABB nodeAABB = node.aabb;
        if (!AABB.testOverlap(nodeAABB, segAABB)) {
            continue;
        }
        // Separating axis for segment (Gino, p80).
        // |dot(v, p1 - c)| > dot(|v|, h)
        // node.aabb.getCenterToOut(c);
        // node.aabb.getExtentsToOut(h);
        cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x) * .5f;
        cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5f;
        hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5f;
        hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5f;
        tempx = p1x - cx;
        tempy = p1y - cy;
        float separation = Math.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy);
        if (separation > 0.0f) {
            continue;
        }
        if (node.child1 == null) {
            subInput.p1.x = p1x;
            subInput.p1.y = p1y;
            subInput.p2.x = p2x;
            subInput.p2.y = p2y;
            subInput.maxFraction = maxFraction;
            float value = callback.raycastCallback(subInput, node.id);
            if (value == 0.0f) {
                // The client has terminated the ray cast.
                return;
            }
            if (value > 0.0f) {
                // Update segment bounding box.
                maxFraction = value;
                // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
                // Vec2.minToOut(p1, temp, segAABB.lowerBound);
                // Vec2.maxToOut(p1, temp, segAABB.upperBound);
                tempx = (p2x - p1x) * maxFraction + p1x;
                tempy = (p2y - p1y) * maxFraction + p1y;
                segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
                segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
                segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
                segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
            }
        } else {
            if (stack.length - stackPtr - 2 <= 0) {
                DynamicTreeNode[] newBuffer = new DynamicTreeNode[stack.length * 2];
                System.arraycopy(stack, 0, newBuffer, 0, stack.length);
                stack = newBuffer;
            }
            stack[stackPtr++] = node.child1;
            stack[stackPtr++] = node.child2;
        }
    }
}