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; }