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

Example 21 with Tuple2f

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

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 Tuple2f v1 = m_vertex1;
    final Tuple2f v2 = m_vertex2;
    final Rot xfq = xf;
    final Tuple2f xfp = xf.pos;
    // 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.normalize();
    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 22 with Tuple2f

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

the class PolygonShape method set.

/**
 * Create a convex hull from the given array of points. The count must be in the range [3,
 * Settings.maxPolygonVertices]. This method takes an arraypool for pooling.
 *
 * @param verts
 * @param num
 * @warning the points may be re-ordered, even if they form a convex polygon.
 * @warning collinear points are removed.
 */
public final PolygonShape set(final Tuple2f[] verts, final int num) {
    assert (3 <= num && num <= Settings.maxPolygonVertices);
    // Create the convex hull using the Gift wrapping algorithm
    // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
    // Find the right most point on the hull
    int i0 = 0;
    float x0 = verts[0].x;
    for (int i = 1; i < num; ++i) {
        float x = verts[i].x;
        if (x > x0 || (x == x0 && verts[i].y < verts[i0].y)) {
            i0 = i;
            x0 = x;
        }
    }
    int[] hull = new int[Settings.maxPolygonVertices];
    int m = 0;
    int ih = i0;
    while (true) {
        hull[m] = ih;
        int ie = 0;
        for (int j = 1; j < num; ++j) {
            if (ie == ih) {
                ie = j;
                continue;
            }
            Tuple2f r = pool1.set(verts[ie]).subbed(verts[hull[m]]);
            Tuple2f v = pool2.set(verts[j]).subbed(verts[hull[m]]);
            float c = Tuple2f.cross(r, v);
            if (c < 0.0f) {
                ie = j;
            }
            // Collinearity check
            if (c == 0.0f && v.lengthSquared() > r.lengthSquared()) {
                ie = j;
            }
        }
        ++m;
        ih = ie;
        if (ie == i0) {
            break;
        }
    }
    this.vertices = m;
    // Copy vertices.
    for (int i = 0; i < vertices; ++i) {
        if (vertex[i] == null) {
            vertex[i] = new Vec2();
        }
        vertex[i].set(verts[hull[i]]);
    }
    Tuple2f edge = pool1;
    for (int i = 0; i < vertices; ++i) {
        final int i1 = i;
        final int i2 = i + 1 < vertices ? i + 1 : 0;
        edge.set(vertex[i2]).subbed(vertex[i1]);
        assert (edge.lengthSquared() > Settings.EPSILON * Settings.EPSILON);
        Tuple2f.crossToOutUnsafe(edge, 1f, normals[i]);
        normals[i].normalize();
    }
    // Compute the polygon centroid.
    computeCentroidToOut(vertex, vertices, centroid);
    return this;
}

Example 23 with Tuple2f

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

the class PolygonShape method testPoint.

