/** @see Shape#computeSweptAABB(AABB, XForm, XForm) */
@Override
public void computeSweptAABB(final AABB aabb, final XForm transform1, final XForm transform2) {
// djm this method is pretty hot (called every time step)
final Vec2 sweptV1 = tlSwept1.get();
final Vec2 sweptV2 = tlSwept2.get();
final Vec2 sweptV3 = tlSwept3.get();
final Vec2 sweptV4 = tlSwept4.get();
XForm.mulToOut(transform1, m_v1, sweptV1);
XForm.mulToOut(transform1, m_v2, sweptV2);
XForm.mulToOut(transform2, m_v1, sweptV3);
XForm.mulToOut(transform2, m_v2, sweptV4);
// aabb.lowerBound = Vec2.min(Vec2.min(Vec2.min(v1, v2), v3), v4);
// aabb.upperBound = Vec2.max(Vec2.max(Vec2.max(v1, v2), v3), v4);
// djm ok here's the non object-creation-crazy way
Vec2.minToOut(sweptV1, sweptV2, aabb.lowerBound);
Vec2.minToOut(aabb.lowerBound, sweptV3, aabb.lowerBound);
Vec2.minToOut(aabb.lowerBound, sweptV4, aabb.lowerBound);
Vec2.maxToOut(sweptV1, sweptV2, aabb.upperBound);
Vec2.maxToOut(aabb.upperBound, sweptV3, aabb.upperBound);
Vec2.maxToOut(aabb.upperBound, sweptV4, aabb.upperBound);
}
/** @see SupportsGenericDistance#support(Vec2, XForm, Vec2) */
public void support(final Vec2 dest, final XForm xf, final Vec2 d) {
final Vec2 supportV1 = tlSupportV1.get();
final Vec2 supportV2 = tlSupportV2.get();
XForm.mulToOut(xf, m_coreV1, supportV1);
XForm.mulToOut(xf, m_coreV2, supportV2);
dest.set(Vec2.dot(supportV1, d) > Vec2.dot(supportV2, d) ? supportV1 : supportV2);
}
/** @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;
}
/** @see Shape#computeAABB(AABB, XForm) */
@Override
public void computeAABB(final AABB aabb, final XForm transform) {
/*Vec2 v1 = XForm.mul(transform, m_v1);
Vec2 v2 = XForm.mul(transform, m_v2);
aabb.lowerBound = Vec2.min(v1, v2);
aabb.upperBound = Vec2.max(v1, v2);*/
// djm we avoid one creation. crafty huh?
XForm.mulToOut(transform, m_v1, aabb.lowerBound);
final Vec2 v2 = tlV2.get();
XForm.mulToOut(transform, m_v2, v2);
Vec2.maxToOut(aabb.lowerBound, v2, aabb.upperBound);
Vec2.minToOut(aabb.lowerBound, v2, aabb.lowerBound);
}
public float computeSubmergedArea(final Vec2 normal, float offset, XForm xf, Vec2 c) {
final Vec2 v0 = tlV0.get();
final Vec2 v1 = tlV1.get();
final Vec2 v2 = tlV2.get();
final Vec2 temp = tlTemp.get();
// Note that v0 is independent of any details of the specific edge
// We are relying on v0 being consistent between multiple edges of the same body
v0.set(normal).mul(offset);
// b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal;
XForm.mulToOut(xf, m_v1, v1);
XForm.mulToOut(xf, m_v2, v2);
float d1 = Vec2.dot(normal, v1) - offset;
float d2 = Vec2.dot(normal, v2) - offset;
if (d1 > 0.0f) {
if (d2 > 0.0f) {
return 0.0f;
} else {
temp.set(v2).mulLocal(d1 / (d1 - d2));
v1.mulLocal(-d2 / (d1 - d2)).addLocal(temp);
}
} else {
if (d2 > 0.0f) {
temp.set(v1).mulLocal(-d2 / (d1 - d2));
v2.mulLocal(d1 / (d1 - d2)).addLocal(temp);
} else {
// Nothing
}
}
final Vec2 e1 = tlE1.get();
final Vec2 e2 = tlE2.get();
// v0,v1,v2 represents a fully submerged triangle
float k_inv3 = 1.0f / 3.0f;
// Area weighted centroid
c.x = k_inv3 * (v0.x + v1.x + v2.x);
c.y = k_inv3 * (v0.y + v1.y + v2.y);
e1.set(v1).subLocal(v0);
e2.set(v2).subLocal(v0);
return 0.5f * Vec2.cross(e1, e2);
}