Examples with Rot spacegraph.space2d.phys.common.Rot used on opensource projects

Example 1 with Rot

use of spacegraph.space2d.phys.common.Rot 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 Rot

use of spacegraph.space2d.phys.common.Rot 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 3 with Rot

use of spacegraph.space2d.phys.common.Rot 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 4 with Rot

use of spacegraph.space2d.phys.common.Rot 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 5 with Rot

use of spacegraph.space2d.phys.common.Rot in project narchy by automenta.

the class Body2D method synchronizeFixtures.

protected void synchronizeFixtures() {
    final Transform xf1 = pxf;
    // xf1.position = m_sweep.c0 - Mul(xf1.R, m_sweep.localCenter);
    // xf1.q.set(m_sweep.a0);
    // Rot.mulToOutUnsafe(xf1.q, m_sweep.localCenter, xf1.p);
    // xf1.p.mulLocal(-1).addLocal(m_sweep.c0);
    // inlined:
    Rot r = xf1;
    r.s = (float) Math.sin(sweep.a0);
    r.c = (float) Math.cos(sweep.a0);
    xf1.pos.x = sweep.c0.x - r.c * sweep.localCenter.x + r.s * sweep.localCenter.y;
    xf1.pos.y = sweep.c0.y - r.s * sweep.localCenter.x - r.c * sweep.localCenter.y;
    for (Fixture f = fixtures; f != null; f = f.next) {
        f.synchronize(W.contactManager.broadPhase, xf1, this);
    }
}