private static int DBoxBox2(ref IndexedVector3 p1, float[] R1,
ref IndexedVector3 side1, ref IndexedVector3 p2,
float[] R2, ref IndexedVector3 side2,
ref IndexedVector3 normal, ref float depth, ref int return_code,
int maxc, Object contact, int skip, IDiscreteCollisionDetectorInterfaceResult output)
{
//IndexedVector3 centerDifference = IndexedVector3.Zero, ppv = IndexedVector3.Zero;
float[] normalR = null;
int normalROffsetResult = 0;
IndexedVector3 A = side1 * 0.5f;
IndexedVector3 B = side2 * 0.5f;
int code;
bool invert_normal;
// get vector from centers of box 1 to box 2, relative to box 1
IndexedVector3 p = p2 - p1;
IndexedVector3 pp = new IndexedVector3();
DMULTIPLY1_331(ref pp, R1, ref p); // get pp = p relative to body 1
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
float R11 = DDOT44(R1, 0, R2, 0);
float R12 = DDOT44(R1, 0, R2, 1);
float R13 = DDOT44(R1, 0, R2, 2);
float R21 = DDOT44(R1, 1, R2, 0);
float R22 = DDOT44(R1, 1, R2, 1);
float R23 = DDOT44(R1, 1, R2, 2);
float R31 = DDOT44(R1, 2, R2, 0);
float R32 = DDOT44(R1, 2, R2, 1);
float R33 = DDOT44(R1, 2, R2, 2);
float Q11 = Math.Abs(R11);
float Q12 = Math.Abs(R12);
float Q13 = Math.Abs(R13);
float Q21 = Math.Abs(R21);
float Q22 = Math.Abs(R22);
float Q23 = Math.Abs(R23);
float Q31 = Math.Abs(R31);
float Q32 = Math.Abs(R32);
float Q33 = Math.Abs(R33);
float s = -float.MaxValue;
invert_normal = false;
code = 0;
int normalROffset = 0;
// separating axis = u1,u2,u3
if (TST(pp.X, (A.X + B.X * Q11 + B.Y * Q12 + B.Z * Q13), R1, ref normalR, 0, ref normalROffset, 1, ref code, ref s, ref invert_normal)) return 0;
if (TST(pp.Y, (A.Y + B.X * Q21 + B.Y * Q22 + B.Z * Q23), R1, ref normalR, 1, ref normalROffset, 2, ref code, ref s, ref invert_normal)) return 0;
if (TST(pp.Z, (A.Z + B.X * Q31 + B.Y * Q32 + B.Z * Q33), R1, ref normalR, 2, ref normalROffset, 3, ref code, ref s, ref invert_normal)) return 0;
// separating axis = v1,v2,v3
if (TST(DDOT41(R2, 0, ref p, 0), (A.X * Q11 + A.Y * Q21 + A.Z * Q31 + B.X), R2, ref normalR, 0, ref normalROffset, 4, ref code, ref s, ref invert_normal)) return 0;
if (TST(DDOT41(R2, 1, ref p, 0), (A.X * Q12 + A.Y * Q22 + A.Z * Q32 + B.Y), R2, ref normalR, 1, ref normalROffset, 5, ref code, ref s, ref invert_normal)) return 0;
if (TST(DDOT41(R2, 2, ref p, 0), (A.X * Q13 + A.Y * Q23 + A.Z * Q33 + B.Z), R2, ref normalR, 2, ref normalROffset, 6, ref code, ref s, ref invert_normal)) return 0;
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
// separating axis = u1 x (v1,v2,v3)
//private static bool TST2(float expr1,float expr2,ref IndexedVector3 normal, ref IndexedVector3 normalC,int cc,ref int code)
IndexedVector3 normalC = new IndexedVector3();
float fudge2 = 1.0e-5f;
Q11 += fudge2;
Q12 += fudge2;
Q13 += fudge2;
Q21 += fudge2;
Q22 += fudge2;
Q23 += fudge2;
Q31 += fudge2;
Q32 += fudge2;
Q33 += fudge2;
// separating axis = u1 x (v1,v2,v3)
if (TST2(pp.Z * R21 - pp.Y * R31, (A.Y * Q31 + A.Z * Q21 + B.Y * Q13 + B.Z * Q12), 0, -R31, R21, ref normalC, ref normalR, 7, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp.Z * R22 - pp.Y * R32, (A.Y * Q32 + A.Z * Q22 + B.X * Q13 + B.Z * Q11), 0, -R32, R22, ref normalC, ref normalR, 8, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp.Z * R23 - pp.Y * R33, (A.Y * Q33 + A.Z * Q23 + B.X * Q12 + B.Y * Q11), 0, -R33, R23, ref normalC, ref normalR, 9, ref code, ref s, ref invert_normal)) return 0;
// separating axis = u2 x (v1,v2,v3)
if (TST2(pp.X * R31 - pp.Z * R11, (A.X * Q31 + A.Z * Q11 + B.Y * Q23 + B.Z * Q22), R31, 0, -R11, ref normalC, ref normalR, 10, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp.X * R32 - pp.Z * R12, (A.X * Q32 + A.Z * Q12 + B.X * Q23 + B.Z * Q21), R32, 0, -R12, ref normalC, ref normalR, 11, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp.X * R33 - pp.Z * R13, (A.X * Q33 + A.Z * Q13 + B.X * Q22 + B.Y * Q21), R33, 0, -R13, ref normalC, ref normalR, 12, ref code, ref s, ref invert_normal)) return 0;
// separating axis = u3 x (v1,v2,v3)
if (TST2(pp.Y * R11 - pp.X * R21, (A.X * Q21 + A.Y * Q11 + B.Y * Q33 + B.Z * Q32), -R21, R11, 0, ref normalC, ref normalR, 13, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp.Y * R12 - pp.X * R22, (A.X * Q22 + A.Y * Q12 + B.X * Q33 + B.Z * Q31), -R22, R12, 0, ref normalC, ref normalR, 14, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp.Y * R13 - pp.X * R23, (A.X * Q23 + A.Y * Q13 + B.X * Q32 + B.Y * Q31), -R23, R13, 0, ref normalC, ref normalR, 15, ref code, ref s, ref invert_normal)) return 0;
if (code == 0)
{
return 0;
}
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR != null)
{
normal.X = normalR[0 + normalROffset];
normal.Y = normalR[4 + normalROffset];
normal.Z = normalR[8 + normalROffset];
}
else
{
DMULTIPLY0_331(ref normal, R1, ref normalC);
}
if (invert_normal)
{
normal = -normal;
}
depth = -s;
// compute contact point(s)
if (code > 6)
{
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
IndexedVector3 pa1 = p1;
for (int j = 0; j < 3; j++)
{
float sign = (DDOT14(ref normal, 0, R1, j) > 0) ? 1.0f : -1.0f;
for (int i = 0; i < 3; i++)
{
pa1[i] += sign * A[j] * R1[i * 4 + j];
}
}
// find a point pb on the intersecting edge of box 2
IndexedVector3 pb1 = p2;
//for (i = 0; i < 3; i++) pb[i] = p2[i];
for (int j = 0; j < 3; j++)
{
float sign = (DDOT14(ref normal, 0, R2, j) > 0) ? -1.0f : 1.0f;
for (int i = 0; i < 3; i++)
{
pb1[i] += sign * B[j] * R2[i * 4 + j];
}
}
float alpha, beta;
IndexedVector3 ua = new IndexedVector3();
IndexedVector3 ub = new IndexedVector3();
for (int i = 0; i < 3; i++)
{
ua[i] = R1[((code) - 7) / 3 + i * 4];
}
for (int i = 0; i < 3; i++)
{
ub[i] = R2[((code) - 7) % 3 + i * 4];
}
DLineClosestApproach(ref pa1, ref ua, ref pb1, ref ub, out alpha, out beta);
for (int i = 0; i < 3; i++)
{
pa1[i] += ua[i] * alpha;
}
for (int i = 0; i < 3; i++)
{
pb1[i] += ub[i] * beta;
}
{
//contact[0].pos[i] = float(0.5)*(pa[i]+pb[i]);
//contact[0].depth = *depth;
#if USE_CENTER_POINT
pointInWorld = (pa + pb) * 0.5f;
output.addContactPoint(-normal,pointInWorld,-depth);
#else
output.AddContactPoint(-normal, pb1, -depth);
#endif //
return_code = code;
}
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
float[] Ra;
float[] Rb;
IndexedVector3 pa;
IndexedVector3 pb;
IndexedVector3 Sa;
IndexedVector3 Sb;
if (code <= 3)
{
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else
{
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
IndexedVector3 normal2;
IndexedVector3 nr = new IndexedVector3();
IndexedVector3 anr;
if (code <= 3)
{
normal2 = normal;
}
else
{
normal2 = -normal;
}
DMULTIPLY1_331(ref nr, Rb, ref normal2);
nr.Abs(out anr);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr, a1, a2;
if (anr.Y > anr.X)
{
if (anr.Y > anr.Z)
{
a1 = 0;
lanr = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else
{
if (anr.X > anr.Z)
{
lanr = 0;
a1 = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
IndexedVector3 center;
if (nr[lanr] < 0)
{
center = new IndexedVector3(
pb.X - pa.X + Sb[lanr] * Rb[lanr],
pb.Y - pa.Y + Sb[lanr] * Rb[lanr + 4],
pb.Z - pa.Z + Sb[lanr] * Rb[lanr + 8]
);
}
else
{
center = new IndexedVector3(
pb.X - pa.X - Sb[lanr] * Rb[lanr],
pb.Y - pa.Y - Sb[lanr] * Rb[lanr + 4],
pb.Z - pa.Z - Sb[lanr] * Rb[lanr + 8]
);
}
// find the normal and non-normal axis numbers of the reference box
int codeN, code1, code2;
if (code <= 3)
{
codeN = code - 1;
}
else
{
codeN = code - 4;
}
if (codeN == 0)
{
code1 = 1;
code2 = 2;
}
else if (codeN == 1)
{
code1 = 0;
code2 = 2;
}
else
{
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
float[] quad = s_quad; // 2D coordinate of incident face (x,y pairs)
float c1, c2, m11, m12, m21, m22;
c1 = DDOT14(ref center, 0, Ra, code1);
c2 = DDOT14(ref center, 0, Ra, code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = DDOT44(Ra, code1, Rb, a1);
m12 = DDOT44(Ra, code1, Rb, a2);
m21 = DDOT44(Ra, code2, Rb, a1);
m22 = DDOT44(Ra, code2, Rb, a2);
{
float k1 = m11 * Sb[a1];
float k2 = m21 * Sb[a1];
float k3 = m12 * Sb[a2];
float k4 = m22 * Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
//float[] s_rectReferenceFace = new float[2];
s_rectReferenceFace[0] = Sa[code1];
s_rectReferenceFace[1] = Sa[code2];
// intersect the incident and reference faces
float[] ret = s_ret;
int n = IntersectRectQuad2(s_rectReferenceFace, quad, ret);
if (n < 1)
{
return 0; // this should never happen
}
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
float[] point = s_point; // penetrating contact points
float[] dep = s_dep; // depths for those points
float det1 = 1f / (m11 * m22 - m12 * m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (int j = 0; j < n; j++)
{
float k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
float k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
for (int i = 0; i < 3; i++)
{
point[cnum * 3 + i] = center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
}
dep[cnum] = Sa[codeN] - DDOT(ref normal2, 0, point, cnum * 3);
if (dep[cnum] >= 0)
{
ret[cnum * 2] = ret[j * 2];
ret[cnum * 2 + 1] = ret[j * 2 + 1];
cnum++;
}
}
if (cnum < 1)
{
return 0; // this should never happen
}
// we can't generate more contacts than we actually have
if (maxc > cnum)
{
maxc = cnum;
}
if (maxc < 1)
{
maxc = 1;
}
if (cnum <= maxc)
{
if (code < 4)
{
// we have less contacts than we need, so we use them all
for (int j = 0; j < cnum; j++)
{
IndexedVector3 pointInWorldFA = new IndexedVector3(); ;
for (int i = 0; i < 3; i++)
{
pointInWorldFA[i] = point[j * 3 + i] + pa[i];
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorldFA);
}
#endif
output.AddContactPoint(-normal, pointInWorldFA, -dep[j]);
}
}
else
{
// we have less contacts than we need, so we use them all
for (int j = 0; j < cnum; j++)
{
IndexedVector3 pointInWorld = new IndexedVector3(); ;
for (int i = 0; i < 3; i++)
{
pointInWorld[i] = point[j * 3 + i] + pa[i];
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
}
#endif
output.AddContactPoint(-normal, pointInWorld, -dep[j]);
}
}
}
else
{
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
float maxdepth = dep[0];
for (int i = 1; i < cnum; i++)
{
if (dep[i] > maxdepth)
{
maxdepth = dep[i];
i1 = i;
}
}
int[] iret = new int[8];
CullPoints2(cnum, ret, maxc, i1, iret);
for (int j = 0; j < maxc; j++)
{
// dContactGeom *con = CONTACT(contact,skip*j);
// for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
// con->depth = dep[iret[j]];
IndexedVector3 posInWorldFA = new IndexedVector3(); ;
for (int i = 0; i < 3; i++)
{
posInWorldFA[i] = point[iret[j] * 3 + i] + pa[i];
}
IndexedVector3 pointInWorld = posInWorldFA;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
}
#endif
output.AddContactPoint((-normal), pointInWorld, -dep[iret[j]]);
}
cnum = maxc;
}
return_code = code;
return cnum;
}
