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

Example 1 with Tuple2f

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

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 static 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 Tuple2f circlep = circle.center;
    final Rot xfBq = xfB;
    final Rot xfAq = xfA;
    final float cx = (xfBq.c * circlep.x - xfBq.s * circlep.y) + xfB.pos.x;
    final float cy = (xfBq.s * circlep.x + xfBq.c * circlep.y) + xfB.pos.y;
    final float px = cx - xfA.pos.x;
    final float py = cy - xfA.pos.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.radius + circle.radius;
    final int vertexCount = polygon.vertices;
    float s;
    final Tuple2f[] vertices = polygon.vertex;
    final Tuple2f[] normals = polygon.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 Tuple2f 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 Tuple2f v1 = vertices[vertIndex1];
    final Tuple2f 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 Tuple2f 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 Tuple2f 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 2 with Tuple2f

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

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.center, temp);
    Transform.mulTransToOutUnsafe(xfA, temp, Q);
    final Tuple2f A = edgeA.m_vertex1;
    final Tuple2f B = edgeA.m_vertex2;
    e.set(B).subbed(A);
    // Barycentric coordinates
    float u = Tuple2f.dot(e, temp.set(B).subbed(Q));
    float v = Tuple2f.dot(e, temp.set(Q).subbed(A));
    float radius = edgeA.radius + circleB.radius;
    // ContactFeature cf;
    cf.indexB = 0;
    cf.typeB = (byte) ContactID.Type.VERTEX.ordinal();
    // Region A
    if (v <= 0.0f) {
        final Tuple2f P = A;
        d.set(Q).subbed(P);
        float dd = Tuple2f.dot(d, d);
        if (dd > radius * radius) {
            return;
        }
        // Is there an edge connected to A?
        if (edgeA.m_hasVertex0) {
            final Tuple2f A1 = edgeA.m_vertex0;
            final Tuple2f B1 = A;
            e1.set(B1).subbed(A1);
            float u1 = Tuple2f.dot(e1, temp.set(B1).subbed(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.center);
        return;
    }
    // Region B
    if (u <= 0.0f) {
        Tuple2f P = B;
        d.set(Q).subbed(P);
        float dd = Tuple2f.dot(d, d);
        if (dd > radius * radius) {
            return;
        }
        // Is there an edge connected to B?
        if (edgeA.m_hasVertex3) {
            final Tuple2f B2 = edgeA.m_vertex3;
            final Tuple2f A2 = B;
            final Tuple2f e2 = e1;
            e2.set(B2).subbed(A2);
            float v2 = Tuple2f.dot(e2, temp.set(Q).subbed(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.center);
        return;
    }
    // Region AB
    float den = Tuple2f.dot(e, e);
    assert (den > 0.0f);
    // Vec2 P = (1.0f / den) * (u * A + v * B);
    P.set(A).scaled(u).added(temp.set(B).scaled(v));
    P.scaled(1.0f / den);
    d.set(Q).subbed(P);
    float dd = Tuple2f.dot(d, d);
    if (dd > radius * radius) {
        return;
    }
    n.x = -e.y;
    n.y = e.x;
    if (Tuple2f.dot(n, temp.set(Q).subbed(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.center);
}

Example 3 with Tuple2f

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

the class Collision method findIncidentEdge.

public static void findIncidentEdge(final ClipVertex[] c, final PolygonShape poly1, final Transform xf1, int edge1, final PolygonShape poly2, final Transform xf2) {
    int count1 = poly1.vertices;
    final Tuple2f[] normals1 = poly1.normals;
    int count2 = poly2.vertices;
    final Tuple2f[] vertices2 = poly2.vertex;
    final Tuple2f[] normals2 = poly2.normals;
    assert (0 <= edge1 && edge1 < count1);
    final ClipVertex c0 = c[0];
    final ClipVertex c1 = c[1];
    final Rot xf1q = xf1;
    final Rot xf2q = xf2;
    // 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 Tuple2f 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) {
        Tuple2f 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]);
    Tuple2f v1 = vertices2[i1];
    Tuple2f out = c0.v;
    out.x = (xf2q.c * v1.x - xf2q.s * v1.y) + xf2.pos.x;
    out.y = (xf2q.s * v1.x + xf2q.c * v1.y) + xf2.pos.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]);
    Tuple2f v2 = vertices2[i2];
    Tuple2f out1 = c1.v;
    out1.x = (xf2q.c * v2.x - xf2q.s * v2.y) + xf2.pos.x;
    out1.y = (xf2q.s * v2.x + xf2q.c * v2.y) + xf2.pos.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 4 with Tuple2f

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

the class Collision method collideCircles.

/**
 * Compute the collision manifold between two circles.
 *
 * @param manifold
 * @param circle1
 * @param xfA
 * @param circle2
 * @param xfB
 */
public static void collideCircles(Manifold manifold, final CircleShape circle1, final Transform xfA, final CircleShape circle2, final Transform xfB) {
    manifold.pointCount = 0;
    // before inline:
    // Transform.mulToOut(xfA, circle1.m_p, pA);
    // Transform.mulToOut(xfB, circle2.m_p, pB);
    // d.set(pB).subLocal(pA);
    // float distSqr = d.x * d.x + d.y * d.y;
    // after inline:
    Tuple2f circle1p = circle1.center;
    Tuple2f circle2p = circle2.center;
    float pAx = (xfA.c * circle1p.x - xfA.s * circle1p.y) + xfA.pos.x;
    float pAy = (xfA.s * circle1p.x + xfA.c * circle1p.y) + xfA.pos.y;
    float pBx = (xfB.c * circle2p.x - xfB.s * circle2p.y) + xfB.pos.x;
    float pBy = (xfB.s * circle2p.x + xfB.c * circle2p.y) + xfB.pos.y;
    float dx = pBx - pAx;
    float dy = pBy - pAy;
    float distSqr = dx * dx + dy * dy;
    // end inline
    final float radius = circle1.radius + circle2.radius;
    if (distSqr > radius * radius) {
        return;
    }
    manifold.type = ManifoldType.CIRCLES;
    manifold.localPoint.set(circle1p);
    manifold.localNormal.setZero();
    manifold.pointCount = 1;
    manifold.points[0].localPoint.set(circle2p);
    manifold.points[0].id.zero();
}

Example 5 with Tuple2f

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

the class WorldManifold method initialize.

public final void initialize(final Manifold manifold, final Transform xfA, float radiusA, final Transform xfB, float radiusB) {
    if (manifold.pointCount == 0) {
        return;
    }
    switch(manifold.type) {
        case CIRCLES:
            {
                final Tuple2f pointA = pool3;
                final Tuple2f pointB = pool4;
                normal.x = 1;
                normal.y = 0;
                Tuple2f v = manifold.localPoint;
                // Transform.mulToOutUnsafe(xfA, manifold.localPoint, pointA);
                // Transform.mulToOutUnsafe(xfB, manifold.points[0].localPoint, pointB);
                pointA.x = (xfA.c * v.x - xfA.s * v.y) + xfA.pos.x;
                pointA.y = (xfA.s * v.x + xfA.c * v.y) + xfA.pos.y;
                Tuple2f mp0p = manifold.points[0].localPoint;
                pointB.x = (xfB.c * mp0p.x - xfB.s * mp0p.y) + xfB.pos.x;
                pointB.y = (xfB.s * mp0p.x + xfB.c * mp0p.y) + xfB.pos.y;
                if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) {
                    normal.x = pointB.x - pointA.x;
                    normal.y = pointB.y - pointA.y;
                    normal.normalize();
                }
                final float cAx = normal.x * radiusA + pointA.x;
                final float cAy = normal.y * radiusA + pointA.y;
                final float cBx = -normal.x * radiusB + pointB.x;
                final float cBy = -normal.y * radiusB + pointB.y;
                points[0].x = (cAx + cBx) * .5f;
                points[0].y = (cAy + cBy) * .5f;
                separations[0] = (cBx - cAx) * normal.x + (cBy - cAy) * normal.y;
            }
            break;
        case FACE_A:
            {
                final Tuple2f planePoint = pool3;
                Rot.mulToOutUnsafe(xfA, manifold.localNormal, normal);
                Transform.mulToOut(xfA, manifold.localPoint, planePoint);
                final Tuple2f clipPoint = pool4;
                for (int i = 0; i < manifold.pointCount; i++) {
                    // b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
                    // b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint,
                    // normal)) * normal;
                    // b2Vec2 cB = clipPoint - radiusB * normal;
                    // points[i] = 0.5f * (cA + cB);
                    Transform.mulToOut(xfB, manifold.points[i].localPoint, clipPoint);
                    // use cA as temporary for now
                    // cA.set(clipPoint).subLocal(planePoint);
                    // float scalar = radiusA - Vec2.dot(cA, normal);
                    // cA.set(normal).mulLocal(scalar).addLocal(clipPoint);
                    // cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal();
                    // points[i].set(cA).addLocal(cB).mulLocal(0.5f);
                    final float scalar = radiusA - ((clipPoint.x - planePoint.x) * normal.x + (clipPoint.y - planePoint.y) * normal.y);
                    final float cAx = normal.x * scalar + clipPoint.x;
                    final float cAy = normal.y * scalar + clipPoint.y;
                    final float cBx = -normal.x * radiusB + clipPoint.x;
                    final float cBy = -normal.y * radiusB + clipPoint.y;
                    points[i].x = (cAx + cBx) * .5f;
                    points[i].y = (cAy + cBy) * .5f;
                    separations[i] = (cBx - cAx) * normal.x + (cBy - cAy) * normal.y;
                }
            }
            break;
        case FACE_B:
            final Tuple2f planePoint = pool3;
            Rot.mulToOutUnsafe(xfB, manifold.localNormal, normal);
            Transform.mulToOut(xfB, manifold.localPoint, planePoint);
            // final Mat22 R = xfB.q;
            // normal.x = R.ex.x * manifold.localNormal.x + R.ey.x * manifold.localNormal.y;
            // normal.y = R.ex.y * manifold.localNormal.x + R.ey.y * manifold.localNormal.y;
            // final Vec2 v = manifold.localPoint;
            // planePoint.x = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y;
            // planePoint.y = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y;
            final Tuple2f clipPoint = pool4;
            for (int i = 0; i < manifold.pointCount; i++) {
                // b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
                // b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint,
                // normal)) * normal;
                // b2Vec2 cA = clipPoint - radiusA * normal;
                // points[i] = 0.5f * (cA + cB);
                Transform.mulToOut(xfA, manifold.points[i].localPoint, clipPoint);
                // cB.set(clipPoint).subLocal(planePoint);
                // float scalar = radiusB - Vec2.dot(cB, normal);
                // cB.set(normal).mulLocal(scalar).addLocal(clipPoint);
                // cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal();
                // points[i].set(cA).addLocal(cB).mulLocal(0.5f);
                // points[i] = 0.5f * (cA + cB);
                // 
                // clipPoint.x = xfA.p.x + xfA.q.ex.x * manifold.points[i].localPoint.x + xfA.q.ey.x *
                // manifold.points[i].localPoint.y;
                // clipPoint.y = xfA.p.y + xfA.q.ex.y * manifold.points[i].localPoint.x + xfA.q.ey.y *
                // manifold.points[i].localPoint.y;
                final float scalar = radiusB - ((clipPoint.x - planePoint.x) * normal.x + (clipPoint.y - planePoint.y) * normal.y);
                final float cBx = normal.x * scalar + clipPoint.x;
                final float cBy = normal.y * scalar + clipPoint.y;
                final float cAx = -normal.x * radiusA + clipPoint.x;
                final float cAy = -normal.y * radiusA + clipPoint.y;
                points[i].x = (cAx + cBx) * .5f;
                points[i].y = (cAy + cBy) * .5f;
                separations[i] = (cAx - cBx) * normal.x + (cAy - cBy) * normal.y;
            }
            // Ensure normal points from A to B.
            normal.x = -normal.x;
            normal.y = -normal.y;
            break;
    }
}