public static bool Intersect(OBB a, OBB b)
{
Vector3 ac = a.center;
Vector3 bc = b.center;
Vector3 ae = a.dimension;
Vector3 be = b.dimension;
Vector3 u0 = new Vector3(a.matrix.m00, a.matrix.m10, a.matrix.m20);
u0.Normalize();
Vector3 u1 = new Vector3(a.matrix.m01, a.matrix.m11, a.matrix.m21);
u1.Normalize();
Vector3 u2 = new Vector3(a.matrix.m02, a.matrix.m12, a.matrix.m22);
u2.Normalize();
Vector3[] au = new Vector3[3] {
u0, u1, u2
};
u0 = new Vector3(b.matrix.m00, b.matrix.m10, b.matrix.m20);
u0.Normalize();
u1 = new Vector3(b.matrix.m01, b.matrix.m11, b.matrix.m21);
u1.Normalize();
u2 = new Vector3(b.matrix.m02, b.matrix.m12, b.matrix.m22);
u2.Normalize();
Vector3[] bu = new Vector3[3] {
u0, u1, u2
};
float[][] R = new float[3][];
R[0] = new float[3];
R[1] = new float[3];
R[2] = new float[3];
float[][] AbsR = new float[3][];
AbsR[0] = new float[3];
AbsR[1] = new float[3];
AbsR[2] = new float[3];
float ra, rb;
// 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] = Vector3.Dot(au[i], bu[j]);
}
}
// Compute translation vector t
Vector3 t = bc - ac;
// Bring translation into a's coordinate frame
t = new Vector3(Vector3.Dot(t, au[0]), Vector3.Dot(t, au[1]), Vector3.Dot(t, au[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] = Mathf.Abs(R[i][j]) + Mathf.Epsilon;
}
}
// Test axes L = A0, L = A1, L = A2
for (int i = 0; i < 3; i++)
{
ra = ae[i];
rb = be[0] * AbsR[i][0] + be[1] * AbsR[i][1] + be[2] * AbsR[i][2];
if (Mathf.Abs(t[i]) > ra + rb)
{
return(false);
}
}
// Test axes L = B0, L = B1, L = B2
for (int i = 0; i < 3; i++)
{
ra = ae[0] * AbsR[0][i] + ae[1] * AbsR[1][i] + ae[2] * AbsR[2][i];
rb = be[i];
if (Mathf.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 = ae[1] * AbsR[2][0] + ae[2] * AbsR[1][0];
rb = be[1] * AbsR[0][2] + be[2] * AbsR[0][1];
if (Mathf.Abs(t[2] * R[1][0] - t[1] * R[2][0]) > ra + rb)
{
return(false);
}
// Test axis L = A0 x B1
ra = ae[1] * AbsR[2][1] + ae[2] * AbsR[1][1];
rb = be[0] * AbsR[0][2] + be[2] * AbsR[0][0];
if (Mathf.Abs(t[2] * R[1][1] - t[1] * R[2][1]) > ra + rb)
{
return(false);
}
// Test axis L = A0 x B2
ra = ae[1] * AbsR[2][2] + ae[2] * AbsR[1][2];
rb = be[0] * AbsR[0][1] + be[1] * AbsR[0][0];
if (Mathf.Abs(t[2] * R[1][2] - t[1] * R[2][2]) > ra + rb)
{
return(false);
}
// Test axis L = A1 x B0
ra = ae[0] * AbsR[2][0] + ae[2] * AbsR[0][0];
rb = be[1] * AbsR[1][2] + be[2] * AbsR[1][1];
if (Mathf.Abs(t[0] * R[2][0] - t[2] * R[0][0]) > ra + rb)
{
return(false);
}
// Test axis L = A1 x B1
ra = ae[0] * AbsR[2][1] + ae[2] * AbsR[0][1];
rb = be[0] * AbsR[1][2] + be[2] * AbsR[1][0];
if (Mathf.Abs(t[0] * R[2][1] - t[2] * R[0][1]) > ra + rb)
{
return(false);
}
// Test axis L = A1 x B2
ra = ae[0] * AbsR[2][2] + ae[2] * AbsR[0][2];
rb = be[0] * AbsR[1][1] + be[1] * AbsR[1][0];
if (Mathf.Abs(t[0] * R[2][2] - t[2] * R[0][2]) > ra + rb)
{
return(false);
}
// Test axis L = A2 x B0
ra = ae[0] * AbsR[1][0] + ae[1] * AbsR[0][0];
rb = be[1] * AbsR[2][2] + be[2] * AbsR[2][1];
if (Mathf.Abs(t[1] * R[0][0] - t[0] * R[1][0]) > ra + rb)
{
return(false);
}
// Test axis L = A2 x B1
ra = ae[0] * AbsR[1][1] + ae[1] * AbsR[0][1];
rb = be[0] * AbsR[2][2] + be[2] * AbsR[2][0];
if (Mathf.Abs(t[1] * R[0][1] - t[0] * R[1][1]) > ra + rb)
{
return(false);
}
// Test axis L = A2 x B2
ra = ae[0] * AbsR[1][2] + ae[1] * AbsR[0][2];
rb = be[0] * AbsR[2][1] + be[1] * AbsR[2][0];
if (Mathf.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);
}
public bool Intersect(OBB obb)
{
return(Intersect(this, obb));
}