class SeparationFunction {
DistanceProxy proxyA;
DistanceProxy proxyB;
int type;
final vec2 localPoint;
final vec2 axis;
Sweep sweepA;
Sweep sweepB;
/** Pooling */
final vec2 localPointA;
final vec2 localPointB;
final vec2 pointA;
final vec2 pointB;
final vec2 localPointA1;
final vec2 localPointA2;
final vec2 normal;
final vec2 localPointB1;
final vec2 localPointB2;
final vec2 axisA;
final vec2 axisB;
final vec2 temp;
final Transform xfa;
final Transform xfb;
SeparationFunction() :
proxyA = new DistanceProxy(),
proxyB = new DistanceProxy(),
type = SeparationType.POINTS,
localPoint = new vec2.zero(),
axis = new vec2.zero(),
sweepA = new Sweep(),
sweepB = new Sweep(),
localPointA = new vec2.zero(),
localPointB = new vec2.zero(),
pointA = new vec2.zero(),
pointB = new vec2.zero(),
localPointA1 = new vec2.zero(),
localPointA2 = new vec2.zero(),
normal = new vec2.zero(),
localPointB1 = new vec2.zero(),
localPointB2 = new vec2.zero(),
temp = new vec2.zero(),
xfa = new Transform(),
xfb = new Transform(),
axisA = new vec2.zero(),
axisB = new vec2.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.copyFrom(proxyA.vertices[cache.indexA[0]]);
localPointB.copyFrom(proxyB.vertices[cache.indexB[0]]);
Transform.mulToOut(xfa, localPointA, pointA);
Transform.mulToOut(xfb, localPointB, pointB);
axis.copyFrom(pointB).sub(pointA);
num s = axis.length;
axis.normalize();
return s;
} else if (cache.indexA[0] == cache.indexA[1]) {
// Two points on B and one on A.
type = SeparationType.FACE_B;
localPointB1.copyFrom(proxyB.vertices[cache.indexB[0]]);
localPointB2.copyFrom(proxyB.vertices[cache.indexB[1]]);
temp.copyFrom(localPointB2).sub(localPointB1);
cross(temp, 1, axis);
axis.normalize();
xfb.rotation.transformed(axis, normal);
localPoint.copyFrom(localPointB1).add(localPointB2).scale(0.5);
Transform.mulToOut(xfb, localPoint, pointB);
localPointA.copyFrom(proxyA.vertices[cache.indexA[0]]);
Transform.mulToOut(xfa, localPointA, pointA);
temp.copyFrom(pointA).sub(pointB);
num s = dot(temp, 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.copyFrom(proxyA.vertices[cache.indexA[0]]);
localPointA2.copyFrom(proxyA.vertices[cache.indexA[1]]);
temp.copyFrom(localPointA2).sub(localPointA1);
cross(temp, 1, axis);
axis.normalize();
xfa.rotation.transformed(axis, normal);
localPoint.copyFrom(localPointA1).add(localPointA2).scale(0.5);
Transform.mulToOut(xfa, localPoint, pointA);
localPointB.copyFrom(proxyB.vertices[cache.indexB[0]]);
Transform.mulToOut(xfb, localPointB, pointB);
temp.copyFrom(pointB).sub(pointA);
num s = dot(temp, 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.copyFrom(proxyA.vertices[indexes[0]]);
localPointB.copyFrom(proxyB.vertices[indexes[1]]);
Transform.mulToOut(xfa, localPointA, pointA);
Transform.mulToOut(xfb, localPointB, pointB);
return dot(pointB.sub(pointA), axis);
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.copyFrom(proxyB.vertices[indexes[1]]);
Transform.mulToOut(xfb, localPointB, pointB);
return dot(pointB.sub(pointA), normal);
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.copyFrom(proxyA.vertices[indexes[0]]);
Transform.mulToOut(xfa, localPointA, pointA);
num separation = dot(pointA.sub(pointB), 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.copyFrom(proxyA.vertices[indexA]);
localPointB.copyFrom(proxyB.vertices[indexB]);
Transform.mulToOut(xfa, localPointA, pointA);
Transform.mulToOut(xfb, localPointB, pointB);
return dot(pointB.sub(pointA), axis);
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.copyFrom(proxyB.vertices[indexB]);
Transform.mulToOut(xfb, localPointB, pointB);
num separation = dot(pointB.sub(pointA), 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.copyFrom(proxyA.vertices[indexA]);
Transform.mulToOut(xfa, localPointA, pointA);
num separation = dot(pointA.sub(pointB), normal);
return separation;
default:
assert (false);
return 0;
}
}
}