public static Vector<RayCastInfo> getAllIntersecting(
World w, Point pos, Point line, float MINIMUMDIST) {
Vector<RayCastInfo> cols = new Vector<RayCastInfo>();
RayCastInfo k = null;
Ray r = new Ray(pos, line);
for (int i = 0; i < w.numBodies; i++) {
Body b = w.getBody(i);
if (b.shape instanceof Polygon) {
k = rayPolygon(r, (Polygon) b.shape);
} else if (b.shape instanceof Circle) {
k = rayCircle(r, (Circle) b.shape);
}
if (k == null) continue;
if (k.pos == null) continue;
if (k.pos2 != null) { // Sort, pos should be the nearest, pos2 the one further away
float d1 = k.pos.sub(pos).mul(line);
float d2 = k.pos2.sub(pos).mul(line);
if (d1 > d2) { // pos further away than pos2? -> Swap
Point tmp = k.pos;
k.pos = k.pos2;
k.pos2 = tmp;
}
}
float dist = k.pos.squaredDistance(r.pos);
if (dist > MINIMUMDIST * MINIMUMDIST && k.pos.sub(pos).mul(r.dir) > 0) {
k.body = b;
cols.add(k);
}
}
return cols;
}
/**
* Casts a SRay against a SPolygon.
*
* @param ray : The SRay that should be casted against the SPolygon.
* @param polygon : The SPolygon that the ray should be casted against.
* @return ColInfo about
*/
public static RayCastInfo rayPolygon(Ray ray, Polygon polygon) {
RayCastInfo col = new RayCastInfo();
Point pos;
for (int i = 0, j = polygon.numPoints - 1; i < polygon.numPoints; i++, j = i - 1) {
pos = rayLine(ray, polygon.points[i], polygon.points[j]);
if (pos != null) {
if (col.pos != null) {
col.pos2 = pos;
} else {
col.pos = pos;
}
}
}
if (col.pos2 == null) {
} else if (col.pos.squaredDistance(ray.pos) > col.pos2.squaredDistance(ray.pos)) {
Point tmp = col.pos;
col.pos = col.pos2;
col.pos2 = tmp;
}
return col;
}
/**
* Casts a SRay against a SCircle.
*
* @param ray : The SRay that should be casted against the SCircle.
* @param circle : The SCircle that the ray should be casted against.
*/
public static RayCastInfo rayCircle(Ray ray, Circle circle) {
RayCastInfo col = new RayCastInfo();
float af, bf, cf;
af = 1;
bf = 2 * (ray.pos.sub(circle.pos)).mul(ray.dir);
cf = ray.pos.sub(circle.pos).square() - circle.radius * circle.radius;
float disc = bf * bf - 4 * af * cf;
if (disc < 0) {
return null;
}
if (disc == 0) {
return null;
}
float t1, t2;
float sq = Floatmath.sqrt(disc);
float divisor = 1.0f / (2 * af);
t1 = (-bf + sq) * divisor;
t2 = (-bf - sq) * divisor;
// assert(t1 > t2);
col.pos = ray.pos.add(ray.dir.mul(t1));
col.pos2 = ray.pos.add(ray.dir.mul(t2));
// t²+ 2t(P-C)·D + |P-C|² - R² = 0
return col;
}