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

Example 26 with Rot

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

the class EdgeShape method computeAABB.

@Override
public void computeAABB(AABB aabb, Transform xf, int childIndex) {
    final Vec2 lowerBound = aabb.lowerBound;
    final Vec2 upperBound = aabb.upperBound;
    final Rot xfq = xf.q;
    final float v1x = (xfq.c * m_vertex1.x - xfq.s * m_vertex1.y) + xf.p.x;
    final float v1y = (xfq.s * m_vertex1.x + xfq.c * m_vertex1.y) + xf.p.y;
    final float v2x = (xfq.c * m_vertex2.x - xfq.s * m_vertex2.y) + xf.p.x;
    final float v2y = (xfq.s * m_vertex2.x + xfq.c * m_vertex2.y) + xf.p.y;
    lowerBound.x = v1x < v2x ? v1x : v2x;
    lowerBound.y = v1y < v2y ? v1y : v2y;
    upperBound.x = v1x > v2x ? v1x : v2x;
    upperBound.y = v1y > v2y ? v1y : v2y;
    lowerBound.x -= m_radius;
    lowerBound.y -= m_radius;
    upperBound.x += m_radius;
    upperBound.y += m_radius;
}

Example 27 with Rot

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

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 Vec2 v1 = m_vertex1;
    final Vec2 v2 = m_vertex2;
    final Rot xfq = xf.q;
    final Vec2 xfp = xf.p;
    // 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 28 with Rot

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

the class ChainShape method computeAABB.

@Override
public void computeAABB(AABB aabb, Transform xf, int childIndex) {
    assert (childIndex < m_count);
    final Vec2 lower = aabb.lowerBound;
    final Vec2 upper = aabb.upperBound;
    int i1 = childIndex;
    int i2 = childIndex + 1;
    if (i2 == m_count) {
        i2 = 0;
    }
    final Vec2 vi1 = m_vertices[i1];
    final Vec2 vi2 = m_vertices[i2];
    final Rot xfq = xf.q;
    final Vec2 xfp = xf.p;
    float v1x = (xfq.c * vi1.x - xfq.s * vi1.y) + xfp.x;
    float v1y = (xfq.s * vi1.x + xfq.c * vi1.y) + xfp.y;
    float v2x = (xfq.c * vi2.x - xfq.s * vi2.y) + xfp.x;
    float v2y = (xfq.s * vi2.x + xfq.c * vi2.y) + xfp.y;
    lower.x = v1x < v2x ? v1x : v2x;
    lower.y = v1y < v2y ? v1y : v2y;
    upper.x = v1x > v2x ? v1x : v2x;
    upper.y = v1y > v2y ? v1y : v2y;
}

Example 29 with Rot

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

the class CircleShape method computeDistanceToOut.

@Override
public float computeDistanceToOut(Transform xf, Vec2 p, int childIndex, Vec2 normalOut) {
    final Rot xfq = xf.q;
    float centerx = xfq.c * m_p.x - xfq.s * m_p.y + xf.p.x;
    float centery = xfq.s * m_p.x + xfq.c * m_p.y + xf.p.y;
    float dx = p.x - centerx;
    float dy = p.y - centery;
    float d1 = MathUtils.sqrt(dx * dx + dy * dy);
    normalOut.x = dx * 1 / d1;
    normalOut.y = dy * 1 / d1;
    return d1 - m_radius;
}

Example 30 with Rot

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

the class CircleShape method raycast.

// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
@Override
public final boolean raycast(RayCastOutput output, RayCastInput input, Transform transform, int childIndex) {
    final Vec2 inputp1 = input.p1;
    final Vec2 inputp2 = input.p2;
    final Rot tq = transform.q;
    final Vec2 tp = transform.p;
    // Rot.mulToOutUnsafe(transform.q, m_p, position);
    // position.addLocal(transform.p);
    final float positionx = tq.c * m_p.x - tq.s * m_p.y + tp.x;
    final float positiony = tq.s * m_p.x + tq.c * m_p.y + tp.y;
    final float sx = inputp1.x - positionx;
    final float sy = inputp1.y - positiony;
    // final float b = Vec2.dot(s, s) - m_radius * m_radius;
    final float b = sx * sx + sy * sy - m_radius * m_radius;
    // Solve quadratic equation.
    final float rx = inputp2.x - inputp1.x;
    final float ry = inputp2.y - inputp1.y;
    // final float c = Vec2.dot(s, r);
    // final float rr = Vec2.dot(r, r);
    final float c = sx * rx + sy * ry;
    final float rr = rx * rx + ry * ry;
    final float sigma = c * c - rr * b;
    // Check for negative discriminant and short segment.
    if (sigma < 0.0f || rr < Settings.EPSILON) {
        return false;
    }
    // Find the point of intersection of the line with the circle.
    float a = -(c + MathUtils.sqrt(sigma));
    // Is the intersection point on the segment?
    if (0.0f <= a && a <= input.maxFraction * rr) {
        a /= rr;
        output.fraction = a;
        output.normal.x = rx * a + sx;
        output.normal.y = ry * a + sy;
        output.normal.normalize();
        return true;
    }
    return false;
}