vwemirror/firestorm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
e84d6e603c115c333f157807ce486ee0a73862ad
firestorm/Gameleap/code/mw4/Libraries/stuff/OBB.cpp
T
Cyd 2b8ca921cb Initial full mirror of c:\VWE (source + assets + toolchain + outputs) via Git LFS 
Complete disaster-recovery snapshot: engine/game source, game data assets,
VC6 toolchain + DX SDKs, build outputs, deployed game, and _UNUSED archive.
Large binaries in Git LFS; text preserved byte-for-byte (core.autocrlf=false,
no eol attributes). See RECOVERY.md for the one-clone rebuild procedure.
2026-06-24 21:28:16 -05:00

1574 lines
56 KiB
C++
Raw Blame History

#include "StuffHeaders.hpp"
OBB OBB::Identity(LinearMatrix4D(0), Vector3D(0.0f, 0.0f, 0.0f), 0.0f);
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void OBB::TestInstance() const
{
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
int OBB::ComputeBounds(ReadOnlyArrayOf<Point3D> &points)
{
Check_Object(&points);
//
//----------------------------------------------------------
// Find the coordinate system the points would like to be in
//----------------------------------------------------------
//
localToParent.ComputeAxes(points);
LinearMatrix4D world_to_local;
world_to_local.Invert(localToParent);
int type = 1;
//
//----------------------------------------------------------------
// Find the extents of both the local and world bounds and use the
// smaller set
//----------------------------------------------------------------
//
DynamicArrayOf<Point3D> local_points(points.GetLength());
for (int i=0; i<points.GetLength(); ++i)
local_points[i].Multiply(points[i], world_to_local);
ExtentBox local_box;
ExtentBox box;
local_box.ComputeBounds(local_points);
box.ComputeBounds(points);
//
//----------------------------------------------------
// If we are using the world box reset localToParent
//----------------------------------------------------
//
Point3D center_in_world;
if (box.GetVolume() < local_box.GetVolume())
{
// Get center-in-world directly from world box
box.GetCenterpoint(&center_in_world);
localToParent = LinearMatrix4D::Identity;
axisExtents.x = 0.5f * (box.maxX - box.minX) + SMALL;
axisExtents.y = 0.5f * (box.maxY - box.minY) + SMALL;
axisExtents.z = 0.5f * (box.maxZ - box.minZ) + SMALL;
type = 2;
}
//
//--------------------------------------------------------------
// Else we are using the local box recompute the center-in-world
//--------------------------------------------------------------
//
else {
Point3D center;
local_box.GetCenterpoint(&center);
center_in_world.Multiply(center, localToParent);
axisExtents.x = 0.5f * (local_box.maxX - local_box.minX) + SMALL;
axisExtents.y = 0.5f * (local_box.maxY - local_box.minY) + SMALL;
axisExtents.z = 0.5f * (local_box.maxZ - local_box.minZ) + SMALL;
}
localToParent(3,0) = center_in_world.x;
localToParent(3,1) = center_in_world.y;
localToParent(3,2) = center_in_world.z;
sphereRadius = axisExtents.GetLength();
return type;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
#if !defined(Spew)
void Spew(
const char* group,
const OBB &box
)
{
Check_Object(&box);
SPEW((group, ""));
SPEW((group, " Transform = +"));
Spew(group, box.localToParent);
SPEW((group, ""));
SPEW((
group,
" Extents = <%4f,%4f,%4f>, Radius = %4f+",
box.axisExtents.x,
box.axisExtents.y,
box.axisExtents.z,
box.sphereRadius
));
}
#endif
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
OBB& OBB::Multiply(
const OBB &obb,
const LinearMatrix4D &matrix
)
{
Check_Pointer(this);
Check_Object(&obb);
Check_Object(&matrix);
localToParent.Multiply(obb.localToParent, matrix);
axisExtents = obb.axisExtents;
sphereRadius = obb.sphereRadius;
return *this;
}
#if 0 // moved it into hpp-file
//
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
OBB& OBB::MultiplySphereOnly(
const OBB &obb,
const LinearMatrix4D &matrix
)
{
Check_Pointer(this);
Check_Object(&obb);
Check_Object(&matrix);
#if USE_ASSEMBLER_CODE
Scalar *f = localToParent.entries;
_asm {
mov edx, matrix
push esi
mov esi, obb.localToParent
mov eax, f
fld dword ptr [edx+4] // m[1][0]
fmul dword ptr [esi+01ch] // obb.localToParent(3,1)
fld dword ptr [edx+8] // m[2][0]
fmul dword ptr [esi+02Ch] // obb.localToParent(3,2)
fxch st(1)
fadd dword ptr [edx+0Ch] // m[3][0]
fld dword ptr [edx] // m[0][0]
fmul dword ptr [esi+0Ch] // obb.localToParent(3,0)
fxch st(2)
faddp st(1),st
fld dword ptr [edx+14h] // m[1][1]
fmul dword ptr [esi+01ch] // obb.localToParent(3,1)
fxch st(2)
faddp st(1),st
fld dword ptr [edx+18h] // m[2][1]
fmul dword ptr [esi+02ch] // obb.localToParent(3,2)
fxch st(1)
fstp dword ptr [eax+0ch] // localToParent(3,0)
fadd dword ptr [edx+1Ch] // m[3][1]
fld dword ptr [edx+10h] // m[0][1]
fmul dword ptr [esi+0ch] // obb.localToParent(3,0)
fxch st(2)
faddp st(1),st
fld dword ptr [edx+24h] // m[1][2]
fmul dword ptr [esi+01ch] // obb.localToParent(3,1)
fxch st(2)
faddp st(1),st
fld dword ptr [edx+28h] // m[2][2]
fmul dword ptr [esi+02ch] // obb.localToParent(3,2)
fxch st(1)
fstp dword ptr [eax+01ch] // localToParent(3,1)
fadd dword ptr [edx+2Ch] // m[3][2]
fld dword ptr [edx+20h] // m[0][2]
fmul dword ptr [esi+0ch] // obb.localToParent(3,0)
fxch st(2)
faddp st(1),st
pop esi
faddp st(1),st
fstp dword ptr [eax+02ch] // localToParent(3,2)
}
#else
localToParent(3,0) =
obb.localToParent(3,0)*matrix(0,0)
+ obb.localToParent(3,1)*matrix(1,0)
+ obb.localToParent(3,2)*matrix(2,0)
+ matrix(3,0);
localToParent(3,1) =
obb.localToParent(3,0)*matrix(0,1)
+ obb.localToParent(3,1)*matrix(1,1)
+ obb.localToParent(3,2)*matrix(2,1)
+ matrix(3,1);
localToParent(3,2) =
obb.localToParent(3,0)*matrix(0,2)
+ obb.localToParent(3,1)*matrix(1,2)
+ obb.localToParent(3,2)*matrix(2,2)
+ matrix(3,2);
#endif
sphereRadius = obb.sphereRadius;
return *this;
}
#endif
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
OBB::SeparatingAxis OBB::FindSeparatingAxis(const OBB& box) const
{
Check_Object(this);
Check_Object(&box);
//
//-----------------------------------------------------------------------
// The following routine is based on various simplifications of the
// following formula: If L represents a line in space, two OBBs (A and B)
// are separated by on that axis if (B-A)*L > sum of the projected radii.
// The projected radius of each OBB is equal to the sum of the three
// cardinal axes (in parent space) projected unto the line and multiplied
// by the OBBs extent size in that direction
//-----------------------------------------------------------------------
//
Verify(axisExtents.x >= 0.0f);
Verify(axisExtents.y >= 0.0f);
Verify(axisExtents.z >= 0.0f);
Verify(box.axisExtents.x >= 0.0f);
Verify(box.axisExtents.y >= 0.0f);
Verify(box.axisExtents.z >= 0.0f);
//
//----------------------------------------------------------------------
// Get the distance between the centerpoints. Both boxes are assumed to
// convert their local axes into a common parent space
//----------------------------------------------------------------------
//
Check_Object(&localToParent);
Check_Object(&box.localToParent);
Point3D
distance(
localToParent(3,0) - box.localToParent(3,0),
localToParent(3,1) - box.localToParent(3,1),
localToParent(3,2) - box.localToParent(3,2)
);
//
//--------------------------------------------------------------------------------------------------
// The rows of the matrix encode the local axes in world space. The first
// separation axis we will be testing our x axis in world space, so
// project the other box's primary axes onto it, and store the results
// away for future reference
//
// The math for L = A0: (A0 is the X axis row of our matrix)
// Given: A0,A1, and B2 are orthogonal
// B0,B1, and B2 are orthogonal
// Ax, Ay, and Az >= 0
// Bx, By, and Bz >= 0
//
// projection = (B-A)*A0
// radius = |Ax*(A0*A0)| + |Ay*(A0*A1)| + |Az*(A0*A2)| + |Bx*(A0*B0)| + |By*(A0*B1)| + |Bz*(A0*B2)|
// = Ax + Bx*|A0*B0| + By*|A0*B1| + Bz*|A0*B2|
//--------------------------------------------------------------------------------------------------
//
Scalar axis_projections[3][3];
axis_projections[X_Axis][X_Axis] =
Fabs(
localToParent(X_Axis,0) * box.localToParent(X_Axis,0)
+ localToParent(X_Axis,1) * box.localToParent(X_Axis,1)
+ localToParent(X_Axis,2) * box.localToParent(X_Axis,2)
);
axis_projections[X_Axis][Y_Axis] =
Fabs(
localToParent(X_Axis,0) * box.localToParent(Y_Axis,0)
+ localToParent(X_Axis,1) * box.localToParent(Y_Axis,1)
+ localToParent(X_Axis,2) * box.localToParent(Y_Axis,2)
);
axis_projections[X_Axis][Z_Axis] =
Fabs(
localToParent(X_Axis,0) * box.localToParent(Z_Axis,0)
+ localToParent(X_Axis,1) * box.localToParent(Z_Axis,1)
+ localToParent(X_Axis,2) * box.localToParent(Z_Axis,2)
);
//
//-----------------------------------------------------------------------
// The maximum distance the centerpoints can be away and still touch is
// equal to the sum of our x extent plus the other's extents, as modified
// by the projection values. If the boxes can't touch, return A0 as the
// separating plane
//-----------------------------------------------------------------------
//
Scalar radius =
axisExtents.x
+ box.axisExtents.x * axis_projections[X_Axis][0]
+ box.axisExtents.y * axis_projections[X_Axis][1]
+ box.axisExtents.z * axis_projections[X_Axis][2];
Scalar projection =
localToParent(X_Axis,0) * distance.x
+ localToParent(X_Axis,1) * distance.y
+ localToParent(X_Axis,2) * distance.z;
if (projection > radius || projection < -radius)
return A0;
//
//--------------------------------------------------------------------------------------------------
// Do the same check against our Y axis
//
// The math for L = A1:
//
// projection = (B-A)*A1
// radius = |Ax*(A1*A0)| + |Ay*(A1*A1)| + |Az*(A1*A2)| + |Bx*(A1*B0)| + |By*(A1*B1)| + |Bz*(A1*B2)|
// = Ay + Bx*|A1*B0| + By*|A1*B1| + Bz*|A1*B2|
//--------------------------------------------------------------------------------------------------
//
axis_projections[Y_Axis][X_Axis] =
Fabs(
localToParent(Y_Axis,0) * box.localToParent(X_Axis,0)
+ localToParent(Y_Axis,1) * box.localToParent(X_Axis,1)
+ localToParent(Y_Axis,2) * box.localToParent(X_Axis,2)
);
axis_projections[Y_Axis][Y_Axis] =
Fabs(
localToParent(Y_Axis,0) * box.localToParent(Y_Axis,0)
+ localToParent(Y_Axis,1) * box.localToParent(Y_Axis,1)
+ localToParent(Y_Axis,2) * box.localToParent(Y_Axis,2)
);
axis_projections[Y_Axis][Z_Axis] =
Fabs(
localToParent(Y_Axis,0) * box.localToParent(Z_Axis,0)
+ localToParent(Y_Axis,1) * box.localToParent(Z_Axis,1)
+ localToParent(Y_Axis,2) * box.localToParent(Z_Axis,2)
);
radius =
axisExtents.y
+ box.axisExtents.x * axis_projections[1][0]
+ box.axisExtents.y * axis_projections[1][1]
+ box.axisExtents.z * axis_projections[1][2];
projection =
localToParent(1,0) * distance.x
+ localToParent(1,1) * distance.y
+ localToParent(1,2) * distance.z;
if (projection > radius || projection < -radius)
return A1;
//
//--------------------------------------------------------------------------------------------------
// Check our z axis
//
// The math for L = A2:
//
// projection = (B-A)*A2
// radius = |Ax*(A2*A0)| + |Ay*(A2*A1)| + |Az*(A2*A2)| + |Bx*(A2*B0)| + |By*(A2*B1)| + |Bz*(A2*B2)|
// = Az + Bx*|A2*B0| + By*|A2*B1| + Bz*|A2*B2|
//--------------------------------------------------------------------------------------------------
//
axis_projections[Z_Axis][X_Axis] =
Fabs(
localToParent(Z_Axis,0) * box.localToParent(X_Axis,0)
+ localToParent(Z_Axis,1) * box.localToParent(X_Axis,1)
+ localToParent(Z_Axis,2) * box.localToParent(X_Axis,2)
);
axis_projections[Z_Axis][Y_Axis] =
Fabs(
localToParent(Z_Axis,0) * box.localToParent(Y_Axis,0)
+ localToParent(Z_Axis,1) * box.localToParent(Y_Axis,1)
+ localToParent(Z_Axis,2) * box.localToParent(Y_Axis,2)
);
axis_projections[Z_Axis][Z_Axis] =
Fabs(
localToParent(Z_Axis,0) * box.localToParent(Z_Axis,0)
+ localToParent(Z_Axis,1) * box.localToParent(Z_Axis,1)
+ localToParent(Z_Axis,2) * box.localToParent(Z_Axis,2)
);
radius =
axisExtents.z
+ box.axisExtents.x * axis_projections[2][0]
+ box.axisExtents.y * axis_projections[2][1]
+ box.axisExtents.z * axis_projections[2][2];
projection =
localToParent(2,0) * distance.x
+ localToParent(2,1) * distance.y
+ localToParent(2,2) * distance.z;
if (projection > radius || projection < -radius)
return A2;
//
//--------------------------------------------------------------------------------------------------
// See if the other box's X axis is a separator
//
// The math for L = B0:
//
// projection = (B-A)*B0
// radius = |Ax*(B0*A0)| + |Ay*(B0*A1)| + |Az*(B0*A2)| + |Bx*(B0*B0)| + |By*(B0*B1)| + |Bz*(B0*B2)|
// = Bx + Ax*|B0*A0| + Ay*|B0*A1| + Az*|B0*A2|
//--------------------------------------------------------------------------------------------------
//
radius =
box.axisExtents.x
+ axisExtents.x * axis_projections[0][X_Axis]
+ axisExtents.y * axis_projections[1][X_Axis]
+ axisExtents.z * axis_projections[2][X_Axis];
projection =
box.localToParent(X_Axis,0) * distance.x
+ box.localToParent(X_Axis,1) * distance.y
+ box.localToParent(X_Axis,2) * distance.z;
if (projection > radius || projection < -radius)
return B0;
//
//--------------------------------------------------------------------------------------------------
// Check its Y axis
//
// The math for L = B1:
//
// projection = (B-A)*B1
// radius = |Ax*(B1*A0)| + |Ay*(B1*A1)| + |Az*(B1*A2)| + |Bx*(B1*B0)| + |By*(B1*B1)| + |Bz*(B1*B2)|
// = By + Ax*|B1*A0| + Ay*|B1*A1| + Az*|B1*A2|
//--------------------------------------------------------------------------------------------------
//
radius =
box.axisExtents.y
+ axisExtents.x * axis_projections[0][Y_Axis]
+ axisExtents.y * axis_projections[1][Y_Axis]
+ axisExtents.z * axis_projections[2][Y_Axis];
projection =
box.localToParent(Y_Axis,0) * distance.x
+ box.localToParent(Y_Axis,1) * distance.y
+ box.localToParent(Y_Axis,2) * distance.z;
if (projection > radius || projection < -radius)
return B1;
//
//--------------------------------------------------------------------------------------------------
// Check its Z axis
//
// The math for L = B2:
//
// projection = (B-A)*B2
// radius = |Ax*(B2*A0)| + |Ay*(B2*A1)| + |Az*(B2*A2)| + |Bx*(B2*B0)| + |By*(B2*B1)| + |Bz*(B2*B2)|
// = Bz + Ax*|B2*A0| + Ay*|B2*A1| + Az*|B2*A2|
//--------------------------------------------------------------------------------------------------
//
radius =
box.axisExtents.z
+ axisExtents.x * axis_projections[0][Z_Axis]
+ axisExtents.y * axis_projections[1][Z_Axis]
+ axisExtents.z * axis_projections[2][Z_Axis];
projection =
box.localToParent(Z_Axis,0) * distance.x
+ box.localToParent(Z_Axis,1) * distance.y
+ box.localToParent(Z_Axis,2) * distance.z;
if (projection > radius || projection < -radius)
return B2;
//
//--------------------------------------------------------------------------
// We have finished with the separation tests based upon the box faces. We
// now need to test with the edges, so make a matrix that converts the other
// box into our space, then recompute the distance vector (its just the
// other boxes's new origin)
//--------------------------------------------------------------------------
//
LinearMatrix4D parent_to_local;
parent_to_local.Invert(localToParent);
LinearMatrix4D b;
b.Multiply(box.localToParent, parent_to_local);
distance = b;
//
//------------------------------------------------------------------------------------------------------------
// The next 9 tests will create separation axes by crossing the 3 direction
// vectors of our box with the 3 direction vectors of the other box
//
// The math for L = A0xB0 or <1,0,0>xB0:
//
// projection = (B-A)*(A0xB0) = (B-A)*<0,-B(0,2),B(0,1)>
// radius = |Ax*((A0xB0)*A0)| + |Ay*((A0xB0)*A1)| + |Az*((A0xB0)*A2)|
// + |Bx*((A0xB0)*B0)| + |By*((A0xB0)*B1)| + |Bz*((A0xB0)*B2)|
// = Ay*|(A0xB0)*A1| + Az*|(A0xB0)*A2| + By*|(A0xB0)*B1| + Bz*|(A0xB0)*B2|
// = Ay*|(<1,0,0>xB0)*<0,1,0>| + Az*|(<1,0,0>xB0)*<0,0,1>|
// + By*|(<1,0,0>xB0)*B1| + Bz*|(<1,0,0>xB0)*B2|
// = Ay*|<0,-B(0,2),B(0,1)>*<0,1,0>| + Az*|<0,-B(0,2),B(0,1)>*<0,0,1>|
// + By*|<0,-B(0,2),B(0,1)>*B1| + Bz*|<0,-B(0,2),B(0,1)>*B2|
// = Ay*|-B(0,2)| + Az*|B(0,1)| + By*|<0,-B(0,2),B(0,1)>*B1| + Bz*|<0,-B(0,2),B(0,1)>*B2|
// = Ay*|B(0,2)| + Az*|B(0,1)|
// + By*|<0,-B(0,2),B(0,1)>*<B(1,0),B(1,1),B(1,2)>| + Bz*|<0,-B(0,2),B(0,1)>*<B(2,0),B(2,1),B(2,2)>|
// = Ay*|B(0,2)| + Az*|B(0,1)|
// + By*|<-B(0,2),B(0,1)>*<B(1,1),B(1,2)>| + Bz*|<-B(0,2),B(0,1)>*<B(2,1),B(2,2)>|
// = Ay*|B(0,2)| + Az*|B(0,1)|
// + By*|B(0,1)*B(1,2) - B(0,2)*<B(1,1)| + Bz*|B(0,1)*B(2,2) - B(0,2)*B(2,1)|
// = Ay*|B(0,2)| + Az*|B(0,1)| + By*dets(2,0) + Bz*dets(1,0)
//------------------------------------------------------------------------------------------------------------
//
Scalar dets[3][3];
dets[1][0] = Fabs(b(0,1)*b(2,2) - b(2,1)*b(0,2));
dets[2][0] = Fabs(b(0,1)*b(1,2) - b(1,1)*b(0,2));
Scalar absolutes[3][3];
absolutes[0][1] = Fabs(b(0,1));
absolutes[0][2] = Fabs(b(0,2));
radius =
axisExtents.y*absolutes[0][2]
+ axisExtents.z*absolutes[0][1]
+ box.axisExtents.y*dets[2][0]
+ box.axisExtents.z*dets[1][0];
projection = distance.z*b(0,1) - distance.y*b(0,2);
if (projection > radius || projection < -radius)
return A0xB0;
//
//------------------------------------------------------------------------------------------------------------
// The math for L = A0xB1 or <1,0,0>xB1:
//
// dets absolutes
// ... .**
// *.. ...
// *.. ...
//
// projection = (B-A)*(A0xB1) = (B-A)*<0,-B(1,2),B(1,1)>
// radius = Ax*|((A0xB1)*A0)| + Ay*|((A0xB1)*A1)| + Az*|((A0xB1)*A2)|
// + Bx*|((A0xB1)*B0)| + By*|((A0xB1)*B1)| + Bz*|((A0xB1)*B2)|
// = Ay*|(A0xB1)*A1| + Az*|(A0xB1)*A2| + Bx*|(A0xB1)*B0| + Bz*|(A0xB1)*B2|
// = Ay*|(<1,0,0>xB1)*A1| + Az*|(<1,0,0>xB1)*A2| + Bx*|(<1,0,0>xB1)*B0| + Bz*|(<1,0,0>xB1)*B2|
// = Ay*|<0,-B(1,2),B(1,1)>*<0,1,0>| + Az*|<0,-B(1,2),B(1,1)>*<0,0,1>|
// + Bx*|<0,-B(1,2),B(1,1)>*B0| + Bz*|<0,-B(1,2),B(1,1)>*B2|
// = Ay*|-B(1,2)| + Az*|B(1,1)| + Bx*|<0,-B(1,2),B(1,1)>*B0| + Bz*|<0,-B(1,2),B(1,1)>*B2|
// = Ay*|B(1,2)| + Az*|B(1,1)|
// + Bx*|<0,-B(1,2),B(1,1)>*<B(0,0),B(0,1),B(0,2)>| + Bz*|<0,-B(1,2),B(1,1)>*<B(2,0),B(2,1),B(2,2)>|
// = Ay*|B(1,2)| + Az*|B(1,1)|
// + Bx*|<-B(1,2),B(1,1)>*<B(0,1),B(0,2)>| + Bz*|<-B(1,2),B(1,1)>*<B(2,1),B(2,2)>|
// = Ay*|B(1,2)| + Az*|B(1,1)|
// + Bx*|B(1,1)*B(0,2) - B(1,2)*B(0,1)| + Bz*|B(1,1)*B(2,2) - B(1,2)*B(2,1)|
// = Ay*|B(1,2)| + Az*|B(1,1)| + Bx*dets(2,0) + Bz*dets(0,0)
//------------------------------------------------------------------------------------------------------------
//
dets[0][0] = Fabs(b(1,1)*b(2,2) - b(2,1)*b(1,2));
absolutes[1][1] = Fabs(b(1,1));
absolutes[1][2] = Fabs(b(1,2));
radius =
axisExtents.y*absolutes[1][2]
+ axisExtents.z*absolutes[1][1]
+ box.axisExtents.x*dets[2][0]
+ box.axisExtents.z*dets[0][0];
projection = distance.z*b(1,1) - distance.y*b(1,2);
if (projection > radius || projection < -radius)
return A0xB1;
//
//------------------------------------------------------------------------------------------------------------
// The math for L = A0xB2 or <1,0,0>xB2:
//
// dets absolutes
// *.. .**
// *.. .**
// *.. ...
//
// projection = (B-A)*(A0xB2) = (B-A)*<0,-B(2,2),B(2,1)>
// radius = Ax*|(A0xB2)*A0| + Ay*|(A0xB2)*A1| + Az*|(A0xB2)*A2|
// + Bx*|(A0xB2)*B0| + By*|(A0xB2)*B1| + Bz*|(A0xB2)*B2|
// = Ay*|(A0xB2)*A1| + Az*|(A0xB2)*A2| + Bx*|(A0xB2)*B0| + By*|(A0xB2)*B1|
// = Ay*|<0,-B(2,2),B(2,1)>*A1| + Az*|<0,-B(2,2),B(2,1)>*A2|
// + Bx*|<0,-B(2,2),B(2,1)>*B0| + By*|<0,-B(2,2),B(2,1)>*B1|
// = Ay*|<0,-B(2,2),B(2,1)>*<0,1,0>| + Az*|<0,-B(2,2),B(2,1)>*<0,0,1>|
// + Bx*|<0,-B(2,2),B(2,1)>*<B(0,0),B(0,1),B(0,2)>| + By*|<0,-B(2,2),B(2,1)>*<B(1,0),B(1,1),B(1,2)>|
// = Ay*|-B(2,2)| + Az*|B(2,1)|
// + Bx*|<-B(2,2),B(2,1)>*<B(0,1),B(0,2)>| + By*|<-B(2,2),B(2,1)>*<B(1,1),B(1,2)>|
// = Ay*|B(2,2)| + Az*|B(2,1)|
// + Bx*|B(2,1)*B(0,2) - B(2,2)*B(0,1)| + By*|B(2,1)*B(1,2) - B(2,2)*B(1,1)|
// = Ay*|B(2,2)| + Az*|B(2,1)| + Bx*dets(1,0) + By*dets(0,0)
//------------------------------------------------------------------------------------------------------------
//
absolutes[2][1] = Fabs(b(2,1));
absolutes[2][2] = Fabs(b(2,2));
radius =
axisExtents.y*absolutes[2][2]
+ axisExtents.z*absolutes[2][1]
+ box.axisExtents.x*dets[1][0]
+ box.axisExtents.y*dets[0][0];
projection = distance.z*b(2,1) - distance.y*b(2,2);
if (projection > radius || projection < -radius)
return A0xB2;
//
//------------------------------------------------------------------------------------------------------------
// The math for L = A1xB0 or <0,1,0>xB0:
//
// dets absolutes
// *.. .**
// *.. .**
// *.. .**
//
// projection = (B-A)*(A1xB0) = (B-A)*<B(0,2),0,-B(0,0)>
// radius = Ax*|(A1xB0)*A0| + Ay*|(A1xB0)*A1| + Az*|(A1xB0)*A2|
// + Bx*|(A1xB0)*B0| + By*|(A1xB0)*B1| + Bz*|(A1xB0)*B2|
// = Ax*|(A1xB0)*A0| + Az*|(A1xB0)*A2| + By*|(A1xB0)*B1| + Bz*|(A1xB0)*B2|
// = Ax*|<B(0,2),0,-B(0,0)>*A0| + Az*|<B(0,2),0,-B(0,0)>*A2|
// + By*|<B(0,2),0,-B(0,0)>*B1| + Bz*|<B(0,2),0,-B(0,0)>*B2|
// = Ax*|<B(0,2),0,-B(0,0)>*<1,0,0>| + Az*|<B(0,2),0,-B(0,0)>*<0,0,1>|
// + By*|<B(0,2),0,-B(0,0)>*<B(1,0),B(1,1),B(1,2)>| + Bz*|<B(0,2),0,-B(0,0)>*<B(2,0),B(2,1),B(2,2)>|
// = Ax*|B(0,2)| + Az*|-B(0,0)|
// + By*|<B(0,2),-B(0,0)>*<B(1,0),B(1,2)>| + Bz*|<B(0,2),-B(0,0)>*<B(2,0),B(2,2)>|
// = Ax*|B(0,2)| + Az*|B(0,0)|
// + By*|B(0,2)*B(1,0) - B(0,0)*B(1,2)| + Bz*|B(0,2)*B(2,0) - B(0,0)*B(2,2)|
// = Ax*|B(0,2)| + Az*|B(0,0)| + By*dets(2,1) + Bz*dets(1,1)
//------------------------------------------------------------------------------------------------------------
//
dets[2][1] = Fabs(b(0,2)*b(1,0) - b(0,0)*b(1,2));
dets[1][1] = Fabs(b(0,0)*b(2,2) - b(2,0)*b(0,2));
absolutes[0][0] = Fabs(b(0,0));
radius =
axisExtents.x*absolutes[0][2]
+ axisExtents.z*absolutes[0][0]
+ box.axisExtents.y*dets[2][1]
+ box.axisExtents.z*dets[1][1];
projection = distance.x*b(0,2) - distance.z*b(0,0);
if (projection > radius || projection < -radius)
return A1xB0;
//
//------------------------------------------------------------------------------------------------------------
// The math for L = A1xB1 or <0,1,0>xB1:
//
// dets absolutes
// *.. ***
// **. .**
// **. .**
//
// projection = (B-A)*(A1xB1) = (B-A)*<B(1,2),0,-B(1,0)>
// radius = Ax*|(A1xB1)*A0| + Ay*|(A1xB1)*A1| + Az*|(A1xB1)*A2|
// + Bx*|(A1xB1)*B0| + By*|(A1xB1)*B1| + Bz*|(A1xB1)*B2|
// = Ax*|(A1xB1)*A0| + Az*|(A1xB1)*A2| + Bx*|(A1xB1)*B0| + Bz*|(A1xB1)*B2|
// = Ax*|<B(1,2),0,-B(1,0)>*A0| + Az*|<B(1,2),0,-B(1,0)>*A2|
// + Bx*|<B(1,2),0,-B(1,0)>*B0| + Bz*|<B(1,2),0,-B(1,0)>*B2|
// = Ax*|<B(1,2),0,-B(1,0)>*<1,0,0>| + Az*|<B(1,2),0,-B(1,0)>*<0,0,1>|
// + Bx*|<B(1,2),0,-B(1,0)>*<B(0,0),B(0,1),B(0,2)>| + Bz*|<B(1,2),0,-B(1,0)>*B(2,0),B(2,1),B(2,2)|
// = Ax*|B(1,2)| + Az*|-B(1,0)|
// + Bx*|<B(1,2),-B(1,0)>*<B(0,0),B(0,2)>| + Bz*|<B(1,2),-B(1,0)>*B(2,0),B(2,2)>|
// = Ax*|B(1,2)| + Az*|B(1,0)|
// + Bx*|B(1,2)*<B(0,0) - B(1,0)*B(0,2)| + Bz*|B(1,2)*B(2,0) - B(1,0)*B(2,2)|
// = Ax*|B(1,2)| + Az*|B(1,0)| + Bx*dets(2,1) + Bz*dets(0,1)
//------------------------------------------------------------------------------------------------------------
//
dets[0][1] = Fabs(b(1,2)*b(2,0) - b(1,0)*b(2,2));
absolutes[1][0] = Fabs(b(1,0));
radius =
axisExtents.x*absolutes[1][2]
+ axisExtents.z*absolutes[1][0]
+ box.axisExtents.x*dets[2][1]
+ box.axisExtents.z*dets[0][1];
projection = distance.x*b(1,2) - distance.z*b(1,0);
if (projection > radius || projection < -radius)
return A1xB1;
//
//------------------------------------------------------------------------------------------------------------
// The math for L = A1xB2 or <0,1,0>xB2:
//
// dets absolutes
// **. ***
// **. ***
// **. .**
//
// projection = (B-A)*(A1xB2) = (B-A)*<B(2,2),0,-B(2,0)>
// radius = Ax*|(A1xB2)*A0| + Ay*|(A1xB2)*A1| + Az*|(A1xB2)*A2|
// + Bx*|(A1xB2)*B0| + By*|(A1xB2)*B1| + Bz*|(A1xB2)*B2|
// = Ax*|(A1xB2)*A0| + Az*|(A1xB2)*A2| + Bx*|(A1xB2)*B0| + By*|(A1xB2)*B1|
// = Ax*|<B(2,2),0,-B(2,0)>*A0| + Az*|<B(2,2),0,-B(2,0)>*A2|
// + Bx*|<B(2,2),0,-B(2,0)>*B0| + By*|<B(2,2),0,-B(2,0)>*B1|
// = Ax*|<B(2,2),0,-B(2,0)>*<1,0,0>| + Az*|<B(2,2),0,-B(2,0)>*<0,0,1>|
// + Bx*|<B(2,2),0,-B(2,0)>*<B(0,0),B(0,1),B(0,2)>| + By*|<B(2,2),0,-B(2,0)>*<B(1,0),B(1,1),B(1,2)>|
// = Ax*|B(2,2)| + Az*|-B(2,0)|
// + Bx*|<B(2,2),-B(2,0)>*<B(0,0),B(0,2)>| + By*|<B(2,2),-B(2,0)>*<B(1,0),B(1,2)>|
// = Ax*|B(2,2)| + Az*|B(2,0)|
// + Bx*|B(2,2)*B(0,0) -B(2,0)*B(0,2)| + By*|B(2,2)*B(1,0) - B(2,0)*B(1,2)|
// = Ax*|B(2,2)| + Az*|B(2,0)| + Bx*dets(1,1) + By*dets(0,1)
//------------------------------------------------------------------------------------------------------------
//
absolutes[2][0] = Fabs(b(2,0));
radius =
axisExtents.x*absolutes[2][2]
+ axisExtents.z*absolutes[2][0]
+ box.axisExtents.x*dets[1][1]
+ box.axisExtents.y*dets[0][1];
projection = distance.x*b(2,2) - distance.z*b(2,0);
if (projection > radius || projection < -radius)
return A1xB2;
//
//------------------------------------------------------------------------------------------------------------
// The math for L = A2xB0 or <0,0,1>xB0:
//
// dets absolutes
// **. ***
// **. ***
// **. ***
//
// projection = (B-A)*(A2xB0) = (B-A)*<-B(0,1),B(0,0),0>
// radius = Ax*|(A2xB0)*A0| + Ay*|(A2xB0)*A1| + Az*|(A2xB0)*A2|
// + Bx*|(A2xB0)*B0| + By*|(A2xB0)*B1| + Bz*|(A2xB0)*B2|
// = Ax*|(A2xB0)*A0| + Ay*|(A2xB0)*A1| + By*|(A2xB0)*B1| + Bz*|(A2xB0)*B2|
// = Ax*|<-B(0,1),B(0,0),0>*A0| + Ay*|<-B(0,1),B(0,0),0>*A1|
// + By*|<-B(0,1),B(0,0),0>*B1| + Bz*|<-B(0,1),B(0,0),0>*B2|
// = Ax*|<-B(0,1),B(0,0),0>*<1,0,0>| + Ay*|<-B(0,1),B(0,0),0>*<0,1,0>|
// + By*|<-B(0,1),B(0,0),0>*<B(1,0),B(1,1),B(1,2)>| + Bz*|<-B(0,1),B(0,0),0>*<B(2,0),B(2,1),B(2,2)>|
// = Ax*|-B(0,1)| + Ay*|B(0,0)|
// + By*|<-B(0,1),B(0,0)>*<B(1,0),B(1,1)>| + Bz*|<-B(0,1),B(0,0)>*<B(2,0),B(2,1)>|
// = Ax*|B(0,1)| + Ay*|B(0,0)|
// + By*|B(0,0)*B(1,1) -B(0,1)*B(1,0)| + Bz*|B(0,0)*B(2,1) - B(0,1)*B(2,0)|
// = Ax*|B(0,1)| + Ay*|B(0,0)| + By*dets(2,2) + Bz*dets(1,2)
//------------------------------------------------------------------------------------------------------------
//
dets[2][2] = Fabs(b(1,1)*b(0,0) - b(0,1)*b(1,0));
dets[1][2] = Fabs(b(2,1)*b(0,0) - b(2,0)*b(0,1));
radius =
axisExtents.x*absolutes[0][1]
+ axisExtents.y*absolutes[0][0]
+ box.axisExtents.y*dets[2][2]
+ box.axisExtents.z*dets[1][2];
projection = distance.y*b(0,0) - distance.x*b(0,1);
if (projection > radius || projection < -radius)
return A2xB0;
//
//------------------------------------------------------------------------------------------------------------
// The math for L = A2xB1 or <0,0,1>xB1:
//
// dets absolutes
// **. ***
// *** ***
// *** ***
//
// projection = (B-A)*(A2xB1) = (B-A)*<-B(1,1),B(1,0),0>
// radius = Ax*|(A2xB1)*A0| + Ay*|(A2xB1)*A1| + Az*|(A2xB1)*A2|
// + Bx*|(A2xB1)*B0| + By*|(A2xB1)*B1| + Bz*|(A2xB1)*B2|
// = Ax*|(A2xB1)*A0| + Ay*|(A2xB1)*A1| + Bx*|(A2xB1)*B0| + Bz*|(A2xB1)*B2|
// = Ax*|<-B(1,1),B(1,0),0>*A0| + Ay*|<-B(1,1),B(1,0),0>*A1|
// + Bx*|<-B(1,1),B(1,0),0>*B0| + Bz*|<-B(1,1),B(1,0),0>*B2|
// = Ax*|<-B(1,1),B(1,0),0>*<1,0,0>| + Ay*|<-B(1,1),B(1,0),0>*<0,1,0>|
// + Bx*|<-B(1,1),B(1,0),0>*<B(0,0),B(0,1),B(0,2)>| + Bz*|<-B(1,1),B(1,0),0>*<B(2,0),B(2,1),B(2,2)>|
// = Ax*|-B(1,1)| + Ay*|B(1,0)|
// + Bx*|<-B(1,1),B(1,0)>*<B(0,0),B(0,1)>| + Bz*|<-B(1,1),B(1,0)>*<B(2,0),B(2,1)>|
// = Ax*|B(1,1)| + Ay*|B(1,0)|
// + Bx*|B(1,0)*B(0,1) - B(1,1)*B(0,0)| + Bz*|B(1,0)*B(2,1) - B(1,1)*B(2,0)|
// = Ax*|B(1,1)| + Ay*|B(1,0)| + Bx*dets(2,2) + Bz*dets(0,2)
//------------------------------------------------------------------------------------------------------------
//
dets[0][2] = Fabs(b(1,0)*b(2,1) - b(2,0)*b(1,1));
radius =
axisExtents.x*absolutes[1][1]
+ axisExtents.y*absolutes[1][0]
+ box.axisExtents.x*dets[2][2]
+ box.axisExtents.z*dets[0][2];
projection = distance.y*b(1,0) - distance.x*b(1,1);
if (projection > radius || projection < -radius)
return A2xB1;
//
//------------------------------------------------------------------------------------------------------------
// The math for L = A2xB2 or <0,0,1>xB2:
//
// dets absolutes
// *** ***
// *** ***
// *** ***
//
// projection = (B-A)*(A2xB2) = (B-A)*<-B(2,1),B(2,0),0>
// radius = Ax*|(A2xB2)*A0| + Ay*|(A2xB2)*A1| + Az*|(A2xB2)*A2|
// + Bx*|(A2xB2)*B0| + By*|(A2xB2)*B1| + Bz*|(A2xB2)*B2|
// = Ax*|(A2xB2)*A0| + Ay*|(A2xB2)*A1| + Bx*|(A2xB2)*B0| + By*|(A2xB2)*B1|
// = Ax*|<-B(2,1),B(2,0),0>*A0| + Ay*|<-B(2,1),B(2,0),0>*A1|
// + Bx*|<-B(2,1),B(2,0),0>*B0| + By*|<-B(2,1),B(2,0),0>*B1|
// = Ax*|<-B(2,1),B(2,0),0>*<1,0,0>| + Ay*|<-B(2,1),B(2,0),0>*<0,1,0>|
// + Bx*|<-B(2,1),B(2,0),0>*<B(0,0),B(0,1),B(0,2)>| + By*|<-B(2,1),B(2,0),0>*<B(1,0),B(1,1),B(1,2)>|
// = Ax*|-B(2,1)| + Ay*|B(2,0)|
// + Bx*|<-B(2,1),B(2,0)>*<B(0,0),B(0,1)>| + By*|<-B(2,1),B(2,0)>*<B(1,0),B(1,1)>|
// = Ax*|B(2,1)| + Ay*|B(2,0)|
// + Bx*|B(2,0)*B(0,1) - B(2,1)*B(0,0)| + By*|B(2,0)*B(1,1) - B(2,1)*B(1,0)|
// = Ax*|B(2,1)| + Ay*|B(2,0)| + Bx*dets(1,2) + By*dets(0,2)
//------------------------------------------------------------------------------------------------------------
//
radius =
axisExtents.x*absolutes[2][1]
+ axisExtents.y*absolutes[2][0]
+ box.axisExtents.x*dets[1][2]
+ box.axisExtents.y*dets[0][2];
projection = distance.y*b(2,0) - distance.x*b(2,1);
if (projection > radius || projection < -radius)
return A2xB2;
return NoSeparation;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
bool OBB::Contains(const Point3D &point) const
{
Check_Pointer(this);
Check_Object(&point);
//
//-------------------------------------
// Translate the point into local space
//-------------------------------------
//
Point3D local;
local.MultiplyByInverse(point, localToParent);
//
//--------------------------
// Compare it to the extents
//--------------------------
//
return Fabs(local.x)<=axisExtents.x && Fabs(local.y)<=axisExtents.y && Fabs(local.z)<=axisExtents.z;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void OBB::Union(
const OBB &first,
const OBB &second
)
{
Check_Pointer(this);
Check_Object(&first);
Check_Object(&second);
//
//-------------------------------------------------
// Learn about the spheres surrounding the two OBBs
//-------------------------------------------------
//
Point3D c1(first.localToParent);
Point3D c2(second.localToParent);
Vector3D diff;
diff.Subtract(c2, c1);
Scalar len = diff.GetLength();
//
//---------------------------------------------------
// See if the first sphere is contained in the second
//---------------------------------------------------
//
if (len+first.sphereRadius <= second.sphereRadius || !first.sphereRadius)
{
if (this != &second)
*this = second;
return;
}
//
//---------------------------------------------------
// See if the second sphere is contained in the first
//---------------------------------------------------
//
if (len+second.sphereRadius <= first.sphereRadius || !second.sphereRadius)
{
if (this != &first)
*this = first;
return;
}
//
//-------------------------------------------------------
// The new sphere will lie somewhere between the old ones
//-------------------------------------------------------
//
localToParent = LinearMatrix4D::Identity;
axisExtents = Vector3D::Identity;
c1.Lerp(c1, c2, (len + second.sphereRadius - first.sphereRadius) / (2.0f*len));
localToParent.BuildTranslation(c1);
sphereRadius = 0.5f * (len + first.sphereRadius + second.sphereRadius);
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
OBB::SeparatingAxis OBB::CalculateSeparationVector(
Vector3D *vector,
const OBB &box
)
{
Check_Object(this);
Check_Pointer(vector);
Check_Object(&box);
//
//-----------------------------------------------------------------------
// We use the same logic as in detecting separation, but since we know
// the boxes are already intersecting, we will look to see which axis
// separates the boxes by the least space, then we will nudge this box by
// that amount. We will initially only concern ourselves with the axes
// based on the face normals
//-----------------------------------------------------------------------
//
Verify(axisExtents.x >= 0.0f);
Verify(axisExtents.y >= 0.0f);
Verify(axisExtents.z >= 0.0f);
Verify(box.axisExtents.x >= 0.0f);
Verify(box.axisExtents.y >= 0.0f);
Verify(box.axisExtents.z >= 0.0f);
//
//----------------------------------------------------------------------
// Get the distance between the centerpoints. Both boxes are assumed to
// convert their local axes into a common parent space
//----------------------------------------------------------------------
//
Check_Object(&localToParent);
Check_Object(&box.localToParent);
Point3D
distance(
localToParent(3,0) - box.localToParent(3,0),
localToParent(3,1) - box.localToParent(3,1),
localToParent(3,2) - box.localToParent(3,2)
);
//
//--------------------------------------------------------------------------------------------------
// The rows of the matrix encode the local axes in world space. The first
// separation axis we will be testing our x axis in world space, so
// project the other box's primary axes onto it, and store the results
// away for future reference
//
// The math for L = A0: (A0 is the X axis row of our matrix)
// Given: A0,A1, and B2 are orthogonal
// B0,B1, and B2 are orthogonal
// Ax, Ay, and Az >= 0
// Bx, By, and Bz >= 0
//
// projection = (B-A)*A0
// radius = |Ax*(A0*A0)| + |Ay*(A0*A1)| + |Az*(A0*A2)| + |Bx*(A0*B0)| + |By*(A0*B1)| + |Bz*(A0*B2)|
// = Ax + Bx*|A0*B0| + By*|A0*B1| + Bz*|A0*B2|
//--------------------------------------------------------------------------------------------------
//
Scalar axis_projections[3][3];
axis_projections[X_Axis][X_Axis] =
Fabs(
localToParent(X_Axis,0) * box.localToParent(X_Axis,0)
+ localToParent(X_Axis,1) * box.localToParent(X_Axis,1)
+ localToParent(X_Axis,2) * box.localToParent(X_Axis,2)
);
axis_projections[X_Axis][Y_Axis] =
Fabs(
localToParent(X_Axis,0) * box.localToParent(Y_Axis,0)
+ localToParent(X_Axis,1) * box.localToParent(Y_Axis,1)
+ localToParent(X_Axis,2) * box.localToParent(Y_Axis,2)
);
axis_projections[X_Axis][Z_Axis] =
Fabs(
localToParent(X_Axis,0) * box.localToParent(Z_Axis,0)
+ localToParent(X_Axis,1) * box.localToParent(Z_Axis,1)
+ localToParent(X_Axis,2) * box.localToParent(Z_Axis,2)
);
//
//-----------------------------------------------------------------------
// The maximum distance the centerpoints can be away and still touch is
// equal to the sum of our x extent plus the other's extents, as modified
// by the projection values. If the boxes can't touch, return A0 as the
// separating plane
//-----------------------------------------------------------------------
//
Scalar radius =
axisExtents.x
+ box.axisExtents.x * axis_projections[X_Axis][0]
+ box.axisExtents.y * axis_projections[X_Axis][1]
+ box.axisExtents.z * axis_projections[X_Axis][2];
Scalar projection =
localToParent(X_Axis,0) * distance.x
+ localToParent(X_Axis,1) * distance.y
+ localToParent(X_Axis,2) * distance.z;
radius -= Fabs(projection);
Scalar best_radius = radius;
radius += SMALL;
Verify(radius > 0.0f);
if (projection<0.0f)
radius = -radius;
vector->x = localToParent(0,0) * radius;
vector->y = localToParent(0,1) * radius;
vector->z = localToParent(0,2) * radius;
SeparatingAxis axis = A0;
//
//--------------------------------------------------------------------------------------------------
// Do the same check against our Y axis
//
// The math for L = A1:
//
// projection = (B-A)*A1
// radius = |Ax*(A1*A0)| + |Ay*(A1*A1)| + |Az*(A1*A2)| + |Bx*(A1*B0)| + |By*(A1*B1)| + |Bz*(A1*B2)|
// = Ay + Bx*|A1*B0| + By*|A1*B1| + Bz*|A1*B2|
//--------------------------------------------------------------------------------------------------
//
axis_projections[Y_Axis][X_Axis] =
Fabs(
localToParent(Y_Axis,0) * box.localToParent(X_Axis,0)
+ localToParent(Y_Axis,1) * box.localToParent(X_Axis,1)
+ localToParent(Y_Axis,2) * box.localToParent(X_Axis,2)
);
axis_projections[Y_Axis][Y_Axis] =
Fabs(
localToParent(Y_Axis,0) * box.localToParent(Y_Axis,0)
+ localToParent(Y_Axis,1) * box.localToParent(Y_Axis,1)
+ localToParent(Y_Axis,2) * box.localToParent(Y_Axis,2)
);
axis_projections[Y_Axis][Z_Axis] =
Fabs(
localToParent(Y_Axis,0) * box.localToParent(Z_Axis,0)
+ localToParent(Y_Axis,1) * box.localToParent(Z_Axis,1)
+ localToParent(Y_Axis,2) * box.localToParent(Z_Axis,2)
);
radius =
axisExtents.y
+ box.axisExtents.x * axis_projections[1][0]
+ box.axisExtents.y * axis_projections[1][1]
+ box.axisExtents.z * axis_projections[1][2];
projection =
localToParent(1,0) * distance.x
+ localToParent(1,1) * distance.y
+ localToParent(1,2) * distance.z;
radius -= Fabs(projection);
if (radius < best_radius)
{
best_radius = radius;
radius += SMALL;
Verify(radius > 0.0f);
if (projection<0.0f)
radius = -radius;
vector->x = localToParent(1,0) * radius;
vector->y = localToParent(1,1) * radius;
vector->z = localToParent(1,2) * radius;
axis = A1;
}
//
//--------------------------------------------------------------------------------------------------
// Check our z axis
//
// The math for L = A2:
//
// projection = (B-A)*A2
// radius = |Ax*(A2*A0)| + |Ay*(A2*A1)| + |Az*(A2*A2)| + |Bx*(A2*B0)| + |By*(A2*B1)| + |Bz*(A2*B2)|
// = Az + Bx*|A2*B0| + By*|A2*B1| + Bz*|A2*B2|
//--------------------------------------------------------------------------------------------------
//
axis_projections[Z_Axis][X_Axis] =
Fabs(
localToParent(Z_Axis,0) * box.localToParent(X_Axis,0)
+ localToParent(Z_Axis,1) * box.localToParent(X_Axis,1)
+ localToParent(Z_Axis,2) * box.localToParent(X_Axis,2)
);
axis_projections[Z_Axis][Y_Axis] =
Fabs(
localToParent(Z_Axis,0) * box.localToParent(Y_Axis,0)
+ localToParent(Z_Axis,1) * box.localToParent(Y_Axis,1)
+ localToParent(Z_Axis,2) * box.localToParent(Y_Axis,2)
);
axis_projections[Z_Axis][Z_Axis] =
Fabs(
localToParent(Z_Axis,0) * box.localToParent(Z_Axis,0)
+ localToParent(Z_Axis,1) * box.localToParent(Z_Axis,1)
+ localToParent(Z_Axis,2) * box.localToParent(Z_Axis,2)
);
radius =
axisExtents.z
+ box.axisExtents.x * axis_projections[2][0]
+ box.axisExtents.y * axis_projections[2][1]
+ box.axisExtents.z * axis_projections[2][2];
projection =
localToParent(2,0) * distance.x
+ localToParent(2,1) * distance.y
+ localToParent(2,2) * distance.z;
radius -= Fabs(projection);
if (radius < best_radius)
{
best_radius = radius;
radius += SMALL;
Verify(radius > 0.0f);
if (projection<0.0f)
radius = -radius;
vector->x = localToParent(2,0) * radius;
vector->y = localToParent(2,1) * radius;
vector->z = localToParent(2,2) * radius;
axis = A2;
}
//
//--------------------------------------------------------------------------------------------------
// See if the other box's X axis is a separator
//
// The math for L = B0:
//
// projection = (B-A)*B0
// radius = |Ax*(B0*A0)| + |Ay*(B0*A1)| + |Az*(B0*A2)| + |Bx*(B0*B0)| + |By*(B0*B1)| + |Bz*(B0*B2)|
// = Bx + Ax*|B0*A0| + Ay*|B0*A1| + Az*|B0*A2|
//--------------------------------------------------------------------------------------------------
//
radius =
box.axisExtents.x
+ axisExtents.x * axis_projections[0][X_Axis]
+ axisExtents.y * axis_projections[1][X_Axis]
+ axisExtents.z * axis_projections[2][X_Axis];
projection =
box.localToParent(X_Axis,0) * distance.x
+ box.localToParent(X_Axis,1) * distance.y
+ box.localToParent(X_Axis,2) * distance.z;
radius -= Fabs(projection);
if (radius < best_radius)
{
best_radius = radius;
radius += SMALL;
Verify(radius > 0.0f);
if (projection<0.0f)
radius = -radius;
vector->x = box.localToParent(0,0) * radius;
vector->y = box.localToParent(0,1) * radius;
vector->z = box.localToParent(0,2) * radius;
axis = B0;
}
//
//--------------------------------------------------------------------------------------------------
// Check its Y axis
//
// The math for L = B1:
//
// projection = (B-A)*B1
// radius = |Ax*(B1*A0)| + |Ay*(B1*A1)| + |Az*(B1*A2)| + |Bx*(B1*B0)| + |By*(B1*B1)| + |Bz*(B1*B2)|
// = By + Ax*|B1*A0| + Ay*|B1*A1| + Az*|B1*A2|
//--------------------------------------------------------------------------------------------------
//
radius =
box.axisExtents.y
+ axisExtents.x * axis_projections[0][Y_Axis]
+ axisExtents.y * axis_projections[1][Y_Axis]
+ axisExtents.z * axis_projections[2][Y_Axis];
projection =
box.localToParent(Y_Axis,0) * distance.x
+ box.localToParent(Y_Axis,1) * distance.y
+ box.localToParent(Y_Axis,2) * distance.z;
radius -= Fabs(projection);
if (radius < best_radius)
{
best_radius = radius;
radius += SMALL;
Verify(radius > 0.0f);
if (projection<0.0f)
radius = -radius;
vector->x = box.localToParent(1,0) * radius;
vector->y = box.localToParent(1,1) * radius;
vector->z = box.localToParent(1,2) * radius;
axis = B1;
}
//
//--------------------------------------------------------------------------------------------------
// Check its Z axis
//
// The math for L = B2:
//
// projection = (B-A)*B2
// radius = |Ax*(B2*A0)| + |Ay*(B2*A1)| + |Az*(B2*A2)| + |Bx*(B2*B0)| + |By*(B2*B1)| + |Bz*(B2*B2)|
// = Bz + Ax*|B2*A0| + Ay*|B2*A1| + Az*|B2*A2|
//--------------------------------------------------------------------------------------------------
//
radius =
box.axisExtents.z
+ axisExtents.x * axis_projections[0][Z_Axis]
+ axisExtents.y * axis_projections[1][Z_Axis]
+ axisExtents.z * axis_projections[2][Z_Axis];
projection =
box.localToParent(Z_Axis,0) * distance.x
+ box.localToParent(Z_Axis,1) * distance.y
+ box.localToParent(Z_Axis,2) * distance.z;
radius -= Fabs(projection);
if (radius < best_radius)
{
best_radius = radius;
radius += SMALL;
Verify(radius > 0.0f);
if (projection<0.0f)
radius = -radius;
vector->x = box.localToParent(2,0) * radius;
vector->y = box.localToParent(2,1) * radius;
vector->z = box.localToParent(2,2) * radius;
axis = B2;
}
return axis;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void OBB::CalculateSecondarySeparationVector(
Vector3D *vector,
const OBB &box,
const UnitVector3D &primary
)
{
Check_Object(this);
Check_Pointer(vector);
Check_Object(&box);
//
//-----------------------------------------------------------------------
// We use the same logic as in detecting separation, but since we know
// the boxes are already intersecting, we will look to see which axis
// separates the boxes by the least space, then we will nudge this box by
// that amount. We will initially only concern ourselves with the axes
// based on the face normals
//-----------------------------------------------------------------------
//
Verify(axisExtents.x >= 0.0f);
Verify(axisExtents.y >= 0.0f);
Verify(axisExtents.z >= 0.0f);
Verify(box.axisExtents.x >= 0.0f);
Verify(box.axisExtents.y >= 0.0f);
Verify(box.axisExtents.z >= 0.0f);
//
//----------------------------------------------------------------------
// Get the distance between the centerpoints. Both boxes are assumed to
// convert their local axes into a common parent space
//----------------------------------------------------------------------
//
Check_Object(&localToParent);
Check_Object(&box.localToParent);
Point3D
distance(
localToParent(3,0) - box.localToParent(3,0),
localToParent(3,1) - box.localToParent(3,1),
localToParent(3,2) - box.localToParent(3,2)
);
//
//--------------------------------------------------------------------------------------------------
// The rows of the matrix encode the local axes in world space. The first
// separation axis we will be testing our x axis in world space, so
// project the other box's primary axes onto it, and store the results
// away for future reference
//
// The math for L = A0: (A0 is the X axis row of our matrix)
// Given: A0,A1, and B2 are orthogonal
// B0,B1, and B2 are orthogonal
// Ax, Ay, and Az >= 0
// Bx, By, and Bz >= 0
//
// projection = (B-A)*A0
// radius = |Ax*(A0*A0)| + |Ay*(A0*A1)| + |Az*(A0*A2)| + |Bx*(A0*B0)| + |By*(A0*B1)| + |Bz*(A0*B2)|
// = Ax + Bx*|A0*B0| + By*|A0*B1| + Bz*|A0*B2|
//--------------------------------------------------------------------------------------------------
//
Scalar axis_projections[3][3];
axis_projections[X_Axis][X_Axis] =
Fabs(
localToParent(X_Axis,0) * box.localToParent(X_Axis,0)
+ localToParent(X_Axis,1) * box.localToParent(X_Axis,1)
+ localToParent(X_Axis,2) * box.localToParent(X_Axis,2)
);
axis_projections[X_Axis][Y_Axis] =
Fabs(
localToParent(X_Axis,0) * box.localToParent(Y_Axis,0)
+ localToParent(X_Axis,1) * box.localToParent(Y_Axis,1)
+ localToParent(X_Axis,2) * box.localToParent(Y_Axis,2)
);
axis_projections[X_Axis][Z_Axis] =
Fabs(
localToParent(X_Axis,0) * box.localToParent(Z_Axis,0)
+ localToParent(X_Axis,1) * box.localToParent(Z_Axis,1)
+ localToParent(X_Axis,2) * box.localToParent(Z_Axis,2)
);
//
//-----------------------------------------------------------------------
// The maximum distance the centerpoints can be away and still touch is
// equal to the sum of our x extent plus the other's extents, as modified
// by the projection values. If the boxes can't touch, return A0 as the
// separating plane
//-----------------------------------------------------------------------
//
Scalar radius =
axisExtents.x
+ box.axisExtents.x * axis_projections[X_Axis][0]
+ box.axisExtents.y * axis_projections[X_Axis][1]
+ box.axisExtents.z * axis_projections[X_Axis][2];
Scalar projection =
localToParent(X_Axis,0) * distance.x
+ localToParent(X_Axis,1) * distance.y
+ localToParent(X_Axis,2) * distance.z;
Scalar primary_dot =
Fabs(localToParent(X_Axis,0) * primary.x
+ localToParent(X_Axis,1) * primary.y
+ localToParent(X_Axis,2) * primary.z);
radius -= Fabs(projection);
Verify(radius > 0.0f);
Scalar best_radius = radius;
radius += SMALL;
if (projection<0.0f)
radius = -radius;
vector->x = localToParent(0,0) * radius;
vector->y = localToParent(0,1) * radius;
vector->z = localToParent(0,2) * radius;
//
//--------------------------------------------------------------------------------------------------
// Do the same check against our Y axis
//
// The math for L = A1:
//
// projection = (B-A)*A1
// radius = |Ax*(A1*A0)| + |Ay*(A1*A1)| + |Az*(A1*A2)| + |Bx*(A1*B0)| + |By*(A1*B1)| + |Bz*(A1*B2)|
// = Ay + Bx*|A1*B0| + By*|A1*B1| + Bz*|A1*B2|
//--------------------------------------------------------------------------------------------------
//
axis_projections[Y_Axis][X_Axis] =
Fabs(
localToParent(Y_Axis,0) * box.localToParent(X_Axis,0)
+ localToParent(Y_Axis,1) * box.localToParent(X_Axis,1)
+ localToParent(Y_Axis,2) * box.localToParent(X_Axis,2)
);
axis_projections[Y_Axis][Y_Axis] =
Fabs(
localToParent(Y_Axis,0) * box.localToParent(Y_Axis,0)
+ localToParent(Y_Axis,1) * box.localToParent(Y_Axis,1)
+ localToParent(Y_Axis,2) * box.localToParent(Y_Axis,2)
);
axis_projections[Y_Axis][Z_Axis] =
Fabs(
localToParent(Y_Axis,0) * box.localToParent(Z_Axis,0)
+ localToParent(Y_Axis,1) * box.localToParent(Z_Axis,1)
+ localToParent(Y_Axis,2) * box.localToParent(Z_Axis,2)
);
radius =
axisExtents.y
+ box.axisExtents.x * axis_projections[1][0]
+ box.axisExtents.y * axis_projections[1][1]
+ box.axisExtents.z * axis_projections[1][2];
projection =
localToParent(Y_Axis,0) * distance.x
+ localToParent(Y_Axis,1) * distance.y
+ localToParent(Y_Axis,2) * distance.z;
Scalar dot =
Fabs(localToParent(Y_Axis,0) * primary.x
+ localToParent(Y_Axis,1) * primary.y
+ localToParent(Y_Axis,2) * primary.z);
radius -= Fabs(projection);
Verify(radius > 0.0f);
if (dot<0.85f && (radius < best_radius || primary_dot<0.85f))
{
best_radius = radius;
radius += SMALL;
if (projection<0.0f)
radius = -radius;
vector->x = localToParent(1,0) * radius;
vector->y = localToParent(1,1) * radius;
vector->z = localToParent(1,2) * radius;
}
else
Verify(primary_dot < 0.85f);
//
//--------------------------------------------------------------------------------------------------
// Check our z axis
//
// The math for L = A2:
//
// projection = (B-A)*A2
// radius = |Ax*(A2*A0)| + |Ay*(A2*A1)| + |Az*(A2*A2)| + |Bx*(A2*B0)| + |By*(A2*B1)| + |Bz*(A2*B2)|
// = Az + Bx*|A2*B0| + By*|A2*B1| + Bz*|A2*B2|
//--------------------------------------------------------------------------------------------------
//
axis_projections[Z_Axis][X_Axis] =
Fabs(
localToParent(Z_Axis,0) * box.localToParent(X_Axis,0)
+ localToParent(Z_Axis,1) * box.localToParent(X_Axis,1)
+ localToParent(Z_Axis,2) * box.localToParent(X_Axis,2)
);
axis_projections[Z_Axis][Y_Axis] =
Fabs(
localToParent(Z_Axis,0) * box.localToParent(Y_Axis,0)
+ localToParent(Z_Axis,1) * box.localToParent(Y_Axis,1)
+ localToParent(Z_Axis,2) * box.localToParent(Y_Axis,2)
);
axis_projections[Z_Axis][Z_Axis] =
Fabs(
localToParent(Z_Axis,0) * box.localToParent(Z_Axis,0)
+ localToParent(Z_Axis,1) * box.localToParent(Z_Axis,1)
+ localToParent(Z_Axis,2) * box.localToParent(Z_Axis,2)
);
radius =
axisExtents.z
+ box.axisExtents.x * axis_projections[2][0]
+ box.axisExtents.y * axis_projections[2][1]
+ box.axisExtents.z * axis_projections[2][2];
projection =
localToParent(2,0) * distance.x
+ localToParent(2,1) * distance.y
+ localToParent(2,2) * distance.z;
dot =
Fabs(localToParent(Z_Axis,0) * primary.x
+ localToParent(Z_Axis,1) * primary.y
+ localToParent(Z_Axis,2) * primary.z);
radius -= Fabs(projection);
Verify(radius > 0.0f);
if (dot<0.85f && radius < best_radius)
{
best_radius = radius;
radius += SMALL;
if (projection<0.0f)
radius = -radius;
vector->x = localToParent(2,0) * radius;
vector->y = localToParent(2,1) * radius;
vector->z = localToParent(2,2) * radius;
}
//
//--------------------------------------------------------------------------------------------------
// See if the other box's X axis is a separator
//
// The math for L = B0:
//
// projection = (B-A)*B0
// radius = |Ax*(B0*A0)| + |Ay*(B0*A1)| + |Az*(B0*A2)| + |Bx*(B0*B0)| + |By*(B0*B1)| + |Bz*(B0*B2)|
// = Bx + Ax*|B0*A0| + Ay*|B0*A1| + Az*|B0*A2|
//--------------------------------------------------------------------------------------------------
//
radius =
box.axisExtents.x
+ axisExtents.x * axis_projections[0][X_Axis]
+ axisExtents.y * axis_projections[1][X_Axis]
+ axisExtents.z * axis_projections[2][X_Axis];
projection =
box.localToParent(X_Axis,0) * distance.x
+ box.localToParent(X_Axis,1) * distance.y
+ box.localToParent(X_Axis,2) * distance.z;
dot =
Fabs(box.localToParent(X_Axis,0) * primary.x
+ box.localToParent(X_Axis,1) * primary.y
+ box.localToParent(X_Axis,2) * primary.z);
radius -= Fabs(projection);
Verify(radius > 0.0f);
if (dot<0.85f && radius < best_radius)
{
best_radius = radius;
radius += SMALL;
if (projection<0.0f)
radius = -radius;
vector->x = box.localToParent(0,0) * radius;
vector->y = box.localToParent(0,1) * radius;
vector->z = box.localToParent(0,2) * radius;
}
//
//--------------------------------------------------------------------------------------------------
// Check its Y axis
//
// The math for L = B1:
//
// projection = (B-A)*B1
// radius = |Ax*(B1*A0)| + |Ay*(B1*A1)| + |Az*(B1*A2)| + |Bx*(B1*B0)| + |By*(B1*B1)| + |Bz*(B1*B2)|
// = By + Ax*|B1*A0| + Ay*|B1*A1| + Az*|B1*A2|
//--------------------------------------------------------------------------------------------------
//
radius =
box.axisExtents.y
+ axisExtents.x * axis_projections[0][Y_Axis]
+ axisExtents.y * axis_projections[1][Y_Axis]
+ axisExtents.z * axis_projections[2][Y_Axis];
projection =
box.localToParent(Y_Axis,0) * distance.x
+ box.localToParent(Y_Axis,1) * distance.y
+ box.localToParent(Y_Axis,2) * distance.z;
dot =
Fabs(box.localToParent(Y_Axis,0) * primary.x
+ box.localToParent(Y_Axis,1) * primary.y
+ box.localToParent(Y_Axis,2) * primary.z);
radius -= Fabs(projection);
Verify(radius > 0.0f);
if (dot<0.85f && radius < best_radius)
{
best_radius = radius;
radius += SMALL;
if (projection<0.0f)
radius = -radius;
vector->x = box.localToParent(1,0) * radius;
vector->y = box.localToParent(1,1) * radius;
vector->z = box.localToParent(1,2) * radius;
}
//
//--------------------------------------------------------------------------------------------------
// Check its Z axis
//
// The math for L = B2:
//
// projection = (B-A)*B2
// radius = |Ax*(B2*A0)| + |Ay*(B2*A1)| + |Az*(B2*A2)| + |Bx*(B2*B0)| + |By*(B2*B1)| + |Bz*(B2*B2)|
// = Bz + Ax*|B2*A0| + Ay*|B2*A1| + Az*|B2*A2|
//--------------------------------------------------------------------------------------------------
//
radius =
box.axisExtents.z
+ axisExtents.x * axis_projections[0][Z_Axis]
+ axisExtents.y * axis_projections[1][Z_Axis]
+ axisExtents.z * axis_projections[2][Z_Axis];
projection =
box.localToParent(Z_Axis,0) * distance.x
+ box.localToParent(Z_Axis,1) * distance.y
+ box.localToParent(Z_Axis,2) * distance.z;
dot =
Fabs(box.localToParent(Z_Axis,0) * primary.x
+ box.localToParent(Z_Axis,1) * primary.y
+ box.localToParent(Z_Axis,2) * primary.z);
radius -= Fabs(projection);
Verify(radius > 0.0f);
if (dot<0.85f && radius < best_radius)
{
best_radius = radius;
radius += SMALL;
if (projection<0.0f)
radius = -radius;
vector->x = box.localToParent(2,0) * radius;
vector->y = box.localToParent(2,1) * radius;
vector->z = box.localToParent(2,2) * radius;
}
}
Reference in New Issue View Git Blame Copy Permalink