class SeparationFunction {
DistanceProxy proxyA;
DistanceProxy proxyB;
int type;
final Vector2 localPoint;
final Vector2 axis;
Sweep sweepA;
Sweep sweepB;
/** Pooling */
final Vector2 localPointA;
final Vector2 localPointB;
final Vector2 pointA;
final Vector2 pointB;
final Vector2 localPointA1;
final Vector2 localPointA2;
final Vector2 normal;
final Vector2 localPointB1;
final Vector2 localPointB2;
final Vector2 axisA;
final Vector2 axisB;
final Vector2 temp;
final Transform xfa;
final Transform xfb;
SeparationFunction() :
proxyA = new DistanceProxy(),
proxyB = new DistanceProxy(),
type = SeparationType.POINTS,
localPoint = new Vector2.zero(),
axis = new Vector2.zero(),
sweepA = new Sweep(),
sweepB = new Sweep(),
localPointA = new Vector2.zero(),
localPointB = new Vector2.zero(),
pointA = new Vector2.zero(),
pointB = new Vector2.zero(),
localPointA1 = new Vector2.zero(),
localPointA2 = new Vector2.zero(),
normal = new Vector2.zero(),
localPointB1 = new Vector2.zero(),
localPointB2 = new Vector2.zero(),
temp = new Vector2.zero(),
xfa = new Transform(),
xfb = new Transform(),
axisA = new Vector2.zero(),
axisB = new Vector2.zero();
num initialize(SimplexCache cache, DistanceProxy argProxyA, Sweep
argSweepA, DistanceProxy argProxyB, Sweep argSweepB, num t1) {
proxyA = argProxyA;
proxyB = argProxyB;
int count = cache.count;
assert (0 < count && count < 3);
sweepA = argSweepA;
sweepB = argSweepB;
sweepA.getTransform(xfa, t1);
sweepB.getTransform(xfb, t1);
if (count == 1) {
type = SeparationType.POINTS;
localPointA.setFrom(proxyA.vertices[cache.indexA[0]]);
localPointB.setFrom(proxyB.vertices[cache.indexB[0]]);
Transform.mulToOut(xfa, localPointA, pointA);
Transform.mulToOut(xfb, localPointB, pointB);
axis.setFrom(pointB).sub(pointA);
num s = axis.normalizeLength();
return s;
} else if (cache.indexA[0] == cache.indexA[1]) {
// Two points on B and one on A.
type = SeparationType.FACE_B;
localPointB1.setFrom(proxyB.vertices[cache.indexB[0]]);
localPointB2.setFrom(proxyB.vertices[cache.indexB[1]]);
temp.setFrom(localPointB2).sub(localPointB1);
Vector2_crossVectorAndNumToOut(temp, 1.0, axis);
axis.normalize();
xfb.rotation.transformed(axis, normal);
localPoint.setFrom(localPointB1);
localPoint.add(localPointB2);
localPoint.scale(.5);
Transform.mulToOut(xfb, localPoint, pointB);
localPointA.setFrom(proxyA.vertices[cache.indexA[0]]);
Transform.mulToOut(xfa, localPointA, pointA);
temp.setFrom(pointA);
temp.sub(pointB);
num s = temp.dot(normal);
if (s < 0.0) {
axis.negate();
s = -s;
}
return s;
} else {
// Two points on A and one or two points on B.
type = SeparationType.FACE_A;
localPointA1.setFrom(proxyA.vertices[cache.indexA[0]]);
localPointA2.setFrom(proxyA.vertices[cache.indexA[1]]);
temp.setFrom(localPointA2);
temp.sub(localPointA1);
Vector2_crossVectorAndNumToOut(temp, 1.0, axis);
axis.normalize();
xfa.rotation.transformed(axis, normal);
localPoint.setFrom(localPointA1);
localPoint.add(localPointA2);
localPoint.scale(.5);
Transform.mulToOut(xfa, localPoint, pointA);
localPointB.setFrom(proxyB.vertices[cache.indexB[0]]);
Transform.mulToOut(xfb, localPointB, pointB);
temp.setFrom(pointB);
temp.sub(pointA);
num s = temp.dot(normal);
if (s < 0.0) {
axis.negate();
s = -s;
}
return s;
}
}
num findMinSeparation(List<int> indexes, num t) {
sweepA.getTransform(xfa, t);
sweepB.getTransform(xfb, t);
switch (type) {
case SeparationType.POINTS:
xfa.rotation.transposed().transformed(axis, axisA);
xfb.rotation.transposed().transformed(axis.negate(),
axisB);
axis.negate();
indexes[0] = proxyA.getSupport(axisA);
indexes[1] = proxyB.getSupport(axisB);
localPointA.setFrom(proxyA.vertices[indexes[0]]);
localPointB.setFrom(proxyB.vertices[indexes[1]]);
Transform.mulToOut(xfa, localPointA, pointA);
Transform.mulToOut(xfb, localPointB, pointB);
num separation = pointB.sub(pointA).dot(axis);
return separation;
case SeparationType.FACE_A:
xfa.rotation.transformed(axis, normal);
Transform.mulToOut(xfa, localPoint, pointA);
normal.negate();
xfb.rotation.transposed().transformed(normal, axisB);
normal.negate();
indexes[0] = -1;
indexes[1] = proxyB.getSupport(axisB);
localPointB.setFrom(proxyB.vertices[indexes[1]]);
Transform.mulToOut(xfb, localPointB, pointB);
num separation = pointB.sub(pointA).dot(normal);
return separation;
case SeparationType.FACE_B:
xfb.rotation.transformed(axis, normal);
Transform.mulToOut(xfb, localPoint, pointB);
xfa.rotation.transposed().transformed(
normal.negate(), axisA);
normal.negate();
indexes[1] = -1;
indexes[0] = proxyA.getSupport(axisA);
localPointA.setFrom(proxyA.vertices[indexes[0]]);
Transform.mulToOut(xfa, localPointA, pointA);
num separation = pointA.sub(pointB).dot(normal);
return separation;
default:
assert (false);
indexes[0] = -1;
indexes[1] = -1;
return 0;
}
}
num evaluate(int indexA, int indexB, num t) {
sweepA.getTransform(xfa, t);
sweepB.getTransform(xfb, t);
switch (type) {
case SeparationType.POINTS:
xfa.rotation.transposed().transformed(axis, axisA);
xfb.rotation.transposed().transformed(axis.negate(),
axisB);
axis.negate();
localPointA.setFrom(proxyA.vertices[indexA]);
localPointB.setFrom(proxyB.vertices[indexB]);
Transform.mulToOut(xfa, localPointA, pointA);
Transform.mulToOut(xfb, localPointB, pointB);
num separation = pointB.sub(pointA).dot(axis);
return separation;
case SeparationType.FACE_A:
xfa.rotation.transformed(axis, normal);
Transform.mulToOut(xfa, localPoint, pointA);
normal.negate();
xfb.rotation.transposed().transformed(normal, axisB);
normal.negate();
localPointB.setFrom(proxyB.vertices[indexB]);
Transform.mulToOut(xfb, localPointB, pointB);
num separation = pointB.sub(pointA).dot(normal);
return separation;
case SeparationType.FACE_B:
xfb.rotation.transformed(axis, normal);
Transform.mulToOut(xfb, localPoint, pointB);
xfa.rotation.transposed().transformed(normal.negate(), axisA);
normal.negate();
localPointA.setFrom(proxyA.vertices[indexA]);
Transform.mulToOut(xfa, localPointA, pointA);
num separation = pointA.sub(pointB).dot(normal);
return separation;
default:
assert (false);
return 0;
}
}
}