public override bool TestWith(OBB obb) { return(Utils.TestOBBOBB(obb, this.ToOBB())); }
public static bool TestOBBOBB(OBB a, OBB b) { LFloat ra, rb; LMatrix33 R = new LMatrix33(), AbsR = new LMatrix33(); // Compute rotation matrix expressing b in a's coordinate frame for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { R[i, j] = Dot(a.u[i], b.u[j]); } } // Compute translation LVector t LVector3 t = b.c - a.c; // Bring translation into a's coordinate frame t = new LVector3(Dot(t, a.u[0]), Dot(t, a.u[1]), Dot(t, a.u[2])); // Compute common subexpressions. Add in an epsilon term to // counteract arithmetic errors when two edges are parallel and // their cross product is (near) null (see text for details) for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { AbsR[i, j] = Abs(R[i, j]) + LFloat.EPSILON; } } // Test axes L = A0, L = A1, L = A2 for (int i = 0; i < 3; i++) { ra = a.e[i]; rb = b.e[0] * AbsR[i, 0] + b.e[1] * AbsR[i, 1] + b.e[2] * AbsR[i, 2]; if (Abs(t[i]) > ra + rb) { return(false); } } // Test axes L = B0, L = B1, L = B2 for (int i = 0; i < 3; i++) { ra = a.e[0] * AbsR[0, i] + a.e[1] * AbsR[1, i] + a.e[2] * AbsR[2, i]; rb = b.e[i]; if (Abs(t[0] * R[0, i] + t[1] * R[1, i] + t[2] * R[2, i]) > ra + rb) { return(false); } } // Test axis L = A0 x B0 ra = a.e[1] * AbsR[2, 0] + a.e[2] * AbsR[1, 0]; rb = b.e[1] * AbsR[0, 2] + b.e[2] * AbsR[0, 1]; if (Abs(t[2] * R[1, 0] - t[1] * R[2, 0]) > ra + rb) { return(false); } // Test axis L = A0 x B1 ra = a.e[1] * AbsR[2, 1] + a.e[2] * AbsR[1, 1]; rb = b.e[0] * AbsR[0, 2] + b.e[2] * AbsR[0, 0]; if (Abs(t[2] * R[1, 1] - t[1] * R[2, 1]) > ra + rb) { return(false); } // Test axis L = A0 x B2 ra = a.e[1] * AbsR[2, 2] + a.e[2] * AbsR[1, 2]; rb = b.e[0] * AbsR[0, 1] + b.e[1] * AbsR[0, 0]; if (Abs(t[2] * R[1, 2] - t[1] * R[2, 2]) > ra + rb) { return(false); } // Test axis L = A1 x B0 ra = a.e[0] * AbsR[2, 0] + a.e[2] * AbsR[0, 0]; rb = b.e[1] * AbsR[1, 2] + b.e[2] * AbsR[1, 1]; if (Abs(t[0] * R[2, 0] - t[2] * R[0, 0]) > ra + rb) { return(false); } // Test axis L = A1 x B1 ra = a.e[0] * AbsR[2, 1] + a.e[2] * AbsR[0, 1]; rb = b.e[0] * AbsR[1, 2] + b.e[2] * AbsR[1, 0]; if (Abs(t[0] * R[2, 1] - t[2] * R[0, 1]) > ra + rb) { return(false); } // Test axis L = A1 x B2 ra = a.e[0] * AbsR[2, 2] + a.e[2] * AbsR[0, 2]; rb = b.e[0] * AbsR[1, 1] + b.e[1] * AbsR[1, 0]; if (Abs(t[0] * R[2, 2] - t[2] * R[0, 2]) > ra + rb) { return(false); } // Test axis L = A2 x B0 ra = a.e[0] * AbsR[1, 0] + a.e[1] * AbsR[0, 0]; rb = b.e[1] * AbsR[2, 2] + b.e[2] * AbsR[2, 1]; if (Abs(t[1] * R[0, 0] - t[0] * R[1, 0]) > ra + rb) { return(false); } // Test axis L = A2 x B1 ra = a.e[0] * AbsR[1, 1] + a.e[1] * AbsR[0, 1]; rb = b.e[0] * AbsR[2, 2] + b.e[2] * AbsR[2, 0]; if (Abs(t[1] * R[0, 1] - t[0] * R[1, 1]) > ra + rb) { return(false); } // Test axis L = A2 x B2 ra = a.e[0] * AbsR[1, 2] + a.e[1] * AbsR[0, 2]; rb = b.e[0] * AbsR[2, 1] + b.e[1] * AbsR[2, 0]; if (Abs(t[1] * R[0, 2] - t[0] * R[1, 2]) > ra + rb) { return(false); } // Since no separating axis found, the OBBs must be intersecting return(true); }