/** @see Shape#testSegment(XForm, RaycastResult, Segment, float) */
@Override
public SegmentCollide testSegment(
final XForm xf, final RaycastResult out, final Segment segment, final float maxLambda) {
final Vec2 r = tlR.get();
final Vec2 v1 = tlV1.get();
final Vec2 d = tlD.get();
final Vec2 n = tlN.get();
final Vec2 b = tlB.get();
r.set(segment.p2).subLocal(segment.p1);
XForm.mulToOut(xf, m_v1, v1);
XForm.mulToOut(xf, m_v2, d);
d.subLocal(v1);
Vec2.crossToOut(d, 1.0f, n);
final float k_slop = 100.0f * Settings.EPSILON;
final float denom = -Vec2.dot(r, n);
// Cull back facing collision and ignore parallel segments.
if (denom > k_slop) {
// Does the segment intersect the infinite line associated with this segment?
b.set(segment.p1).subLocal(v1);
float a = Vec2.dot(b, n);
if (0.0f <= a && a <= maxLambda * denom) {
final float mu2 = -r.x * b.y + r.y * b.x;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop)) {
a /= denom;
n.normalize();
out.lambda = a;
out.normal.set(n);
return SegmentCollide.HIT_COLLIDE;
}
}
}
return SegmentCollide.MISS_COLLIDE;
}
/**
* Don't use this. Instead create using {@link Body#createShape(ShapeDef)} with an {@link
* EdgeChainDef}, not the constructor here.
*
* @see Body#createShape(ShapeDef)
* @see EdgeChainDef
* @param v1
* @param v2
* @param def
*/
public EdgeShape(final Vec2 v1, final Vec2 v2, final ShapeDef def) {
super(def);
assert (def.type == ShapeType.EDGE_SHAPE);
m_type = ShapeType.EDGE_SHAPE;
m_prevEdge = null;
m_nextEdge = null;
m_v1 = v1;
m_v2 = v2;
m_direction = m_v2.sub(m_v1);
m_length = m_direction.normalize();
m_normal = new Vec2(m_direction.y, -m_direction.x);
// djm they are new objects after that first math call
m_coreV1 = (m_normal.sub(m_direction)).mulLocal(-Settings.toiSlop).addLocal(m_v1);
m_coreV2 = (m_normal.add(m_direction)).mulLocal(-Settings.toiSlop).addLocal(m_v2);
m_cornerDir1 = m_normal.clone();
m_cornerDir2 = m_normal.mul(-1.0f);
}
/**
* Compute the collision manifold between two polygons.
*
* @param manifold
* @param polygon1
* @param xf1
* @param polygon2
* @param xf2
*/
public final void collidePolygons(
Manifold manifold,
final PolygonShape polyA,
final Transform xfA,
final PolygonShape polyB,
final Transform xfB) {
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
manifold.pointCount = 0;
float totalRadius = polyA.m_radius + polyB.m_radius;
findMaxSeparation(results1, polyA, xfA, polyB, xfB);
if (results1.separation > totalRadius) {
return;
}
findMaxSeparation(results2, polyB, xfB, polyA, xfA);
if (results2.separation > totalRadius) {
return;
}
final PolygonShape poly1; // reference polygon
final PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
int flip;
final float k_relativeTol = 0.98f;
final float k_absoluteTol = 0.001f;
if (results2.separation > k_relativeTol * results1.separation + k_absoluteTol) {
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = results2.edgeIndex;
manifold.type = ManifoldType.FACE_B;
flip = 1;
} else {
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = results1.edgeIndex;
manifold.type = ManifoldType.FACE_A;
flip = 0;
}
findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_vertexCount;
final Vec2[] vertices1 = poly1.m_vertices;
v11.set(vertices1[edge1]);
v12.set(edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]);
localTangent.set(v12).subLocal(v11);
localTangent.normalize();
Vec2.crossToOut(localTangent, 1f, localNormal); // Vec2 localNormal = Cross(dv,
// 1.0f);
planePoint.set(v11).addLocal(v12).mulLocal(.5f); // Vec2 planePoint = 0.5f * (v11
// + v12);
Mat22.mulToOut(xf1.R, localTangent, tangent); // Vec2 sideNormal = Mul(xf1.R, v12
// - v11);
Vec2.crossToOut(tangent, 1f, normal); // Vec2 frontNormal = Cross(sideNormal,
// 1.0f);
Transform.mulToOut(xf1, v11, v11);
Transform.mulToOut(xf1, v12, v12);
// v11 = Mul(xf1, v11);
// v12 = Mul(xf1, v12);
// Face offset
float frontOffset = Vec2.dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius;
float sideOffset2 = Vec2.dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
// ClipVertex clipPoints1[2];
// ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
// np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
tangent.negateLocal();
np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1);
tangent.negateLocal();
if (np < 2) {
return;
}
// Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2);
if (np < 2) {
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal.set(localNormal);
manifold.localPoint.set(planePoint);
int pointCount = 0;
for (int i = 0; i < Settings.maxManifoldPoints; ++i) {
float separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius) {
ManifoldPoint cp = manifold.points[pointCount];
Transform.mulTransToOut(xf2, clipPoints2[i].v, cp.localPoint);
// cp.m_localPoint = MulT(xf2, clipPoints2[i].v);
cp.id.set(clipPoints2[i].id);
cp.id.features.flip = flip;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
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 Vec2 pointA = pool3;
// final Vec2 pointB = pool4;
//
// normal.set(1, 0);
// Transform.mulToOut(xfA, manifold.localPoint, pointA);
// Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB);
//
// if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON)
// {
// normal.set(pointB).subLocal(pointA);
// normal.normalize();
// }
//
// cA.set(normal).mulLocal(radiusA).addLocal(pointA);
// cB.set(normal).mulLocal(radiusB).subLocal(pointB).negateLocal();
// points[0].set(cA).addLocal(cB).mulLocal(0.5f);
final Vec2 pointA = pool3;
final Vec2 pointB = pool4;
normal.x = 1;
normal.y = 0;
pointA.x =
xfA.position.x
+ xfA.R.col1.x * manifold.localPoint.x
+ xfA.R.col2.x * manifold.localPoint.y;
pointA.y =
xfA.position.y
+ xfA.R.col1.y * manifold.localPoint.x
+ xfA.R.col2.y * manifold.localPoint.y;
pointB.x =
xfB.position.x
+ xfB.R.col1.x * manifold.points[0].localPoint.x
+ xfB.R.col2.x * manifold.points[0].localPoint.y;
pointB.y =
xfB.position.y
+ xfB.R.col1.y * manifold.points[0].localPoint.x
+ xfB.R.col2.y * manifold.points[0].localPoint.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;
}
break;
case FACE_A:
{
// final Vec2 planePoint = pool3;
//
// Mat22.mulToOut(xfA.R, manifold.localNormal, normal);
// Transform.mulToOut(xfA, manifold.localPoint, planePoint);
//
// final Vec2 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 Vec2 planePoint = pool3;
normal.x = xfA.R.col1.x * manifold.localNormal.x + xfA.R.col2.x * manifold.localNormal.y;
normal.y = xfA.R.col1.y * manifold.localNormal.x + xfA.R.col2.y * manifold.localNormal.y;
planePoint.x =
xfA.position.x
+ xfA.R.col1.x * manifold.localPoint.x
+ xfA.R.col2.x * manifold.localPoint.y;
planePoint.y =
xfA.position.y
+ xfA.R.col1.y * manifold.localPoint.x
+ xfA.R.col2.y * manifold.localPoint.y;
final Vec2 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);
clipPoint.x =
xfB.position.x
+ xfB.R.col1.x * manifold.points[i].localPoint.x
+ xfB.R.col2.x * manifold.points[i].localPoint.y;
clipPoint.y =
xfB.position.y
+ xfB.R.col1.y * manifold.points[i].localPoint.x
+ xfB.R.col2.y * manifold.points[i].localPoint.y;
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;
}
}
break;
case FACE_B:
final Vec2 planePoint = pool3;
final Mat22 R = xfB.R;
normal.x = R.col1.x * manifold.localNormal.x + R.col2.x * manifold.localNormal.y;
normal.y = R.col1.y * manifold.localNormal.x + R.col2.y * manifold.localNormal.y;
final Vec2 v = manifold.localPoint;
planePoint.x = xfB.position.x + xfB.R.col1.x * v.x + xfB.R.col2.x * v.y;
planePoint.y = xfB.position.y + xfB.R.col1.y * v.x + xfB.R.col2.y * v.y;
final Vec2 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.position.x
+ xfA.R.col1.x * manifold.points[i].localPoint.x
+ xfA.R.col2.x * manifold.points[i].localPoint.y;
clipPoint.y =
xfA.position.y
+ xfA.R.col1.y * manifold.points[i].localPoint.x
+ xfA.R.col2.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;
}
// Ensure normal points from A to B.
normal.x = -normal.x;
normal.y = -normal.y;
break;
}
}
@Override
public void raycast(
org.jbox2d.callbacks.TreeRayCastCallback callback, org.jbox2d.collision.RayCastInput input) {
final Vec2 p1 = input.p1;
final Vec2 p2 = input.p2;
float p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
float vx, vy;
float rx, ry;
float absVx, absVy;
float cx, cy;
float hx, hy;
float tempx, tempy;
r.x = p2x - p1x;
r.y = p2y - p1y;
assert ((r.x * r.x + r.y * r.y) > 0f);
r.normalize();
rx = r.x;
ry = r.y;
// v is perpendicular to the segment.
vx = -1f * ry;
vy = 1f * rx;
absVx = org.jbox2d.common.MathUtils.abs(vx);
absVy = org.jbox2d.common.MathUtils.abs(vy);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.maxFraction;
// Build a bounding box for the segment.
final org.jbox2d.collision.AABB segAABB = aabb;
// Vec2 t = p1 + maxFraction * (p2 - p1);
// before inline
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x) * maxFraction + p1x;
tempy = (p2y - p1y) * maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
// end inline
nodeStackIndex = 0;
nodeStack[nodeStackIndex++] = m_root;
while (nodeStackIndex > 0) {
final DynamicTreeNode node = nodeStack[--nodeStackIndex];
if (node == null) {
continue;
}
final org.jbox2d.collision.AABB nodeAABB = node.aabb;
if (!org.jbox2d.collision.AABB.testOverlap(nodeAABB, segAABB)) {
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
// node.aabb.getCenterToOut(c);
// node.aabb.getExtentsToOut(h);
cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x) * .5f;
cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5f;
hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5f;
hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5f;
tempx = p1x - cx;
tempy = p1y - cy;
float separation =
org.jbox2d.common.MathUtils.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy);
if (separation > 0.0f) {
continue;
}
if (node.child1 == null) {
subInput.p1.x = p1x;
subInput.p1.y = p1y;
subInput.p2.x = p2x;
subInput.p2.y = p2y;
subInput.maxFraction = maxFraction;
float value = callback.raycastCallback(subInput, node.id);
if (value == 0.0f) {
// The client has terminated the ray cast.
return;
}
if (value > 0.0f) {
// Update segment bounding box.
maxFraction = value;
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x) * maxFraction + p1x;
tempy = (p2y - p1y) * maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
}
} else {
if (nodeStack.length - nodeStackIndex - 2 <= 0) {
DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.length * 2];
System.arraycopy(nodeStack, 0, newBuffer, 0, nodeStack.length);
nodeStack = newBuffer;
}
nodeStack[nodeStackIndex++] = node.child1;
nodeStack[nodeStackIndex++] = node.child2;
}
}
}
