private final void done(
RefInt ret,
DContactGeomBuffer contacts,
int skip,
DxGeom o1,
DxGeom o2,
final int maxc,
final double depth,
DVector3C p,
DVector3C n) {
// done:
if (maxc == 4
&& ret.get() == 3) { // If user requested 4 contacts, and the first 3 were created...
// Combine contacts 2 and 3 (vectorial sum) and get the fourth one
// Result: if a box face is completely inside a plane, contacts are created for all the 4
// vertices
// double d4 = CONTACT(contact,1*skip).depth + CONTACT(contact,2*skip).depth - depth; //
// depth is the depth for first contact
double d4 = contacts.get(1 * skip).depth + contacts.get(2 * skip).depth - depth;
if (d4 > 0) {
// CONTACT(contact,3*skip)->pos[0] = CONTACT(contact,1*skip).pos[0] +
// CONTACT(contact,2*skip).pos[0] - p[0]; // p is the position of first contact
// CONTACT(contact,3*skip)->pos[1] = CONTACT(contact,1*skip).pos[1] +
// CONTACT(contact,2*skip).pos[1] - p[1];
// CONTACT(contact,3*skip)->pos[2] = CONTACT(contact,1*skip).pos[2] +
// CONTACT(contact,2*skip).pos[2] - p[2];
// CONTACT(contact,3*skip)->depth = d4;
DContactGeom c3 = contacts.get(3 * skip);
c3.pos.set(contacts.get(1 * skip).pos).add(contacts.get(2 * skip).pos).sub(p);
c3.depth = d4;
ret.inc();
}
}
for (int i = 0; i < ret.get(); i++) {
// CONTACT(contact,i*skip).g1 = o1;
// CONTACT(contact,i*skip).g2 = o2;
DContactGeom contact = contacts.get(i * skip);
contact.g1 = o1;
contact.g2 = o2;
contact.side1 = -1;
contact.side2 = -1;
contact.normal.set(n);
}
}
// int dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
// int flags, dContactGeom *contact, int skip)
int dCollideBoxPlane(DxBox o1, DxPlane o2, int flags, DContactGeomBuffer contacts, int skip) {
// dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT(skip == 1);
// dIASSERT (o1.type == dBoxClass);
// dIASSERT (o2.type == dPlaneClass);
dIASSERT((flags & DxGeom.NUMC_MASK) >= 1);
DxBox box = o1;
DxPlane plane = o2;
DContactGeom contact = contacts.get(0);
contact.g1 = o1;
contact.g2 = o2;
contact.side1 = -1;
contact.side2 = -1;
// int ret = 0;
RefInt ret = new RefInt();
// @@@ problem: using 4-vector (plane.p) as 3-vector (normal).
// final double *R = o1.final_posr.R; // rotation of box
// final double *n = plane.p; // normal vector
final DMatrix3C R = o1.final_posr().R(); // rotation of box
// final double []n = plane._p; // normal vector
DVector3C n = plane.getNormal();
// project sides lengths along normal vector, get absolute values
double Q1 = dCalcVectorDot3_14(n, R, 0);
double Q2 = dCalcVectorDot3_14(n, R, 1);
double Q3 = dCalcVectorDot3_14(n, R, 2);
// double A1 = box.side.v[0] * Q1;
// double A2 = box.side.v[1] * Q2;
// double A3 = box.side.v[2] * Q3;
double[] A = {box.side.get0() * Q1, box.side.get1() * Q2, box.side.get2() * Q3};
// double B1 = dFabs(A1);
// double B2 = dFabs(A2);
// double B3 = dFabs(A3);
double[] B = {dFabs(A[0]), dFabs(A[1]), dFabs(A[2])};
// early exit test
// double depth = plane._p[3] + (0.5)*(B1+B2+B3) - dDOT(n,o1._final_posr.pos);
RefDouble depth =
new RefDouble(
plane.getDepth() + (0.5) * (B[0] + B[1] + B[2]) - n.dot(o1.final_posr().pos()));
if (depth.get() < 0) return 0;
// find number of contacts requested
int maxc = flags & DxGeom.NUMC_MASK;
// if (maxc < 1) maxc = 1; // an assertion is made on entry
if (maxc > 4) maxc = 4; // not more than 4 contacts per box allowed
// find deepest point
DVector3 p = new DVector3();
// p.v[0] = o1._final_posr.pos.v[0];
// p.v[1] = o1._final_posr.pos.v[1];
// p.v[2] = o1._final_posr.pos.v[2];
p.set(o1.final_posr().pos());
// #define FOO1(i,op) \
// p.v[0] op (0.5)*box.side[i] * R[0+i]; \
// p.v[1] op (0.5)*box.side[i] * R[4+i]; \
// p.v[2] op (0.5)*box.side[i] * R[8+i];
// #define BAR1(i,iinc) if (A ## iinc > 0) { FOO1(i,-=) } else { FOO1(i,+=) }
// BAR1(0,1);
// BAR1(1,2);
// BAR1(2,3);
// #undef FOO
// #undef BAR
// substitute for FOO and BAR above (TZ)
for (int i = 0; i < 3; i++) {
if (A[i] > 0) p.eqSum(p, R.columnAsNewVector(i), -0.5 * box.side.get(i));
else p.eqSum(p, R.columnAsNewVector(i), +0.5 * box.side.get(i));
}
// the deepest point is the first contact point
// contact.pos[0] = p[0];
// contact.pos[1] = p[1];
// contact.pos[2] = p[2];
contact.pos.set(p);
contact.depth = depth.get();
ret.set(1); // ret = 1; // ret is number of contact points found so far
// TZ
do {
if (maxc == 1) {
// goto done;
done(ret, contacts, skip, o1, o2, maxc, depth.get(), p, n);
return ret.get();
}
// get the second and third contact points by starting from `p' and going
// along the two sides with the smallest projected length.
// #define FOO2(i,j,op) \
// CONTACT(contact,i*skip).pos[0] = p[0] op box.side[j] * R[0+j]; \
// CONTACT(contact,i*skip).pos[1] = p[1] op box.side[j] * R[4+j]; \
// CONTACT(contact,i*skip).pos[2] = p[2] op box.side[j] * R[8+j];
// contacts.get(i*skip).pos.sum(p, R.columnAsNewVector(j), op*box.side(j));
// #define BAR2(ctact,side,sideinc) \
// depth -= B ## sideinc; \
// if (depth < 0) goto done; \
// if (A ## sideinc > 0) { FOO(ctact,side,+); } else { FOO2(ctact,side,-); } \
// CONTACT(contact,ctact*skip).depth = depth; \
// ret++;
int p1, p2, p3, go;
// if (B[0] < B[1]) {
// if (B[2] < B[0]) goto use_side_3;
// else {
// BAR2(1,0,1); // use side 1
// if (maxc == 2) { //goto done;
// done(ret, contacts, skip, o1, o2);
// return ret;
// }
// if (B[1] < B[2]) goto contact2_2; else goto contact2_3;
// }
// } else {
// if (B[2] < B[1]) {
// use_side_3: // use side 3
// BAR2(1,2,3);
// if (maxc == 2) { //goto done;
// done(ret, contacts, skip, o1, o2);
// return ret;
// }
// if (B[0] < B[1]) goto contact2_1; else goto contact2_2;
// } else {
// BAR2(1,1,2); // use side 2
// if (maxc == 2) { //goto done;
// done(ret, contacts, skip, o1, o2);
// return ret;
// }
// if (B[0] < B[2]) goto contact2_1; else goto contact2_3;
// }
// }
if (B[0] < B[1]) {
if (B[2] < B[0]) {
// TODO
p1 = 1;
p2 = 2;
p3 = 3; // 1 2 3
if (B[0] < B[1]) go = 21;
else go = 22;
} else {
p1 = 1;
p2 = 0;
p3 = 1; // 1 0 1
if (B[1] < B[2]) go = 22;
else go = 23;
}
} else {
if (B[2] < B[1]) {
p1 = 1;
p2 = 2;
p3 = 3; // 1 2 3
if (B[0] < B[1]) go = 21;
else go = 22;
} else {
p1 = 1;
p2 = 1;
p3 = 2; // 1 1 2
if (B[0] < B[2]) go = 21;
else go = 23;
}
}
BAR2(p1, p2, p3, depth, ret, contacts, skip, A, B, o1, o2, p, R, box.side, maxc, n);
// TODO improve (e.g. make depth a primitive int, reduce arguments, ...
// BAR2_(p1, p2, depth, ret, contacts, skip, A[p2], B[p2], o1, o2, p, R, box.side);
if (maxc == 2) { // goto done;
break;
} else {
if (go == 21) {
if (!BAR2(2, 0, 1, depth, ret, contacts, skip, A, B, o1, o2, p, R, box.side, maxc, n))
return ret.get();
break;
} else if (go == 22) {
if (!BAR2(2, 1, 2, depth, ret, contacts, skip, A, B, o1, o2, p, R, box.side, maxc, n))
return ret.get();
break;
} else if (go == 23) {
if (!BAR2(2, 2, 3, depth, ret, contacts, skip, A, B, o1, o2, p, R, box.side, maxc, n))
return ret.get();
break;
} else {
throw new IllegalStateException("go=" + go);
}
}
// TZ
// throw new IllegalStateException();
// contact2_1: BAR2(2,0,1); goto done;
// contact2_2: BAR2(2,1,2); goto done;
// contact2_3: BAR2(2,2,3); goto done;
// #undef FOO
// #undef BAR
} while (false);
// done:
// for (int i=0; i<ret; i++) {
// CONTACT(contact,i*skip).g1 = o1;
// CONTACT(contact,i*skip).g2 = o2;
// }
done(ret, contacts, skip, o1, o2, maxc, depth.get(), p, n);
return ret.get();
}