@Override
public final boolean testPoint(final Transform xf, final Tuple2f p) {
    float tempx, tempy;
    final Rot xfq = xf;
    tempx = p.x - xf.pos.x;
    tempy = p.y - xf.pos.y;
    final float pLocalx = xfq.c * tempx + xfq.s * tempy;
    final float pLocaly = -xfq.s * tempx + xfq.c * tempy;
    if (m_debug) {
        System.out.println("--testPoint debug--");
        System.out.println("Vertices: ");
        for (int i = 0; i < vertices; ++i) {
            System.out.println(vertex[i]);
        }
        System.out.println("pLocal: " + pLocalx + ", " + pLocaly);
    }
    for (int i = 0; i < vertices; ++i) {
        Tuple2f vertex = this.vertex[i];
        Tuple2f normal = 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 24 with Tuple2f

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

the class PolygonShape method computeMass.

public void computeMass(final MassData massData, float density) {
    assert (vertices >= 3);
    final Tuple2f center = pool1;
    center.setZero();
    float area = 0.0f;
    float I = 0.0f;
    // pRef is the reference point for forming triangles.
    // It's location doesn't change the result (except for rounding error).
    final Vec2 s = pool2;
    s.setZero();
    // This code would put the reference point inside the polygon.
    for (int i = 0; i < vertices; ++i) {
        s.addLocal(vertex[i]);
    }
    s.scaled(1.0f / vertices);
    final float k_inv3 = 1.0f / 3.0f;
    final Tuple2f e1 = pool3;
    final Tuple2f e2 = pool4;
    for (int i = 0; i < vertices; ++i) {
        // Triangle vertices.
        e1.set(vertex[i]).subbed(s);
        e2.set(s).negated().added(i + 1 < vertices ? vertex[i + 1] : vertex[0]);
        final float D = Tuple2f.cross(e1, e2);
        final float triangleArea = 0.5f * D;
        area += triangleArea;
        // Area weighted centroid
        center.x += triangleArea * k_inv3 * (e1.x + e2.x);
        center.y += triangleArea * k_inv3 * (e1.y + e2.y);
        final float ex1 = e1.x, ey1 = e1.y;
        final float ex2 = e2.x, ey2 = e2.y;
        float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
        float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
        I += (0.25f * k_inv3 * D) * (intx2 + inty2);
    }
    // Total mass
    massData.mass = density * area;
    // Center of mass
    assert (area > Settings.EPSILON);
    center.scaled(1.0f / area);
    massData.center.set(center).added(s);
    // Inertia tensor relative to the local origin (point s)
    massData.I = I * density;
    // Shift to center of mass then to original body origin.
    massData.I += massData.mass * (Tuple2f.dot(massData.center, massData.center));
}

Example 25 with Tuple2f

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

the class PolygonShape method computeDistanceToOut.

@Override
public float computeDistanceToOut(Transform xf, Tuple2f p, int childIndex, v2 normalOut) {
    float xfqc = xf.c;
    float xfqs = xf.s;
    float tx = p.x - xf.pos.x;
    float ty = p.y - xf.pos.y;
    float pLocalx = xfqc * tx + xfqs * ty;
    float pLocaly = -xfqs * tx + xfqc * ty;
    float maxDistance = -Float.MAX_VALUE;
    float normalForMaxDistanceX = pLocalx;
    float normalForMaxDistanceY = pLocaly;
    for (int i = 0; i < vertices; ++i) {
        Tuple2f vertex = this.vertex[i];
        Tuple2f normal = normals[i];
        tx = pLocalx - vertex.x;
        ty = pLocaly - vertex.y;
        float dot = normal.x * tx + normal.y * ty;
        if (dot > maxDistance) {
            maxDistance = dot;
            normalForMaxDistanceX = normal.x;
            normalForMaxDistanceY = normal.y;
        }
    }
    float distance;
    if (maxDistance > 0) {
        float minDistanceX = normalForMaxDistanceX;
        float minDistanceY = normalForMaxDistanceY;
        float minDistance2 = maxDistance * maxDistance;
        for (int i = 0; i < vertices; ++i) {
            Tuple2f vertex = this.vertex[i];
            float distanceVecX = pLocalx - vertex.x;
            float distanceVecY = pLocaly - vertex.y;
            float distance2 = (distanceVecX * distanceVecX + distanceVecY * distanceVecY);
            if (minDistance2 > distance2) {
                minDistanceX = distanceVecX;
                minDistanceY = distanceVecY;
                minDistance2 = distance2;
            }
        }
        distance = (float) Math.sqrt(minDistance2);
        normalOut.x = xfqc * minDistanceX - xfqs * minDistanceY;
        normalOut.y = xfqs * minDistanceX + xfqc * minDistanceY;
        normalOut.normalize();
    } else {
        distance = maxDistance;
        normalOut.x = xfqc * normalForMaxDistanceX - xfqs * normalForMaxDistanceY;
        normalOut.y = xfqs * normalForMaxDistanceX + xfqc * normalForMaxDistanceY;
    }
    return distance;
}