www.pudn.com > Gimcrack-v0.0051-Source.zip > obb.cpp
#include
#include
#include "obb.h"
#include "../math/math.h"
#include "../math/plane.h"
#include "../math/triangle.h"
#include "../global.h"
#include
GcOBB::GcOBB()
{
m_orthogonal[0] = GcVector3(1, 0, 0);
m_orthogonal[1] = GcVector3(0, 1, 0);
m_orthogonal[2] = GcVector3(0, 0, 1);
}
GcOBB::GcOBB( GcVector3 & extents )
{
m_extents = extents;
}
GcOBB::GcOBB( GcVector3 & extents, GcVector3 & position )
{
m_extents = extents;
SetTranslation(position);
}
void GcOBB::RenderOutlines()
{
glPushMatrix();
/*glTranslatef( GetWorldTranslation().x ,
GetWorldTranslation().y,
GetWorldTranslation().z);*/
Matrix4 tmp = GetWorldMatrix();
//tmp.SetIdentity();
//tmp.SetTranslation( GetWorldMatrix().GetTranslation() );
glMultMatrixf(tmp);
glLineWidth(2);
glColor3f(0.5f, 0.0f, 0.0f);
glBegin(GL_LINES);
// Front top
glVertex3f(-m_extents.x, m_extents.y, -m_extents.z);
glVertex3f(m_extents.z, m_extents.y, -m_extents.z);
// Back top
glVertex3f(-m_extents.x, m_extents.y, m_extents.z );
glVertex3f(m_extents.z, m_extents.y, m_extents.z);
// Left top
glVertex3f(-m_extents.x, m_extents.y, -m_extents.z);
glVertex3f(-m_extents.x, m_extents.y, m_extents.z );
// Right top
glVertex3f(m_extents.x, m_extents.y, -m_extents.z);
glVertex3f(m_extents.x, m_extents.y, m_extents.z );
// Front bottom
glVertex3f(-m_extents.x, -m_extents.y, -m_extents.z);
glVertex3f(m_extents.z, -m_extents.y, -m_extents.z);
// Back bottom
glVertex3f(-m_extents.x, -m_extents.y, m_extents.z );
glVertex3f(m_extents.z, -m_extents.y, m_extents.z);
// Left bottom
glVertex3f(-m_extents.x, -m_extents.y, -m_extents.z);
glVertex3f(-m_extents.x, -m_extents.y, m_extents.z );
// Right bottom
glVertex3f(m_extents.x, -m_extents.y, -m_extents.z);
glVertex3f(m_extents.x, -m_extents.y, m_extents.z );
// Front left
glVertex3f(-m_extents.x, m_extents.y, -m_extents.z);
glVertex3f(-m_extents.x, -m_extents.y, -m_extents.z);
// Front right
glVertex3f(m_extents.z, m_extents.y, -m_extents.z);
glVertex3f(m_extents.z, -m_extents.y, -m_extents.z);
// Back left
glVertex3f(-m_extents.x, m_extents.y, m_extents.z);
glVertex3f(-m_extents.x, -m_extents.y, m_extents.z);
// Back right
glVertex3f(m_extents.z, m_extents.y, m_extents.z);
glVertex3f(m_extents.z, -m_extents.y, m_extents.z);
glEnd();
glLineWidth(1);
glPopMatrix();
}
const bool GcOBB::Intersects(const GcPlane & plane)
{
glPushMatrix();
GcVector3 point(400.0f, 520.0f, 500.0f);
glTranslatef( point.x, point.y, point.z );
glPointSize(5);
glColor3f(0.5f, 0.0f, 0.0f);
glBegin(GL_POINTS);
//glVertex3fv( point );
glVertex3f(0.0f, 0.0f, 0.0f);
//glVertex3f(500.0f, 0.0f, 0.0f);
glEnd();
GcVector3 normal(0, 1, 0);
normal *= 30;
glColor3f(0.5f, 0.0f, 0.0f);
glBegin(GL_LINES);
//glVertex3fv( point );
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3fv(normal);
//glVertex3f(500.0f, 0.0f, 0.0f);
glEnd();
glPopMatrix();
GcVector3 & extents = m_extents;
GcVector3 & xAxis = m_worldMatrix.Axis(0);
GcVector3 & yAxis = m_worldMatrix.Axis(1);
GcVector3 & zAxis = m_worldMatrix.Axis(2);
/* float r = ( ( normal.DotProduct(xAxis) ) * extents.x ) + ( (normal.DotProduct(yAxis)) * extents.y ) + ( (normal.DotProduct(zAxis)) * extents.z );
//GcVector3 PC = point - GetWorldTranslation();
float distance = plane.Distance( GetWorldTranslation() );
distance = fabs(distance);
//float distance = plane.Distance( GcVector3(0, 0 ,0) );
*/
GcVector3 distance;
distance.x = extents.x * ( plane.m_normal * xAxis );
distance.y = extents.y * ( plane.m_normal * yAxis );
distance.z = extents.z * ( plane.m_normal * zAxis );
float radius = fabs( distance.x ) + fabs( distance.y ) + fabs( distance.z );
float distanceCenterToPlane = plane.Distance( GetWorldTranslation() );
//dbgC.Write("Distance to plane: %f\n", distanceCenterToPlane);
//dbgC.Write("radius: %f\n", radius);
if( fabs(distanceCenterToPlane) <= radius )
return true;
/*
NeMatrix kRotMat = GetWorldRotation().ToMatrix();
NeVector3d kDist( m_kDim.x * ( rkPlane.m_kNormal * kRotMat.GetColumn(0) ),
m_kDim.y * ( rkPlane.m_kNormal * kRotMat.GetColumn(1) ),
m_kDim.z * ( rkPlane.m_kNormal * kRotMat.GetColumn(2) ) );
float fRadius = fabs( kDist.x ) + fabs( kDist.y ) + fabs( kDist.z );
float fCenterDistToPlane = rkPlane.Distance( GetWorldPosition() );
return( fabs( fCenterDistToPlane ) <= fRadius );
*/
glPointSize(1);
return false;
}
const bool GcOBB::Intersects(const GcTriangle & triangle)
{
// A total 13 axes will be tested
// 1) x1 The normal of the triangle
// 2) x3 The faces of the OBB
// 3) x9 The cross products of the triangle - box edges
float triMin, triMax, boxMin, boxMax;
GcVector3 A; // Will hold a given arbitrary axis
GcVector3 triEdge[3]; // Will hold the edges of the triangle
// Compute triangle edges
triEdge[0] = triangle.b - triangle.a;
triEdge[1] = triangle.c - triangle.a;
// Test direction of triangle normal
A = triangle.Normal();
triMin = A.DotProduct( triangle.a );
triMax = triMin;
GcMath::ProjectOBB(A, *this, boxMin, boxMax);
// The projected box and triangle do not overlap on the A axis, return negative
if( boxMax < triMin || triMax < boxMin )
return false;
int i;
GcVector3 center = GetWorldTranslation();
// Test the directions of the faces
for( i = 0; i < 3; i++ )
{
A = this->Axis(i);
GcMath::ProjectTriangle( A, triangle, triMin, triMax );
float centerOnA = A.DotProduct( center );
boxMin = centerOnA - this->Extent(i);
boxMax = centerOnA + this->Extent(i);
if( boxMax < triMin || triMax < boxMin )
return false;
}
// Test the cross products of the triangle - box edges
triEdge[2] = triEdge[1] - triEdge[0];
for( i = 0; i < 3; i++ )
{
for( int j = 0; j < 3; j++)
{
A = triEdge[i].CrossProduct( this->Axis(j) );
GcMath::ProjectTriangle( A, triangle, triMin, triMax );
GcMath::ProjectOBB( A, *this, boxMin, boxMax );
if( boxMax < triMin || triMax < boxMin )
return false;
}
}
return true;
}
const bool GcOBB::Intersects( GcAABB & box )
{
return Intersects( GcOBB( box.GetExtents(), box.GetWorldTranslation() ) );
}
const bool GcOBB::Intersects( GcOBB & box )
{
float R[3][3];
float ra, rb, t;
long i, k;
GcVector3 v = box.GetWorldTranslation() - GetWorldTranslation();
// Translation to B with A as reference
GcVector3 T( v.DotProduct( m_worldMatrix.Axis( 0 ) ), v.DotProduct( m_worldMatrix.Axis( 1 ) ), v.DotProduct( m_worldMatrix.Axis( 2 ) ) );
//B's basis with respect to A's local frame
GcVector3 & extentsA = m_extents;
GcVector3 & extentsB = box.Extents();
// Calculate rotation matrix
/*for( i = 0; i < 3; ++i )
for( k = 0; k < 3; ++k )
R[i][k] = m_orthogonal[i].DotProduct(box->GetOrthogonal()[k]);*/
for( i = 0; i < 3; ++i )
{
for( k = 0; k < 3; ++k )
{
GcVector3 & A = m_worldMatrix.Axis(i);
GcVector3 & B = box.GetWorldMatrix().Axis(k);
//g_Debug->Log("Axis %d %d - %f %f %f\n", i, k, A.x, A.y, A.z);
//g_Debug->Log("Axis %d %d - %f %f %f\n", i, k, B.x, B.y, B.z);
//A = m_worldMatrix.GetAxis(i);
//B = box->GetWorldMatrix().GetAxis(k);
R[i][k] = A.DotProduct( B );
//g_Debug->Log("---------\n");
}
}
Matrix3 mat, matA, matB;
/*mat = box->GetWorldMatrix();*/
matA = m_worldMatrix;
matB = box.GetWorldMatrix();
/*
//mat.SetTranslation( GcVector3(0, 0, 0) );
// It's a rotation matrix - no need do the whole matrix inverse calculations**/
mat.Transpose();
mat = matA * mat;
for( i = 0; i < 3; i++ )
{
ra = m_extents.array[i];
//rb = box->Extents().array[0] * fabs(R[i][0]) + box->Extents().array[1] * fabs(R[i][1]) + box->Extents().array[2] * fabs(R[i][2]);
rb = box.Extent(0) * fabs(R[i][0]) + box.Extent(1) * fabs(R[i][1]) + box.Extent(2) * fabs(R[i][2]);
t = fabs( T.array[i] );
if( t > ra + rb )
{
return false;
}
}
// Test using the other box's axes
for( k = 0; k < 3; k++ )
{
ra = m_extents.array[0] * fabs(R[0][k]) + m_extents.array[1] * fabs(R[1][k]) + m_extents.array[2] * fabs(R[2][k]);
rb = box.Extents().array[k];
t = fabs( T.array[0]*R[0][k] + T.array[1]*R[1][k] + T.array[2]*R[2][k] );
if( t > ra + rb )
{
return false;
}
}
// L = A0 x B0
ra = extentsA.y * mat.a20 + extentsA.z * mat.a10;
rb = extentsB.y * mat.a02 + extentsB.z * mat.a01;
t = fabs( T.z * mat.a10 - T.y * mat.a20 );
if( t > ra + rb )
{
g_DebugConsole->Write("1\n");
return false;
}
// L = A0 x B1
ra = extentsA.y * mat.a21 + extentsA.z * mat.a11;
rb = extentsB.x * mat.a02 + extentsB.z * mat.a00;
t = fabs( T.z * mat.a11 - T.y * mat.a21 );
if( t > ra + rb )
{
g_DebugConsole->Write("2\n");
return false;
}
// L = A0 x B2
ra = extentsA.y * mat.a22 + extentsA.z * mat.a12;
rb = extentsB.x * mat.a01 + extentsB.y * mat.a00;
t = fabs( T.z * mat.a12 - T.y * mat.a22 );
if( t > ra + rb )
{
g_DebugConsole->Write("3\n");
return false;
}
// L = A1 x B0
ra = extentsA.x * mat.a20 + extentsA.z * mat.a00;
rb = extentsB.y * mat.a12 + extentsB.z * mat.a11;
t = fabs( T.x * mat.a20 - T.z * mat.a00 );
if( t > ra + rb )
{
g_DebugConsole->Write("4\n");
return false;
}
// L = A1 x B1
ra = extentsA.x * mat.a21 + extentsA.z * mat.a01;
rb = extentsB.x * mat.a12 + extentsB.z * mat.a10;
t = fabs( T.x * mat.a21 - T.z * mat.a01 );
if( t > ra + rb )
{
g_DebugConsole->Write("5\n");
return false;
}
// L = A1 x B2
ra = extentsA.x * mat.a22 + extentsA.z * mat.a02;
rb = extentsB.x * mat.a11 + extentsB.y * mat.a10;
t = fabs( T.x * mat.a22 - T.z * mat.a02 );
if( t > ra + rb )
{
g_DebugConsole->Write("6\n");
return false;
}
// L = A2 x B0
ra = extentsA.x * mat.a10 + extentsA.y * mat.a00;
rb = extentsB.y * mat.a22 + extentsB.z * mat.a21;
t = fabs( T.y * mat.a00 - T.x * mat.a10 );
if( t > ra + rb )
{
g_DebugConsole->Write("7\n");
return false;
}
// L = A2 x B1
ra = extentsA.x * mat.a11 + extentsA.y * mat.a01;
rb = extentsB.x * mat.a22 + extentsB.z * mat.a20;
t = fabs( T.y * mat.a01 - T.x * mat.a11 );
if( t > ra + rb )
{
g_DebugConsole->Write("8\n");
return false;
}
// L = A2 x B2
ra = extentsA.x * mat.a12 + extentsA.y * mat.a02;
rb = extentsB.x * mat.a21 + extentsB.y * mat.a20;
t = fabs( T.y * mat.a02 - T.x * mat.a12 );
if( t > ra + rb )
{
g_DebugConsole->Write("9\n");
return false;
}
/*// L = A0 x B0
ra = m_extents.array[1] * fabs(R[2][0]) + m_extents.array[2] * fabs(R[1][0]);
rb = box->Extents().array[1] * fabs(R[0][2]) + box->Extents().array[2] * fabs(R[0][1]);
t = fabs( T.array[2] * R[1][0] - T.array[1] * R[2][0] );
if( t > ra + rb )
{
dbgC.Write("1\n");
return false;
}
// L = A0 x B1
ra = m_extents.array[1] * fabs(R[2][1]) + m_extents.array[2] * fabs(R[1][1]);
rb = box->Extents().array[0] * fabs(R[0][2]) + box->Extents().array[2] * fabs(R[0][0]);
t = fabs( T.array[2] * R[1][1] - T.array[1] * R[2][1] );
if( t > ra + rb )
{
dbgC.Write("2\n");
return false;
}
// L = A0 x B2
ra = m_extents.array[1] * fabs(R[2][2]) + m_extents.array[2] * fabs(R[1][2]);
rb = box->Extents().array[0] * fabs(R[0][1]) + box->Extents().array[1] * fabs(R[0][0]);
t = fabs( T.array[2] * R[1][2] - T.array[1] * R[2][2] );
if( t > ra + rb )
{
dbgC.Write("3\n");
return false;
}
// L = A1 x B0
ra = m_extents.array[0] * fabs(R[2][0]) + m_extents.array[2] * fabs(R[0][0]);
rb = box->Extents().array[1] * fabs(R[1][2]) + box->Extents().array[2] * fabs(R[1][1]);
t = fabs( T.array[0] * R[2][0] - T.array[2] * R[0][0] );
if( t > ra + rb )
{
dbgC.Write("4\n");
return false;
}
// L = A1 x B1
ra = m_extents.array[0] * fabs(R[2][1]) + m_extents.array[2] * fabs(R[0][1]);
rb = box->Extents().array[0] * fabs(R[1][2]) + box->Extents().array[2] * fabs(R[1][0]);
t = fabs( T.array[0] * R[2][1] - T.array[2] * R[0][1] );
if( t > ra + rb )
{
dbgC.Write("5\n");
return false;
}
// L = A1 x B2
ra = m_extents.array[0] * fabs(R[2][2]) + m_extents.array[2] * fabs(R[0][2]);
rb = box->Extents().array[0] * fabs(R[1][1]) + box->Extents().array[1] * fabs(R[1][0]);
t = fabs( T.array[0] * R[2][2] - T.array[2] * R[0][2] );
if( t > ra + rb )
{
dbgC.Write("6\n");
return false;
}
// L = A2 x B0
ra = m_extents.array[0] * fabs(R[1][0]) + m_extents.array[1] * fabs(R[0][0]);
rb = box->Extents().array[1] * fabs(R[2][2]) + box->Extents().array[2] * fabs(R[2][1]);
t = fabs( T.array[1] * R[0][0] - T.array[0] * R[1][0] );
if( t > ra + rb )
{
dbgC.Write("7\n");
return false;
}
// L = A2 x B1
ra = m_extents.array[0] * fabs(R[1][1]) + m_extents.array[1] * fabs(R[0][1]);
rb = box->Extents().array[0] * fabs(R[2][2]) + box->Extents().array[2] * fabs(R[2][0]);
t = fabs( T.array[1] * R[0][1] - T.array[0] * R[1][1] );
if( t > ra + rb )
{
dbgC.Write("8\n");
return false;
}
// L = A2 x B2
ra = m_extents.array[0] * fabs(R[1][2]) + m_extents.array[1] * fabs(R[0][2]);
rb = box->Extents().array[0] * fabs(R[2][1]) + box->Extents().array[1] * fabs(R[2][0]);
t = fabs( T.array[1] * R[0][2] - T.array[0] * R[1][2] );
if( t > ra + rb )
{
dbgC.Write("9\n");
return false;
}
*/
// GOTCHA! No separating axis found. The boxes overlap!
return true;
}
const bool GcOBB::Collides( GcVector3 & thisVel, GcTriangle & tri, GcVector3 & triVel, float maxT, float & firstT, float & lastT )
{
// David Eberly's collision code
// The syntax is slightly modified.
float min0, max0, min1, max1, speed;
GcVector3 D, triEdge[3];
// Consider the box moving and the triangle stationary for simplifying calcualtions
GcVector3 W = thisVel - triVel;
firstT = 0.0f;
lastT = FLT_MAX;
// Test direction of triangle normal
triEdge[0] = tri.b - tri.a;
triEdge[1] = tri.c - tri.a;
D = triEdge[0].CrossProduct(triEdge[1]);
min0 = D.DotProduct(tri.a);
max0 = min0;
GcMath::ProjectOBB(D, *this, min1, max1);
speed = D.DotProduct(W);
if( NoIntersect(maxT, speed, min0, max0, min1, max1, firstT, lastT) )
return false;
// test direction of box faces
for( int i = 0; i < 3; ++i )
{
D = this->Axis(i);
GcMath::ProjectTriangle(D, tri, min0, max0);
float fDdC = D.DotProduct( this->Center() );
min1 = fDdC - this->Extent(i);
max1 = fDdC + this->Extent(i);
speed = D.DotProduct(W);
if( NoIntersect(maxT, speed, min0, max0, min1, max1, firstT, lastT) )
{
return false;
}
}
// test direction of triangle-box edge cross products
triEdge[2] = triEdge[1] - triEdge[0];
for( int i0 = 0; i0 < 3; ++i0 )
{
for( int i1 = 0; i1 < 3; ++i1 )
{
D = triEdge[i0].CrossProduct(this->Axis(i1));
GcMath::ProjectTriangle(D, tri, min0, max0);
GcMath::ProjectOBB(D, *this, min1, max1);
speed = D.DotProduct(W);
if( NoIntersect(maxT, speed, min0, max0, min1, max1, firstT, lastT) )
{
return false;
}
}
}
return true;
}
const bool GcOBB::Collides( GcVector3 & thisVel, GcOBB & box, GcVector3 & boxVel, float time, float & firstT, float & lastT )
{
// David Eberly's collision code
// The syntax is slightly modified.
// Convenience variables
GcVector3 akA[3];
akA[0] = this->Axis(0);
akA[1] = this->Axis(1);
akA[2] = this->Axis(2);
GcVector3 akB[3];
akB[0] = box.Axis(0);
akB[1] = box.Axis(1);
akB[2] = box.Axis(2);
float afEA[3];
afEA[0] = this->Extent(0);
afEA[1] = this->Extent(1);
afEA[2] = this->Extent(2);
float afEB[3];
afEB[0] = box.Extent(0);
afEB[1] = box.Extent(1);
afEB[2] = box.Extent(2);
// Compute relative velocity of box1 with respect to box0 so that box0
// may as well be stationary.
GcVector3 kW = boxVel - thisVel;
// Compute difference of box centers at time 0 and time 'fTime'.
GcVector3 kD0 = box.Center() - this->Center();
GcVector3 kD1 = kD0 + kW * time;
float aafC[3][3]; // matrix C = A^T B, c_{ij} = Dot(A_i,B_j)
float aafAbsC[3][3]; // |c_{ij}|
float afAD0[3]; // Dot(A_i,D0)
float afAD1[3]; // Dot(A_i,D1)
float fR0, fR1, fR; // interval radii and distance between centers
float fR01; // = R0 + R1
// axis C0+t*A0
aafC[0][0] = akA[0].DotProduct(akB[0]);
aafC[0][1] = akA[0].DotProduct(akB[1]);
aafC[0][2] = akA[0].DotProduct(akB[2]);
afAD0[0] = akA[0].DotProduct(kD0);
afAD1[0] = akA[0].DotProduct(kD1);
aafAbsC[0][0] = fabs(aafC[0][0]);
aafAbsC[0][1] = fabs(aafC[0][1]);
aafAbsC[0][2] = fabs(aafC[0][2]);
fR1 = afEB[0]*aafAbsC[0][0]+afEB[1]*aafAbsC[0][1]+afEB[2]*aafAbsC[0][2];
fR01 = afEA[0] + fR1;
if( afAD0[0] > fR01 )
{
if( afAD1[0] > fR01 )
return false;
}
else if( afAD0[0] < -fR01 )
{
if( afAD1[0] < -fR01 )
return false;
}
// axis C0+t*A1
aafC[1][0] = akA[1].DotProduct(akB[0]);
aafC[1][1] = akA[1].DotProduct(akB[1]);
aafC[1][2] = akA[1].DotProduct(akB[2]);
afAD0[1] = akA[1].DotProduct(kD0);
afAD1[1] = akA[1].DotProduct(kD1);
aafAbsC[1][0] = fabs(aafC[1][0]);
aafAbsC[1][1] = fabs(aafC[1][1]);
aafAbsC[1][2] = fabs(aafC[1][2]);
fR1 = afEB[0] * aafAbsC[1][0] + afEB[1] * aafAbsC[1][1] + afEB[2] * aafAbsC[1][2];
fR01 = afEA[1] + fR1;
if( afAD0[1] > fR01 )
{
if( afAD1[1] > fR01 )
return false;
}
else if( afAD0[1] < -fR01 )
{
if( afAD1[1] < -fR01 )
return false;
}
// axis C0+t*A2
aafC[2][0] = akA[2].DotProduct(akB[0]);
aafC[2][1] = akA[2].DotProduct(akB[1]);
aafC[2][2] = akA[2].DotProduct(akB[2]);
afAD0[2] = akA[2].DotProduct(kD0);
afAD1[2] = akA[2].DotProduct(kD1);
aafAbsC[2][0] = fabs(aafC[2][0]);
aafAbsC[2][1] = fabs(aafC[2][1]);
aafAbsC[2][2] = fabs(aafC[2][2]);
fR1 = afEB[0]*aafAbsC[2][0]+afEB[1]*aafAbsC[2][1]+afEB[2]*aafAbsC[2][2];
fR01 = afEA[2] + fR1;
if( afAD0[2] > fR01 )
{
if( afAD1[2] > fR01 )
return false;
}
else if( afAD0[2] < -fR01 )
{
if( afAD1[2] < -fR01 )
return false;
}
// axis C0+t*B0
fR = akB[0].DotProduct(kD0);
fR0 = afEA[0]*aafAbsC[0][0]+afEA[1]*aafAbsC[1][0]+afEA[2]*aafAbsC[2][0];
fR01 = fR0 + afEB[0];
if( fR > fR01 )
{
fR = akB[0].DotProduct(kD1);
if( fR > fR01)
return false;
}
else if( fR < -fR01 )
{
fR = akB[0].DotProduct(kD1);
if( fR < -fR01 )
return false;
}
// axis C0+t*B1
fR = akB[1].DotProduct(kD0);
fR0 = afEA[0]*aafAbsC[0][1]+afEA[1]*aafAbsC[1][1]+afEA[2]*aafAbsC[2][1];
fR01 = fR0 + afEB[1];
if( fR > fR01 )
{
fR = akB[1].DotProduct(kD1);
if( fR > fR01 )
return false;
}
else if( fR < -fR01 )
{
fR = akB[1].DotProduct(kD1);
if( fR < -fR01 )
return false;
}
// axis C0+t*B2
fR = akB[2].DotProduct(kD0);
fR0 = afEA[0]*aafAbsC[0][2]+afEA[1]*aafAbsC[1][2]+afEA[2]*aafAbsC[2][2];
fR01 = fR0 + afEB[2];
if( fR > fR01 )
{
fR = akB[2].DotProduct(kD1);
if( fR > fR01 )
return false;
}
else if( fR < -fR01 )
{
fR = akB[2].DotProduct(kD1);
if( fR < -fR01 )
return false;
}
// axis C0+t*A0xB0
fR = afAD0[2]*aafC[1][0]-afAD0[1]*aafC[2][0];
fR0 = afEA[1]*aafAbsC[2][0] + afEA[2]*aafAbsC[1][0];
fR1 = afEB[1]*aafAbsC[0][2] + afEB[2]*aafAbsC[0][1];
fR01 = fR0 + fR1;
if( fR > fR01 )
{
fR = afAD1[2]*aafC[1][0]-afAD1[1]*aafC[2][0];
if( fR > fR01 )
return false;
}
else if( fR < -fR01 )
{
fR = afAD1[2]*aafC[1][0]-afAD1[1]*aafC[2][0];
if( fR < -fR01 )
return false;
}
// axis C0+t*A0xB1
fR = afAD0[2]*aafC[1][1]-afAD0[1]*aafC[2][1];
fR0 = afEA[1]*aafAbsC[2][1] + afEA[2]*aafAbsC[1][1];
fR1 = afEB[0]*aafAbsC[0][2] + afEB[2]*aafAbsC[0][0];
fR01 = fR0 + fR1;
if( fR > fR01 )
{
fR = afAD1[2]*aafC[1][1]-afAD1[1]*aafC[2][1];
if( fR > fR01 )
return false;
}
else if( fR < -fR01 )
{
fR = afAD1[2]*aafC[1][1]-afAD1[1]*aafC[2][1];
if( fR < -fR01 )
return false;
}
// axis C0+t*A0xB2
fR = afAD0[2]*aafC[1][2]-afAD0[1]*aafC[2][2];
fR0 = afEA[1]*aafAbsC[2][2] + afEA[2]*aafAbsC[1][2];
fR1 = afEB[0]*aafAbsC[0][1] + afEB[1]*aafAbsC[0][0];
fR01 = fR0 + fR1;
if( fR > fR01 )
{
fR = afAD1[2]*aafC[1][2]-afAD1[1]*aafC[2][2];
if( fR > fR01 )
return false;
}
else if( fR < -fR01 )
{
fR = afAD1[2]*aafC[1][2]-afAD1[1]*aafC[2][2];
if( fR < -fR01 )
return false;
}
// axis C0+t*A1xB0
fR = afAD0[0]*aafC[2][0]-afAD0[2]*aafC[0][0];
fR0 = afEA[0]*aafAbsC[2][0] + afEA[2]*aafAbsC[0][0];
fR1 = afEB[1]*aafAbsC[1][2] + afEB[2]*aafAbsC[1][1];
fR01 = fR0 + fR1;
if( fR > fR01 )
{
fR = afAD1[0]*aafC[2][0]-afAD1[2]*aafC[0][0];
if( fR > fR01 )
return false;
}
else if( fR < -fR01 )
{
fR = afAD1[0]*aafC[2][0]-afAD1[2]*aafC[0][0];
if( fR < -fR01 )
return false;
}
// axis C0+t*A1xB1
fR = afAD0[0]*aafC[2][1]-afAD0[2]*aafC[0][1];
fR0 = afEA[0]*aafAbsC[2][1] + afEA[2]*aafAbsC[0][1];
fR1 = afEB[0]*aafAbsC[1][2] + afEB[2]*aafAbsC[1][0];
fR01 = fR0 + fR1;
if( fR > fR01 )
{
fR = afAD1[0]*aafC[2][1]-afAD1[2]*aafC[0][1];
if( fR > fR01 )
return false;
}
else if( fR < -fR01 )
{
fR = afAD1[0]*aafC[2][1]-afAD1[2]*aafC[0][1];
if( fR < -fR01 )
return false;
}
// axis C0+t*A1xB2
fR = afAD0[0] * aafC[2][2] - afAD0[2] * aafC[0][2];
fR0 = afEA[0] * aafAbsC[2][2] + afEA[2] * aafAbsC[0][2];
fR1 = afEB[0] * aafAbsC[1][1] + afEB[1] * aafAbsC[1][0];
fR01 = fR0 + fR1;
if( fR > fR01 )
{
fR = afAD1[0]*aafC[2][2]-afAD1[2]*aafC[0][2];
if( fR > fR01 )
return false;
}
else if( fR < -fR01 )
{
fR = afAD1[0]*aafC[2][2]-afAD1[2]*aafC[0][2];
if( fR < -fR01 )
return false;
}
// axis C0+t*A2xB0
fR = afAD0[1] * aafC[0][0]-afAD0[0]*aafC[1][0];
fR0 = afEA[0] * aafAbsC[1][0] + afEA[1]*aafAbsC[0][0];
fR1 = afEB[1] * aafAbsC[2][2] + afEB[2]*aafAbsC[2][1];
fR01 = fR0 + fR1;
if( fR > fR01 )
{
fR = afAD1[1] * aafC[0][0] - afAD1[0] * aafC[1][0];
if( fR > fR01 )
return false;
}
else if( fR < -fR01 )
{
fR = afAD1[1] * aafC[0][0] - afAD1[0] * aafC[1][0];
if( fR < -fR01 )
return false;
}
// axis C0+t*A2xB1
fR = afAD0[1]*aafC[0][1]-afAD0[0]*aafC[1][1];
fR0 = afEA[0]*aafAbsC[1][1] + afEA[1]*aafAbsC[0][1];
fR1 = afEB[0]*aafAbsC[2][2] + afEB[2]*aafAbsC[2][0];
fR01 = fR0 + fR1;
if( fR > fR01 )
{
fR = afAD1[1]*aafC[0][1]-afAD1[0]*aafC[1][1];
if( fR > fR01 )
return false;
}
else if( fR < -fR01 )
{
fR = afAD1[1]*aafC[0][1]-afAD1[0]*aafC[1][1];
if( fR < -fR01 )
return false;
}
// axis C0+t*A2xB2
fR = afAD0[1] * aafC[0][2] - afAD0[0] * aafC[1][2];
fR0 = afEA[0] * aafAbsC[1][2] + afEA[1] * aafAbsC[0][2];
fR1 = afEB[0] * aafAbsC[2][1] + afEB[1] * aafAbsC[2][0];
fR01 = fR0 + fR1;
if( fR > fR01 )
{
fR = afAD1[1] * aafC[0][2] - afAD1[0] * aafC[1][2];
if( fR > fR01 )
return false;
}
else if( fR < -fR01 )
{
fR = afAD1[1]*aafC[0][2]-afAD1[0]*aafC[1][2];
if( fR < -fR01 )
return false;
}
// At this point none of the 15 axes separate the boxes. It is still
// possible that they are separated as viewed in any plane orthogonal
// to the relative direction of motion W. In the worst case, the two
// projected boxes are hexagons. This requires three separating axis
// tests per box.
GcVector3 kWxD0 = kW.CrossProduct(kD0);
float afWA[3], afWB[3];
// axis C0 + t*WxA0
afWA[1] = kW.DotProduct(akA[1]);
afWA[2] = kW.DotProduct(akA[2]);
fR = fabs(akA[0].DotProduct(kWxD0));
fR0 = afEA[1]*fabs(afWA[2]) + afEA[2]*fabs(afWA[1]);
fR1 = afEB[0] * fabs(aafC[1][0] * afWA[2] - aafC[2][0] * afWA[1]) +
afEB[1] * fabs(aafC[1][1] * afWA[2] - aafC[2][1] * afWA[1]) +
afEB[2] * fabs(aafC[1][2] * afWA[2] - aafC[2][2] * afWA[1]);
fR01 = fR0 + fR1;
if( fR > fR01 )
return false;
// axis C0 + t*WxA1
afWA[0] = kW.DotProduct(akA[0]);
fR = fabs(akA[1].DotProduct(kWxD0));
fR0 = afEA[2] * fabs(afWA[0]) + afEA[0]*fabs(afWA[2]);
fR1 = afEB[0] * fabs(aafC[2][0] * afWA[0] - aafC[0][0] * afWA[2]) +
afEB[1] * fabs(aafC[2][1] * afWA[0] - aafC[0][1] * afWA[2]) +
afEB[2] * fabs(aafC[2][2] * afWA[0] - aafC[0][2] * afWA[2]);
fR01 = fR0 + fR1;
if( fR > fR01 )
return false;
// axis C0 + t*WxA2
fR = fabs(akA[2].DotProduct(kWxD0));
fR0 = afEA[0] * fabs(afWA[1]) + afEA[1] * fabs(afWA[0]);
fR1 = afEB[0] * fabs(aafC[0][0]*afWA[1] - aafC[1][0] * afWA[0]) +
afEB[1] * fabs(aafC[0][1]*afWA[1] - aafC[1][1] * afWA[0]) +
afEB[2] * fabs(aafC[0][2]*afWA[1] - aafC[1][2] * afWA[0]);
fR01 = fR0 + fR1;
if( fR > fR01 )
return false;
// axis C0 + t*WxB0
afWB[1] = kW.DotProduct(akB[1]);
afWB[2] = kW.DotProduct(akB[2]);
fR = fabs(akB[0].DotProduct(kWxD0));
fR0 = afEA[0] * fabs(aafC[0][1] * afWB[2] - aafC[0][2] * afWB[1]) +
afEA[1] * fabs(aafC[1][1] * afWB[2] - aafC[1][2] * afWB[1]) +
afEA[2] * fabs(aafC[2][1] * afWB[2] - aafC[2][2] * afWB[1]);
fR1 = afEB[1] * fabs(afWB[2]) + afEB[2] * fabs(afWB[1]);
fR01 = fR0 + fR1;
if( fR > fR01 )
return false;
// axis C0 + t*WxB1
afWB[0] = kW.DotProduct(akB[0]);
fR = fabs( akB[1].DotProduct(kWxD0) );
fR0 = afEA[0]*fabs(aafC[0][2]*afWB[0] - aafC[0][0]*afWB[2]) +
afEA[1]*fabs(aafC[1][2]*afWB[0] - aafC[1][0]*afWB[2]) +
afEA[2]*fabs(aafC[2][2]*afWB[0] - aafC[2][0]*afWB[2]);
fR1 = afEB[2]*fabs(afWB[0]) + afEB[0]*fabs(afWB[2]);
fR01 = fR0 + fR1;
if( fR > fR01 )
return false;
// axis C0 + t*WxB2
fR = fabs( akB[2].DotProduct(kWxD0) );
fR0 = afEA[0] * fabs(aafC[0][0] * afWB[1] - aafC[0][1] * afWB[0]) +
afEA[1] * fabs(aafC[1][0] * afWB[1] - aafC[1][1] * afWB[0]) +
afEA[2] * fabs(aafC[2][0] * afWB[1] - aafC[2][1] * afWB[0]);
fR1 = afEB[0] * fabs(afWB[1]) + afEB[1] * fabs(afWB[0]);
fR01 = fR0 + fR1;
if( fR > fR01 )
return false;
return true;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool GcOBB::NoIntersect( float maxT, float speed, float min0, float max0, float min1, float max1, float & firstT, float & lastT )
{
float invSpeed, T;
if( max1 < min0 ) // C1 initially on left of C0
{
if( speed <= 0.0f )
{
// intervals moving apart
return true;
}
invSpeed = 1.0f / speed;
T = (min0 - max1)*invSpeed;
if( T > firstT )
firstT = T;
if( firstT > maxT )
return true;
T = (max0 - min1)*invSpeed;
if( T < lastT )
lastT = T;
if( firstT > lastT )
return true;
}
else if( max0 < min1 ) // C1 initially on right of C0
{
if( speed >= 0.0f )
{
// intervals moving apart
return true;
}
invSpeed = 1.0f / speed;
T = (max0 - min1) * invSpeed;
if( T > firstT )
firstT = T;
if( firstT > maxT )
return true;
T = (min0 - max1) * invSpeed;
if( T < lastT )
lastT = T;
if( firstT > lastT )
return true;
}
else // C0 and C1 overlap
{
if( speed > 0.0f )
{
T = (max0 - min1)/speed;
if( T < lastT )
lastT = T;
if( firstT > lastT )
return true;
}
else if( speed < 0.0f )
{
T = (min0 - max1) / speed;
if( T < lastT )
lastT = T;
if( firstT > lastT )
return true;
}
}
return false;
}