public bool Intersects(AABBThreeD aabb)
{
var obb = new OBBThreeD();
obb.SetFrom(aabb);
return(Intersects(obb, 0));
}
void LogClosePoint()
{
var s = OBBThreeD.ClosestPointTo(new Vector3(10, 10, 10), new Vector3(15, 15, 15), new Vector3(0.5f, 0.5f, 0.5f), new Quaternion(30, 30, 30, 30));
Debug.Log(s);
}
public bool Intersects(OBBThreeD obb)
{
return(obb.Intersects(this));
}
public bool Contains(OBBThreeD obb)
{
return(Contains(obb.MinimalEnclosingAABB()));
}
public void SetFrom(OBBThreeD obb)
{
Vector3 halfSize = MathFunc.Abs(obb.axis[0] * obb.r[0]) + MathFunc.Abs(obb.axis[1] * obb.r[1]) + MathFunc.Abs(obb.axis[2] * obb.r[2]);
SetFromCenterAndSize(obb.pos, 2.0f * halfSize);
}
internal void OnObbEnter(OBBThreeD obb)
{
Debug.Log("OnObbEnter");
}
public bool SphereVsObb(OBBThreeD obb)
{
return(obb.Intersects(this));
}
public static bool IntersectBoxBox(OBBThreeD box0, OBBThreeD box1)
{
Vector3 v = box1.pos - box0.pos;
//Compute A's basis
Vector3 VAx = box0.rotation * new Vector3(1, 0, 0);
Vector3 VAy = box0.rotation * new Vector3(0, 1, 0);
Vector3 VAz = box0.rotation * new Vector3(0, 0, 1);
Vector3[] VA = new Vector3[3];
VA[0] = VAx;
VA[1] = VAy;
VA[2] = VAz;
//Compute B's basis
Vector3 VBx = box1.rotation * new Vector3(1, 0, 0);
Vector3 VBy = box1.rotation * new Vector3(0, 1, 0);
Vector3 VBz = box1.rotation * new Vector3(0, 0, 1);
Vector3[] VB = new Vector3[3];
VB[0] = VBx;
VB[1] = VBy;
VB[2] = VBz;
Vector3 T = new Vector3(Vector3.Dot(v, VAx), Vector3.Dot(v, VAy), Vector3.Dot(v, VAz));
float[,] R = new float[3, 3];
float[,] FR = new float[3, 3];
float ra, rb, t;
for (int i = 0; i < 3; i++)
{
for (int k = 0; k < 3; k++)
{
R[i, k] = Vector3.Dot(VA[i], VB[k]);
FR[i, k] = 1e-6f + Mathf.Abs(R[i, k]);
}
}
// A's basis vectors
for (int i = 0; i < 3; i++)
{
ra = box0.halfExtents[i];
rb = box1.halfExtents[0] * FR[i, 0] + box1.halfExtents[1] * FR[i, 1] + box1.halfExtents[2] * FR[i, 2];
t = Mathf.Abs(T[i]);
if (t > ra + rb)
{
return(false);
}
}
// B's basis vectors
for (int k = 0; k < 3; k++)
{
ra = box0.halfExtents[0] * FR[0, k] + box0.halfExtents[1] * FR[1, k] + box0.halfExtents[2] * FR[2, k];
rb = box1.halfExtents[k];
t = Mathf.Abs(T[0] * R[0, k] + T[1] * R[1, k] + T[2] * R[2, k]);
if (t > ra + rb)
{
return(false);
}
}
//9 cross products
//L = A0 x B0
ra = box0.halfExtents[1] * FR[2, 0] + box0.halfExtents[2] * FR[1, 0];
rb = box1.halfExtents[1] * FR[0, 2] + box1.halfExtents[2] * FR[0, 1];
t = Mathf.Abs(T[2] * R[1, 0] - T[1] * R[2, 0]);
if (t > ra + rb)
{
return(false);
}
//L = A0 x B1
ra = box0.halfExtents[1] * FR[2, 1] + box0.halfExtents[2] * FR[1, 1];
rb = box1.halfExtents[0] * FR[0, 2] + box1.halfExtents[2] * FR[0, 0];
t = Mathf.Abs(T[2] * R[1, 1] - T[1] * R[2, 1]);
if (t > ra + rb)
{
return(false);
}
//L = A0 x B2
ra = box0.halfExtents[1] * FR[2, 2] + box0.halfExtents[2] * FR[1, 2];
rb = box1.halfExtents[0] * FR[0, 1] + box1.halfExtents[1] * FR[0, 0];
t = Mathf.Abs(T[2] * R[1, 2] - T[1] * R[2, 2]);
if (t > ra + rb)
{
return(false);
}
//L = A1 x B0
ra = box0.halfExtents[0] * FR[2, 0] + box0.halfExtents[2] * FR[0, 0];
rb = box1.halfExtents[1] * FR[1, 2] + box1.halfExtents[2] * FR[1, 1];
t = Mathf.Abs(T[0] * R[2, 0] - T[2] * R[0, 0]);
if (t > ra + rb)
{
return(false);
}
//L = A1 x B1
ra = box0.halfExtents[0] * FR[2, 1] + box0.halfExtents[2] * FR[0, 1];
rb = box1.halfExtents[0] * FR[1, 2] + box1.halfExtents[2] * FR[1, 0];
t = Mathf.Abs(T[0] * R[2, 1] - T[2] * R[0, 1]);
if (t > ra + rb)
{
return(false);
}
//L = A1 x B2
ra = box0.halfExtents[0] * FR[2, 2] + box0.halfExtents[2] * FR[0, 2];
rb = box1.halfExtents[0] * FR[1, 1] + box1.halfExtents[1] * FR[1, 0];
t = Mathf.Abs(T[0] * R[2, 2] - T[2] * R[0, 2]);
if (t > ra + rb)
{
return(false);
}
//L = A2 x B0
ra = box0.halfExtents[0] * FR[1, 0] + box0.halfExtents[1] * FR[0, 0];
rb = box1.halfExtents[1] * FR[2, 2] + box1.halfExtents[2] * FR[2, 1];
t = Mathf.Abs(T[1] * R[0, 0] - T[0] * R[1, 0]);
if (t > ra + rb)
{
return(false);
}
//L = A2 x B1
ra = box0.halfExtents[0] * FR[1, 1] + box0.halfExtents[1] * FR[0, 1];
rb = box1.halfExtents[0] * FR[2, 2] + box1.halfExtents[2] * FR[2, 0];
t = Mathf.Abs(T[1] * R[0, 1] - T[0] * R[1, 1]);
if (t > ra + rb)
{
return(false);
}
//L = A2 x B2
ra = box0.halfExtents[0] * FR[1, 2] + box0.halfExtents[1] * FR[0, 2];
rb = box1.halfExtents[0] * FR[2, 1] + box1.halfExtents[1] * FR[2, 0];
t = Mathf.Abs(T[1] * R[0, 2] - T[0] * R[1, 2]);
if (t > ra + rb)
{
return(false);
}
return(true);
}
public bool Intersects(OBBThreeD b, float epsilon = 0)
{
//assume(pos.IsFinite());
// assume(b.pos.IsFinite());
// assume(float3::AreOrthonormal(axis[0], axis[1], axis[2]));
// assume(float3::AreOrthonormal(b.axis[0], b.axis[1], b.axis[2]));
float ra;
float rb;
// Generate a rotation matrix that transforms from world space to this OBB's coordinate space.
var R = Float3X3.Create();
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
R[i][j] = Dot(axis[i], b.axis[j]);
}
}
Vector3 t = b.pos - pos;
// Express the translation vector in a's coordinate frame.
t = new Vector3(Dot(t, axis[0]), Dot(t, axis[1]), Dot(t, axis[2]));
var AbsR = Float3X3.Create();
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
AbsR[i][j] = Mathf.Abs(R[i][j]) + epsilon;
}
}
// Test the three major axes of this OBB.
for (int i = 0; i < 3; ++i)
{
ra = r[i];
rb = DOT3(b.r, AbsR[i]);
if (Mathf.Abs(t[i]) > ra + rb)
{
return(false);
}
}
// Test the three major axes of the OBB b.
for (int i = 0; i < 3; ++i)
{
ra = r[0] * AbsR[0][i] + r[1] * AbsR[1][i] + r[2] * AbsR[2][i];
rb = b.r[i];
if (Mathf.Abs(t.x * R[0][i] + t.y * R[1][i] + t.z * R[2][i]) > ra + rb)
{
return(false);
}
}
// Test the 9 different cross-axes.
// A.x <cross> B.x
ra = r.y * AbsR[2][0] + r.z * AbsR[1][0];
rb = b.r.y * AbsR[0][2] + b.r.z * AbsR[0][1];
if (Mathf.Abs(t.z * R[1][0] - t.y * R[2][0]) > ra + rb)
{
return(false);
}
// A.x < cross> B.y
ra = r.y * AbsR[2][1] + r.z * AbsR[1][1];
rb = b.r.x * AbsR[0][2] + b.r.z * AbsR[0][0];
if (Mathf.Abs(t.z * R[1][1] - t.y * R[2][1]) > ra + rb)
{
return(false);
}
// A.x <cross> B.z
ra = r.y * AbsR[2][2] + r.z * AbsR[1][2];
rb = b.r.x * AbsR[0][1] + b.r.y * AbsR[0][0];
if (Mathf.Abs(t.z * R[1][2] - t.y * R[2][2]) > ra + rb)
{
return(false);
}
// A.y <cross> B.x
ra = r.x * AbsR[2][0] + r.z * AbsR[0][0];
rb = b.r.y * AbsR[1][2] + b.r.z * AbsR[1][1];
if (Mathf.Abs(t.x * R[2][0] - t.z * R[0][0]) > ra + rb)
{
return(false);
}
// A.y <cross> B.y
ra = r.x * AbsR[2][1] + r.z * AbsR[0][1];
rb = b.r.x * AbsR[1][2] + b.r.z * AbsR[1][0];
if (Mathf.Abs(t.x * R[2][1] - t.z * R[0][1]) > ra + rb)
{
return(false);
}
// A.y <cross> B.z
ra = r.x * AbsR[2][2] + r.z * AbsR[0][2];
rb = b.r.x * AbsR[1][1] + b.r.y * AbsR[1][0];
if (Mathf.Abs(t.x * R[2][2] - t.z * R[0][2]) > ra + rb)
{
return(false);
}
// A.z <cross> B.x
ra = r.x * AbsR[1][0] + r.y * AbsR[0][0];
rb = b.r.y * AbsR[2][2] + b.r.z * AbsR[2][1];
if (Mathf.Abs(t.y * R[0][0] - t.x * R[1][0]) > ra + rb)
{
return(false);
}
// A.z <cross> B.y
ra = r.x * AbsR[1][1] + r.y * AbsR[0][1];
rb = b.r.x * AbsR[2][2] + b.r.z * AbsR[2][0];
if (Mathf.Abs(t.y * R[0][1] - t.x * R[1][1]) > ra + rb)
{
return(false);
}
// A.z <cross> B.z
ra = r.x * AbsR[1][2] + r.y * AbsR[0][2];
rb = b.r.x * AbsR[2][1] + b.r.y * AbsR[2][0];
if (Mathf.Abs(t.y * R[0][2] - t.x * R[1][2]) > ra + rb)
{
return(false);
}
// No separating axis exists, so the two OBB don't intersect.
return(true);
}
