You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
3035 lines
77 KiB
3035 lines
77 KiB
/*==LICENSE==* |
|
|
|
CyanWorlds.com Engine - MMOG client, server and tools |
|
Copyright (C) 2011 Cyan Worlds, Inc. |
|
|
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
|
|
Additional permissions under GNU GPL version 3 section 7 |
|
|
|
If you modify this Program, or any covered work, by linking or |
|
combining it with any of RAD Game Tools Bink SDK, Autodesk 3ds Max SDK, |
|
NVIDIA PhysX SDK, Microsoft DirectX SDK, OpenSSL library, Independent |
|
JPEG Group JPEG library, Microsoft Windows Media SDK, or Apple QuickTime SDK |
|
(or a modified version of those libraries), |
|
containing parts covered by the terms of the Bink SDK EULA, 3ds Max EULA, |
|
PhysX SDK EULA, DirectX SDK EULA, OpenSSL and SSLeay licenses, IJG |
|
JPEG Library README, Windows Media SDK EULA, or QuickTime SDK EULA, the |
|
licensors of this Program grant you additional |
|
permission to convey the resulting work. Corresponding Source for a |
|
non-source form of such a combination shall include the source code for |
|
the parts of OpenSSL and IJG JPEG Library used as well as that of the covered |
|
work. |
|
|
|
You can contact Cyan Worlds, Inc. by email legal@cyan.com |
|
or by snail mail at: |
|
Cyan Worlds, Inc. |
|
14617 N Newport Hwy |
|
Mead, WA 99021 |
|
|
|
*==LICENSE==*/ |
|
|
|
#include "HeadSpin.h" |
|
#pragma hdrstop |
|
|
|
#include "hsBounds.h" |
|
#include "hsStream.h" |
|
|
|
#include "hsFastMath.h" |
|
|
|
const float hsBounds::kRealSmall = 1.0e-5f; |
|
|
|
/////////////////////////////////////////////////////////////////////////////////////// |
|
// |
|
// hsBounds |
|
// |
|
///////////////////////////////////////////////////////////////////////////////////////// |
|
|
|
void hsBounds::Read(hsStream *s) |
|
{ |
|
fType =(hsBoundsType) s->ReadLE32(); |
|
} |
|
|
|
void hsBounds::Write(hsStream *s) |
|
{ |
|
s->WriteLE32((int32_t)fType); |
|
} |
|
|
|
/////////////////////////////////////////////////////////////////////////////////////// |
|
// |
|
// hsBounds3 |
|
// |
|
///////////////////////////////////////////////////////////////////////////////////////// |
|
|
|
void hsBounds3::Transform(const hsMatrix44 *mat) |
|
{ |
|
#if 0 // IDENT |
|
if( mat->fFlags & hsMatrix44::kIsIdent ) |
|
return; |
|
#endif // IDENT |
|
|
|
hsAssert(fType != kBoundsUninitialized, "Can't transform an unitialized bound"); |
|
if(fType == kBoundsNormal) |
|
{ |
|
hsPoint3 corners[8]; |
|
this->GetCorners(corners); |
|
|
|
mat->MapPoints(8, corners); |
|
this->Reset(8,corners); |
|
fBounds3Flags &= ~kCenterValid; |
|
} |
|
} |
|
|
|
void hsBounds3::Reset(const hsPoint3 *p) |
|
{ |
|
fType = kBoundsNormal; |
|
fMins = fMaxs = *p; |
|
fBounds3Flags |= kCenterValid; |
|
fCenter = *p; |
|
} |
|
|
|
void hsBounds3::Reset(const hsBounds3 *b) |
|
{ |
|
if( kBoundsNormal == b->fType ) |
|
{ |
|
fType = kBoundsNormal; |
|
fMins = b->fMins; |
|
fMaxs = b->fMaxs; |
|
if( b->fBounds3Flags & kCenterValid ) |
|
{ |
|
fBounds3Flags |= kCenterValid; |
|
fCenter = b->fCenter; |
|
} |
|
else |
|
fBounds3Flags &= ~kCenterValid; |
|
} |
|
else |
|
fType = b->fType; |
|
} |
|
|
|
void hsBounds3::Reset(int n, const hsPoint3 *p) |
|
{ |
|
fType = kBoundsNormal; |
|
fMins = fMaxs = *p; |
|
for(int i = 1; i < n ; i++) |
|
this->Union(&p[i]); |
|
fBounds3Flags &= ~kCenterValid; |
|
} |
|
|
|
void hsBounds3::Union(const hsPoint3 *p) |
|
{ |
|
if(fType == kBoundsNormal) // Add this point if bounds is normal |
|
{ |
|
for (int i = 0; i < 3; i++) |
|
{ |
|
if ((*p)[i] > fMaxs[i]) |
|
fMaxs[i] =(*p)[i]; |
|
else if ((*p)[i] < fMins[i]) |
|
fMins[i] =(*p)[i]; |
|
} |
|
fBounds3Flags &= ~kCenterValid; |
|
} |
|
else |
|
{ |
|
if(fType != kBoundsFull) // Otherwise re-init unless bounds is full already |
|
this->Reset(p); |
|
} |
|
} |
|
|
|
void hsBounds3::Union(const hsVector3 *v) |
|
{ |
|
if(fType == kBoundsNormal) // Add this point if bounds is normal |
|
{ |
|
for (int i = 0; i < 3; i++) |
|
{ |
|
if( (*v)[i] > 0 ) |
|
fMaxs[i] += (*v)[i]; |
|
else |
|
fMins[i] += (*v)[i]; |
|
} |
|
fBounds3Flags &= ~kCenterValid; |
|
} |
|
} |
|
|
|
|
|
void hsBounds3::Union(const hsBounds3 *p) |
|
{ |
|
if(fType == kBoundsNormal && p->GetType() == kBoundsNormal) // Add this point if bounds is normal |
|
{ |
|
for (int i = 0; i < 3; i++) |
|
{ |
|
if (p->fMaxs[i] > fMaxs[i]) |
|
fMaxs[i] = p->fMaxs[i]; |
|
if (p->fMins[i] < fMins[i]) |
|
fMins[i] = p->fMins[i]; |
|
} |
|
fBounds3Flags &= ~kCenterValid; |
|
} |
|
else if(fType == kBoundsEmpty || fType == kBoundsUninitialized) |
|
{ |
|
*this = *p; |
|
} |
|
// If fType is kBoundsFull don't do anything |
|
} |
|
|
|
void hsBounds3::MakeSymmetric(const hsPoint3* p) |
|
{ |
|
if( fType != kBoundsNormal ) |
|
return; |
|
|
|
float delMax = 0; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float delUp; |
|
|
|
delUp = fMaxs[i] - (*p)[i]; |
|
delMax = hsMaximum(delMax, delUp); |
|
delUp = (*p)[i] - fMins[i]; |
|
delMax = hsMaximum(delMax, delUp); |
|
} |
|
const float sqrtTwo = 1.41421f; |
|
delMax *= sqrtTwo; |
|
hsAssert((delMax > -1.e6f)&&(delMax < 1.e6f), "MakeSymmetric going out to sea"); |
|
fCenter = *p; |
|
fMaxs.Set(delMax, delMax, delMax); |
|
fMaxs += fCenter; |
|
fMins.Set(-delMax, -delMax, -delMax); |
|
fMins += fCenter; |
|
fBounds3Flags |= kCenterValid; |
|
} |
|
|
|
void hsBounds3::InscribeSphere() |
|
{ |
|
if( fType != kBoundsNormal ) |
|
return; |
|
|
|
const float ooSix = hsInvert(2.f * 3.f); |
|
float a = GetMaxDim() * ooSix; |
|
hsPoint3 p = GetCenter(); |
|
p.fX += a; |
|
p.fY += a; |
|
p.fZ += a; |
|
fMaxs = p; |
|
a *= -2.f; |
|
p.fX += a; |
|
p.fY += a; |
|
p.fZ += a; |
|
fMins = p; |
|
|
|
// Center still valid, type still normal |
|
} |
|
|
|
// neg, pos, zero == disjoint, I contain other, overlap |
|
int32_t hsBounds3::TestBound(const hsBounds3& other) const |
|
{ |
|
int32_t retVal = 1; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
if( GetMins()[i] > other.GetMaxs()[i] ) |
|
return -1; |
|
if( GetMaxs()[i] < other.GetMins()[i] ) |
|
return -1; |
|
|
|
if( GetMaxs()[i] < other.GetMaxs()[i] ) |
|
retVal = 0; |
|
if( GetMins()[i] > other.GetMins()[i] ) |
|
retVal = 0; |
|
} |
|
return retVal; |
|
} |
|
|
|
bool hsBounds3::IsInside(const hsPoint3* pos) const |
|
{ |
|
hsAssert(fType != kBoundsUninitialized, "Invalid bounds type for hsBounds3::IsInside() "); |
|
if(fType == kBoundsEmpty) |
|
return false; |
|
if(fType == kBoundsFull) |
|
return true; |
|
return !(pos->fX>fMaxs.fX || pos->fY>fMaxs.fY || pos->fZ>fMaxs.fZ || |
|
pos->fX<fMins.fX || pos->fY<fMins.fY || pos->fZ<fMins.fZ); |
|
} |
|
|
|
#if 0 // MESH_GEN_DEFER |
|
void hsBounds3::MakeTriMeshSphere(hsGTriMesh* tMesh, hsPoint3* cornersIn) const |
|
{ |
|
hsPoint3 center = (*GetMaxs() + *GetMins()) * 0.5f; |
|
float radius = GetMaxDim() * 0.5f; |
|
|
|
const int nLong = 9; |
|
const int nLati = 5; |
|
|
|
int nPts = nLong * nLati + 3; |
|
int nFaces = nLong * 2 + nLong * (nLati - 1) * 2; // == nLong * nLati * 2 |
|
tMesh->AllocatePointers(nFaces /*faces*/, nPts /*pts*/, 0 /*uvs*/, 0 /*colors*/); |
|
tMesh->SetNumTriVertex(nPts); |
|
|
|
int iCenter = nPts - 3; |
|
int iNorthPole = nPts - 2; |
|
int iSouthPole = nPts - 1; |
|
hsPoint3 pt; |
|
pt = center; |
|
tMesh->SetPoint(iCenter, &pt); |
|
pt.fZ += radius; |
|
tMesh->SetPoint(iNorthPole, &pt); |
|
pt.fZ -= 2.f * radius; |
|
tMesh->SetPoint(iSouthPole, &pt); |
|
|
|
int i, j; |
|
for( i = 0; i < nLong; i++ ) |
|
{ |
|
for( j = 0; j < nLati; j++ ) |
|
{ |
|
float theta = (float(i) / nLong) * 2.f * M_PI; |
|
float cosTheta = cos(theta); |
|
float sinTheta = sin(theta); |
|
|
|
float phi = (float(j+1) / (nLati+1)) * M_PI; |
|
float cosPhi = cos(phi); |
|
float sinPhi = sin(phi); |
|
|
|
pt.fX = center.fX + radius * sinPhi * cosTheta; |
|
pt.fY = center.fY + radius * sinPhi * sinTheta; |
|
pt.fZ = center.fZ + radius * cosPhi; |
|
tMesh->SetPoint(j + i * nLati, &pt); |
|
} |
|
} |
|
|
|
hsTriangle3* tri; |
|
int nTris = 0; |
|
int iNext; |
|
for( i = 0; i < nLong; i++ ) |
|
{ |
|
if( (iNext = i + 1) >= nLong ) |
|
iNext = 0; |
|
|
|
tri = tMesh->GetTriFromPool(nTris); |
|
tri->Zero(); |
|
tri->fFlags |= hsTriangle3::kTwoSided; |
|
tMesh->SetTriangle(nTris++, tri); |
|
tri->SetQuickMeshVerts(i * nLati, iNext * nLati, iNorthPole); |
|
|
|
tri = tMesh->GetTriFromPool(i); |
|
tri->Zero(); |
|
tri->fFlags |= hsTriangle3::kTwoSided; |
|
tMesh->SetTriangle(nTris++, tri); |
|
tri->SetQuickMeshVerts(nLati-1 + iNext * nLati, nLati-1 + i * nLati, iSouthPole); |
|
|
|
int jNext; |
|
for( j = 0; j < nLati-1; j++ ) |
|
{ |
|
jNext = j + 1; |
|
|
|
tri = tMesh->GetTriFromPool(nTris); |
|
tri->Zero(); |
|
tri->fFlags |= hsTriangle3::kTwoSided; |
|
tMesh->SetTriangle(nTris++, tri); |
|
tri->SetQuickMeshVerts(j + i * nLati, j + iNext * nLati, jNext + i * nLati); |
|
|
|
tri = tMesh->GetTriFromPool(nTris); |
|
tri->Zero(); |
|
tri->fFlags |= hsTriangle3::kTwoSided; |
|
tMesh->SetTriangle(nTris++, tri); |
|
tri->SetQuickMeshVerts(jNext + iNext * nLati, jNext + i * nLati, j + iNext * nLati); |
|
} |
|
} |
|
} |
|
|
|
// |
|
// Allocate and create mesh from bounding box |
|
// |
|
void hsBounds3::MakeTriMesh(hsGTriMesh* tMesh, uint32_t triFlags, hsPoint3* cornersIn) const |
|
{ |
|
hsAssert(cornersIn || fType == kBoundsNormal, |
|
"Invalid bounds type for hsBounds3::MakeTriMesh "); |
|
|
|
const int maxNew= 12; |
|
// Setup tMesh |
|
tMesh->AllocatePointers(maxNew /*faces*/, 8 /*pts*/, 0 /*uvs*/, 0 /*colors*/); |
|
tMesh->SetNumTriVertex(8); |
|
int i; |
|
hsPoint3 corners[8]; |
|
// Set Points |
|
if( !cornersIn ) |
|
{ |
|
GetCorners(corners); |
|
cornersIn = corners; |
|
} |
|
for(i=0; i<8; i++) |
|
{ |
|
tMesh->SetPoint(i, &cornersIn[i]); |
|
} |
|
tMesh->GetVertexPool()->SetCount(8); |
|
|
|
// Set faces |
|
hsTriangle3 *tri; |
|
int triNum=0; |
|
|
|
|
|
static int verts[maxNew * 3] = { |
|
/* -Y */ 6,2,3, |
|
/* -Y */ 6,3,7, |
|
/* Y */ 5,1,0, |
|
/* Y */ 5,0,4, |
|
/* -X */ 7,3,1, |
|
/* -X */ 7,1,5, |
|
/* X */ 4,0,2, |
|
/* X */ 4,2,6, |
|
/* Z */ 3,0,1, |
|
/* Z */ 3,2,0, |
|
/* -Z */ 7,4,6, |
|
/* -Z */ 7,5,4 |
|
}; |
|
int v=0; |
|
for (;triNum < maxNew;triNum++) |
|
{ |
|
tri = tMesh->GetTriFromPool(triNum); |
|
tri->Zero(); |
|
tri->fFlags |= triFlags; |
|
tMesh->SetTriangle(triNum, tri); |
|
tri->SetQuickMeshVerts(verts[v + 0],verts[v + 1],verts[v + 2]); |
|
v += 3; |
|
} |
|
tMesh->SetTrianglePointers(); |
|
} |
|
#endif // MESH_GEN_DEFER |
|
|
|
void hsBounds3::TestPlane(const hsPlane3 *p, hsPoint2 &depth) const |
|
{ |
|
TestPlane(p->fN, depth); |
|
} |
|
void hsBounds3::TestPlane(const hsVector3 &n, hsPoint2 &depth) const |
|
{ |
|
hsAssert(fType == kBoundsNormal, "TestPlane only valid for kBoundsNormal filled bounds"); |
|
|
|
float dmax = fMins.InnerProduct(n); |
|
float dmin = dmax; |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float dd; |
|
dd = fMaxs[i] - fMins[i]; |
|
dd *= n[i]; |
|
|
|
if( dd < 0 ) |
|
dmin += dd; |
|
else |
|
dmax += dd; |
|
} |
|
|
|
depth.fX = dmin; |
|
depth.fY = dmax; |
|
} |
|
|
|
float hsBounds3::ClosestPointToLine(const hsPoint3 *p, const hsPoint3 *v0, const hsPoint3 *v1, hsPoint3 *out) |
|
{ |
|
hsVector3 del(v1, v0); |
|
float magSq = del.MagnitudeSquared(); |
|
float t = 0.f; |
|
if( magSq < hsBounds::kRealSmall ) |
|
{ |
|
*out = *v0; |
|
} |
|
else |
|
{ |
|
t = del.InnerProduct(hsVector3(p, v0)) * hsInvert(magSq); |
|
if( t >= 1.f ) |
|
*out = *v1; |
|
else if( t <= 0 ) |
|
*out = *v0; |
|
else |
|
*out = *v0 + del * t; |
|
} |
|
return t; |
|
} |
|
|
|
float hsBounds3::ClosestPointToInfiniteLine(const hsPoint3* p, const hsVector3* v, hsPoint3* out) |
|
{ |
|
float magSq = v->MagnitudeSquared(); |
|
float t = 0.f; |
|
hsPoint3 origin(0,0,0); |
|
if( magSq < hsBounds::kRealSmall ) |
|
{ |
|
*out = origin; |
|
} |
|
else |
|
{ |
|
t = v->InnerProduct(hsVector3(*p)) * hsInvert(magSq); |
|
*out = hsPoint3(*v * t); |
|
} |
|
return t; |
|
} |
|
|
|
bool hsBounds3::ClosestPoint(const hsPoint3& p, hsPoint3& inner, hsPoint3& outer) const |
|
{ |
|
// Look for axis intervals p is within |
|
int nSect = 0; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
if( p[i] < fMins[i] ) |
|
{ |
|
inner[i] = fMins[i]; |
|
outer[i] = fMaxs[i]; |
|
} |
|
else if( p[i] > fMaxs[i] ) |
|
{ |
|
inner[i] = fMaxs[i]; |
|
outer[i] = fMins[i]; |
|
} |
|
else |
|
{ |
|
inner[i] = p[i]; |
|
outer[i] = (p[i] - fMins[i] > fMaxs[i] - p[i]) ? fMins[i] : fMaxs[i]; |
|
nSect++; |
|
} |
|
} |
|
|
|
return nSect == 3; |
|
} |
|
|
|
void hsBounds3::Read(hsStream *stream) |
|
{ |
|
hsBounds::Read(stream); |
|
fMins.Read(stream); |
|
fMaxs.Read(stream); |
|
fBounds3Flags = 0; |
|
} |
|
|
|
void hsBounds3::Write(hsStream *stream) |
|
{ |
|
hsBounds::Write(stream); |
|
fMins.Write(stream); |
|
fMaxs.Write(stream); |
|
} |
|
|
|
////////////////////////////////// |
|
////////////////////////////////////////////////// |
|
// Plane Bounds util class |
|
////////////////////////////////////////////////// |
|
|
|
hsPoint3 hsBoundsOriented::GetCenter() const |
|
{ |
|
hsAssert(fCenterValid==true, "Unset center for hsBoundsOriented::GetCenter()"); |
|
return fCenter; |
|
} |
|
|
|
void hsBoundsOriented::TestPlane(const hsVector3 &n, hsPoint2 &depth) const |
|
{ |
|
hsAssert(false, "TestPlane not a valid operation for hsBounsOriented"); |
|
} |
|
// |
|
// Return true if inside all the planes |
|
// |
|
bool hsBoundsOriented::IsInside(const hsPoint3* pos) const |
|
{ |
|
hsAssert(fType == kBoundsNormal, "Invalid bounds type for hsBounds3::IsInside() "); |
|
if(fType == kBoundsEmpty) |
|
return false; |
|
if(fType == kBoundsFull) |
|
return true; |
|
int i; |
|
for( i = 0; i < fNumPlanes; i++ ) |
|
{ |
|
float dis = fPlanes[i].fN.InnerProduct(pos); |
|
dis += fPlanes[i].fD; |
|
if( dis > 0.f ) |
|
return false; |
|
} |
|
|
|
return true; |
|
} |
|
|
|
void hsBoundsOriented::SetNumberPlanes(uint32_t n) |
|
{ |
|
delete [] fPlanes; |
|
fPlanes = new hsPlane3[fNumPlanes = n]; |
|
} |
|
|
|
void hsBoundsOriented::SetPlane(uint32_t i, hsPlane3 *pln) |
|
{ |
|
fType = kBoundsNormal; |
|
if( i >= fNumPlanes ) |
|
{ |
|
hsPlane3 *newPlanes = new hsPlane3[i+1]; |
|
if( fPlanes ) |
|
{ |
|
int k; |
|
for( k = 0; k < fNumPlanes; k++ ) |
|
*newPlanes++ = *fPlanes++; |
|
delete [] fPlanes; |
|
} |
|
fPlanes = newPlanes; |
|
fNumPlanes = i+1; |
|
} |
|
fPlanes[i] = *pln; |
|
} |
|
|
|
// |
|
// Make mesh from bounds3. Make boundsOriented from mesh tris. |
|
// |
|
void hsBoundsOriented::Reset(const hsBounds3* bounds) |
|
{ |
|
#if 0 // MESH_GEN_DEFER |
|
hsGTriMesh tMesh; |
|
bounds->MakeTriMesh(&tMesh, 0 /* triFlags */); |
|
Reset(&tMesh); |
|
#endif // MESH_GEN_DEFER |
|
} |
|
|
|
#if 0 |
|
// |
|
// Make mesh from bounds3. Make boundsOriented from mesh tris. |
|
// |
|
void hsBoundsOriented::Union(const hsBounds3 *b) |
|
{ |
|
#if 0 // MESH_GEN_DEFER |
|
hsGTriMesh tMesh; |
|
bounds->MakeTriMesh(&tMesh); |
|
int i; |
|
hsTriangle3 tri; |
|
for (i=0; i<tMesh.GetNumTriangles(); i++) |
|
{ |
|
tMesh.GetTriangle(i, &tri); |
|
Union(&tri); |
|
} |
|
#endif // MESH_GEN_DEFER |
|
} |
|
#endif |
|
|
|
// |
|
// |
|
// |
|
|
|
void hsBoundsOriented::Write(hsStream *stream) |
|
{ |
|
hsBounds::Write(stream); |
|
fCenter.Write(stream); |
|
stream->WriteLE32(fCenterValid); |
|
stream->WriteLE32(fNumPlanes); |
|
int i; |
|
for( i = 0; i < fNumPlanes; i++ ) |
|
{ |
|
fPlanes[i].Write(stream); |
|
} |
|
} |
|
|
|
void hsBoundsOriented::Read(hsStream *stream) |
|
{ |
|
hsBounds::Read(stream); |
|
fCenter.Read(stream); |
|
fCenterValid = (bool)stream->ReadLE32(); |
|
fNumPlanes = stream->ReadLE32(); |
|
if (fPlanes) |
|
delete [] fPlanes; |
|
fPlanes = new hsPlane3[fNumPlanes]; |
|
int i; |
|
for( i = 0; i < fNumPlanes; i++ ) |
|
{ |
|
fPlanes[i].Read(stream); |
|
} |
|
} |
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////// |
|
/////////////////////////////////////////////////////////////////////////////////////////// |
|
/////////////////////////////////////////////////////////////////////////////////////////// |
|
|
|
hsBounds3Ext::hsBounds3Ext(const hsBounds3 &b) |
|
{ |
|
Reset(&b); |
|
} |
|
hsBounds3Ext &hsBounds3Ext::operator=(const hsBounds3 &b) |
|
{ |
|
Reset(&b); |
|
return *this; |
|
} |
|
|
|
void hsBounds3Ext::IMakeMinsMaxs() |
|
{ |
|
hsAssert(!(fExtFlags & kAxisAligned), "Axis aligned box defined by min and max"); |
|
fMins = fMaxs = fCorner; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
if(!IAxisIsZero(i) ) |
|
{ |
|
int j; |
|
for( j = 0; j < 3; j++ ) |
|
{ |
|
if( fAxes[i][j] < 0 ) |
|
fMins[j] += fAxes[i][j]; |
|
else |
|
fMaxs[j] += fAxes[i][j]; |
|
} |
|
} |
|
} |
|
} |
|
|
|
void hsBounds3Ext::IMakeDists() const |
|
{ |
|
hsAssert(!(fExtFlags & kAxisAligned), "Dists only useful for transformed BB"); |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
fDists[i].fX = fCorner.InnerProduct(fAxes[i]); |
|
if( !IAxisIsZero(i) ) |
|
{ |
|
fDists[i].fY = fDists[i].fX + fAxes[i].InnerProduct(fAxes[i]); |
|
if( fDists[i].fX > fDists[i].fY ) |
|
{ |
|
float t = fDists[i].fX; |
|
fDists[i].fX = fDists[i].fY; |
|
fDists[i].fY = t; |
|
} |
|
} |
|
else |
|
fDists[i].fY = fDists[i].fX; |
|
} |
|
fExtFlags |= kDistsSet; |
|
} |
|
|
|
void hsBounds3Ext::IMakeSphere() const |
|
{ |
|
if(!(fBounds3Flags & kCenterValid) ) |
|
ICalcCenter(); |
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
if( fBounds3Flags & kIsSphere ) |
|
{ |
|
fRadius = fMaxs[0] - fMins[0]; |
|
int i; |
|
for( i = 1; i < 3; i++ ) |
|
{ |
|
float dist = fMaxs[i] - fMins[i]; |
|
if( dist < fRadius ) |
|
fRadius = dist; |
|
} |
|
fRadius *= 0.5f; |
|
} |
|
else |
|
{ |
|
fRadius = sqrt(hsVector3(&fMaxs, &fCenter).MagnitudeSquared()); |
|
} |
|
} |
|
else |
|
{ |
|
if( fBounds3Flags & kIsSphere ) |
|
{ |
|
float minMagSq = fAxes[0].MagnitudeSquared(); |
|
float magSq = fAxes[1].MagnitudeSquared(); |
|
if( magSq < minMagSq ) |
|
magSq = minMagSq; |
|
magSq = fAxes[2].MagnitudeSquared(); |
|
if( magSq < minMagSq ) |
|
magSq = minMagSq; |
|
fRadius = sqrt(magSq); |
|
} |
|
else |
|
{ |
|
hsVector3 accum; |
|
accum.Set(0,0,0); |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
if( !IAxisIsZero(i) ) |
|
accum += fAxes[i]; |
|
} |
|
fRadius = sqrt((accum * 0.5f).MagnitudeSquared()); |
|
} |
|
} |
|
fExtFlags |= kSphereSet; |
|
} |
|
|
|
void hsBounds3Ext::Reset(const hsBounds3 *b) |
|
{ |
|
fExtFlags = kAxisAligned; |
|
hsBounds3::Reset(b); |
|
} |
|
|
|
void hsBounds3Ext::Reset(const hsPoint3 *p) |
|
{ |
|
fExtFlags = kAxisAligned | kSphereSet; |
|
hsBounds3::Reset(p); |
|
fRadius = 0; |
|
} |
|
|
|
void hsBounds3Ext::Reset(const hsBounds3Ext *b) |
|
{ |
|
hsBounds3::Reset(b); |
|
fExtFlags = b->fExtFlags; |
|
if (!(fExtFlags & kAxisAligned)) |
|
{ |
|
fCorner = b->fCorner; |
|
fAxes[0] = b->fAxes[0]; |
|
fAxes[1] = b->fAxes[1]; |
|
fAxes[2] = b->fAxes[2]; |
|
} |
|
if (fExtFlags & kDistsSet) |
|
{ |
|
fDists[0] = b->fDists[0]; |
|
fDists[1] = b->fDists[1]; |
|
fDists[2] = b->fDists[2]; |
|
} |
|
if (fExtFlags & kSphereSet) |
|
fRadius = b->fRadius; |
|
} |
|
|
|
|
|
void hsBounds3Ext::GetCorners(hsPoint3 *b) const |
|
{ |
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
hsBounds3::GetCorners(b); |
|
} |
|
else |
|
{ |
|
int i; |
|
for( i = 0; i < 8; i++ ) |
|
{ |
|
b[i] = fCorner; |
|
if( !(i & 0x1) && !(fExtFlags & kAxisZeroZero) )b[i] += fAxes[0]; |
|
if( !(i & 0x2) && !(fExtFlags & kAxisOneZero) )b[i] += fAxes[1]; |
|
if( !(i & 0x4) && !(fExtFlags & kAxisTwoZero) )b[i] += fAxes[2]; |
|
} |
|
} |
|
} |
|
|
|
void hsBounds3Ext::GetAxes(hsVector3 *fAxis0, hsVector3 *fAxis1, hsVector3 *fAxis2) const |
|
{ |
|
if( !(fExtFlags & kAxisAligned) ) |
|
{ |
|
*fAxis0 = fAxes[0]; |
|
*fAxis1 = fAxes[1]; |
|
*fAxis2 = fAxes[2]; |
|
} |
|
else |
|
{ |
|
fAxis0->Set(fMaxs.fX - fMins.fX, 0, 0); |
|
fAxis1->Set(0, fMaxs.fY - fMins.fY, 0); |
|
fAxis2->Set(0, 0, fMaxs.fZ - fMins.fZ); |
|
} |
|
} |
|
|
|
void hsBounds3Ext::Reset(int n, const hsPoint3 *p) |
|
{ |
|
fExtFlags = kAxisAligned; |
|
hsBounds3::Reset(n, p); |
|
} |
|
|
|
// mf horse - could union in a point preserving axes... |
|
void hsBounds3Ext::Union(const hsPoint3 *p) |
|
{ |
|
fExtFlags = kAxisAligned; |
|
hsBounds3::Union(p); |
|
} |
|
void hsBounds3Ext::Union(const hsVector3 *v) |
|
{ |
|
#if 0 // smarter union |
|
fExtFlags = kAxisAligned; |
|
hsBounds3::Union(v); |
|
#else // smarter union |
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
hsBounds3::Union(v); |
|
} |
|
else |
|
{ |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float dot = fAxes[i].InnerProduct(v); |
|
dot /= fAxes[i].MagnitudeSquared(); |
|
if( dot > 0 ) |
|
{ |
|
fAxes[i] += dot * fAxes[i]; |
|
fExtFlags &= ~(1 << (20+i)); // axis not zero no more |
|
} |
|
else if( dot < 0 ) |
|
{ |
|
hsVector3 del = dot * fAxes[i]; |
|
fCorner += del; |
|
del = -del; |
|
fAxes[i] += del; |
|
fExtFlags &= ~(1 << (20+i)); // axis not zero no more |
|
} |
|
} |
|
fExtFlags &= ~(kSphereSet | kDistsSet); |
|
fBounds3Flags &= ~kCenterValid; |
|
} |
|
#endif // smarter union |
|
} |
|
|
|
void hsBounds3Ext::Union(const hsBounds3 *b) |
|
{ |
|
fExtFlags = kAxisAligned; |
|
hsBounds3::Union(b); |
|
} |
|
|
|
void hsBounds3Ext::MakeSymmetric(const hsPoint3* p) |
|
{ |
|
if( fType != kBoundsNormal ) |
|
return; |
|
|
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
fExtFlags = kAxisAligned; |
|
hsBounds3::MakeSymmetric(p); |
|
return; |
|
} |
|
|
|
// Can do this preserving axes, but may not be worth it. |
|
fExtFlags = kAxisAligned; |
|
hsBounds3::MakeSymmetric(p); |
|
} |
|
|
|
void hsBounds3Ext::InscribeSphere() |
|
{ |
|
fBounds3Flags |= kIsSphere; |
|
fExtFlags |= kAxisAligned; |
|
IMakeSphere(); |
|
return; |
|
#if 0 |
|
if( fType != kBoundsNormal ) |
|
return; |
|
|
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
hsBounds3::InscribeSphere(); |
|
return; |
|
} |
|
|
|
const float oneThird = hsInvert(3.f); |
|
// float a = GetMaxDim() * hsInvert(6.f); |
|
float a = GetRadius() * oneThird; |
|
hsPoint3 p = GetCenter(); |
|
p.fX += a; |
|
p.fY += a; |
|
p.fZ += a; |
|
fMaxs = p; |
|
a *= -2.f; |
|
p.fX += a; |
|
p.fY += a; |
|
p.fZ += a; |
|
fMins = p; |
|
|
|
// Center still valid, type still normal |
|
fExtFlags = kAxisAligned; |
|
#endif |
|
} |
|
|
|
void hsBounds3Ext::Transform(const hsMatrix44 *m) |
|
{ |
|
if( fType != kBoundsNormal ) |
|
return; |
|
|
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
fExtFlags = 0; |
|
|
|
fCorner = *m * fMins; |
|
hsVector3 v; |
|
float span; |
|
span = fMaxs.fX - fMins.fX; |
|
if( span < kRealSmall ) |
|
{ |
|
fExtFlags |= kAxisZeroZero; |
|
span = 1.f; |
|
} |
|
v.Set(span, 0, 0); |
|
fAxes[0] = *m * v; |
|
span = fMaxs.fY - fMins.fY; |
|
if( span < kRealSmall ) |
|
{ |
|
fExtFlags |= kAxisOneZero; |
|
span = 1.f; |
|
} |
|
v.Set(0, span, 0); |
|
fAxes[1] = *m * v; |
|
span = fMaxs.fZ - fMins.fZ; |
|
if( span < kRealSmall ) |
|
{ |
|
fExtFlags |= kAxisTwoZero; |
|
span = 1.f; |
|
} |
|
v.Set(0, 0, span); |
|
fAxes[2] = *m * v; |
|
|
|
} |
|
else |
|
{ |
|
#if 0 // IDENT |
|
if( m->fFlags & hsMatrix44::kIsIdent ) |
|
return; |
|
#endif // IDENT |
|
|
|
fCorner = *m * fCorner; |
|
fAxes[0] = *m * fAxes[0]; |
|
fAxes[1] = *m * fAxes[1]; |
|
fAxes[2] = *m * fAxes[2]; |
|
|
|
fExtFlags &= kAxisZeroZero|kAxisOneZero|kAxisTwoZero; |
|
} |
|
IMakeMinsMaxs(); |
|
fBounds3Flags &= ~kCenterValid; |
|
} |
|
|
|
void hsBounds3Ext::Translate(const hsVector3 &v) |
|
{ |
|
if( fType != kBoundsNormal ) |
|
return; |
|
|
|
fMins += v; |
|
fMaxs += v; |
|
if( fBounds3Flags & kCenterValid ) |
|
fCenter += v; |
|
if( !(fExtFlags & kAxisAligned) ) |
|
{ |
|
fCorner += v; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float d; |
|
d = fAxes[i].InnerProduct(v); |
|
fDists[i].fX += d; |
|
fDists[i].fY += d; |
|
} |
|
} |
|
} |
|
|
|
bool hsBounds3Ext::IsInside(const hsPoint3 *p) const |
|
{ |
|
if( fExtFlags & kAxisAligned ) |
|
return hsBounds3::IsInside(p); |
|
|
|
if( !(fExtFlags & kDistsSet) ) |
|
IMakeDists(); |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float diss = p->InnerProduct(fAxes[i]); |
|
if( (diss < fDists[i].fX) |
|
||(diss > fDists[i].fY) ) |
|
return false; |
|
} |
|
return true; |
|
|
|
} |
|
|
|
// neg, pos, zero == disjoint, I contain other, overlap |
|
int32_t hsBounds3Ext::TestBound(const hsBounds3Ext& other) const |
|
{ |
|
if( fExtFlags & kAxisAligned ) |
|
return hsBounds3::TestBound(other); |
|
|
|
if( !(fExtFlags & kDistsSet) ) |
|
IMakeDists(); |
|
|
|
int32_t retVal = 1; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
hsPoint2 depth; |
|
other.TestPlane(fAxes[i], depth); |
|
|
|
if( fDists[i].fX > depth.fY ) |
|
return -1; |
|
if( fDists[i].fY < depth.fX ) |
|
return -1; |
|
|
|
if( fDists[i].fY < depth.fY ) |
|
retVal = 0; |
|
if( fDists[i].fX > depth.fX ) |
|
retVal = 0; |
|
} |
|
return retVal; |
|
} |
|
|
|
|
|
void hsBounds3Ext::TestPlane(const hsVector3 &n, hsPoint2 &depth) const |
|
{ |
|
hsAssert(fType == kBoundsNormal, "TestPlane only valid for kBoundsNormal filled bounds"); |
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
hsBounds3::TestPlane(n, depth); |
|
} |
|
else |
|
{ |
|
float dmax = fCorner.InnerProduct(n); |
|
float dmin = dmax; |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
if( !IAxisIsZero(i) ) |
|
{ |
|
float d; |
|
d = fAxes[i].InnerProduct(n); |
|
if( d < 0 ) |
|
dmin += d; |
|
else |
|
dmax += d; |
|
} |
|
} |
|
|
|
depth.fX = dmin; |
|
depth.fY = dmax; |
|
} |
|
} |
|
|
|
void hsBounds3Ext::TestPlane(const hsPlane3 *p, const hsVector3 &myVel, hsPoint2 &depth) const |
|
{ |
|
TestPlane(p->fN, myVel, depth); |
|
} |
|
void hsBounds3Ext::TestPlane(const hsVector3 &n, const hsVector3 &myVel, hsPoint2 &depth) const |
|
{ |
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
float dmax = fMins.InnerProduct(n); |
|
float dmin = dmax; |
|
float dvel = myVel.InnerProduct(n); |
|
if( dvel < 0 ) |
|
dmin += dvel; |
|
else |
|
dmax += dvel; |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float dd; |
|
dd = fMaxs[i] - fMins[i]; |
|
dd *= n[i]; |
|
|
|
if( dd < 0 ) |
|
dmin += dd; |
|
else |
|
dmax += dd; |
|
} |
|
|
|
depth.fX = dmin; |
|
depth.fY = dmax; |
|
} |
|
else |
|
{ |
|
float dmax = fCorner.InnerProduct(n); |
|
float dmin = dmax; |
|
float dvel = myVel.InnerProduct(n); |
|
if( dvel < 0 ) |
|
dmin += dvel; |
|
else |
|
dmax += dvel; |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
if( !IAxisIsZero(i) ) |
|
{ |
|
float d; |
|
d = fAxes[i].InnerProduct(n); |
|
if( d < 0 ) |
|
dmin += d; |
|
else |
|
dmax += d; |
|
} |
|
} |
|
|
|
depth.fX = dmin; |
|
depth.fY = dmax; |
|
} |
|
} |
|
|
|
int32_t hsBounds3Ext::TestPoints(int n, const hsPoint3 *pList, const hsVector3 &ptVel) const |
|
{ |
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
int32_t retVal = -1; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float effMax = fMaxs[i]; |
|
float effMin = fMins[i]; |
|
if( ptVel[i] < 0 ) |
|
effMax -= ptVel[i]; |
|
else |
|
effMin -= ptVel[i]; |
|
|
|
int j; |
|
const uint32_t low = 0x1, hi = 0x2; |
|
uint32_t mask = low | hi; |
|
for( j = 0; j < n; j++ ) |
|
{ |
|
if( pList[j][i] > effMin ) |
|
mask &= ~low; |
|
if( pList[j][i] < effMax ) |
|
mask &= ~hi; |
|
if( mask ) |
|
retVal = 0; |
|
} |
|
if( mask ) |
|
return 1; |
|
} |
|
return retVal; |
|
} |
|
else // non-axis aligned case |
|
{ |
|
int32_t retVal = -1; // all inside |
|
if( !(fExtFlags & kDistsSet) ) |
|
IMakeDists(); |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float diff = fAxes[i].InnerProduct(ptVel); |
|
bool someLow = false; |
|
bool someHi = false; |
|
bool someIn = false; |
|
int j; |
|
for( j = 0; j < n; j++ ) |
|
{ |
|
float d = fAxes[i].InnerProduct(pList[j]); |
|
float ddiff = d + diff; |
|
if( d < fDists[i].fX ) |
|
someLow = true; |
|
else if( d > fDists[i].fY ) |
|
someHi = true; |
|
else |
|
someIn = true; |
|
|
|
if( ddiff < fDists[i].fX ) |
|
someLow = true; |
|
else if( ddiff > fDists[i].fY ) |
|
someHi = true; |
|
else |
|
someIn = true; |
|
|
|
if( someIn &&(someHi || someLow) ) |
|
break; |
|
} |
|
if( someHi && !(someLow || someIn) ) |
|
return 1; |
|
if( someLow && !(someHi || someIn) ) |
|
return 1; |
|
if( someHi || someLow ) |
|
retVal = 0; |
|
} |
|
return retVal; |
|
} |
|
} |
|
|
|
int32_t hsBounds3Ext::TestPoints(int n, const hsPoint3 *pList) const |
|
{ |
|
bool someIn = false; |
|
bool someOut = false; |
|
int i; |
|
for( i = 0; i < n; i++ ) |
|
{ |
|
if( IsInside(pList+i) ) |
|
someIn = true; |
|
else |
|
someOut = true; |
|
if( someIn && someOut ) |
|
return 0; |
|
} |
|
if( someIn ) |
|
return -1; |
|
return 1; |
|
} |
|
|
|
bool hsBounds3Ext::ClosestPoint(const hsPoint3& p, hsPoint3& inner, hsPoint3& outer) const |
|
{ |
|
if( fExtFlags & kAxisAligned ) |
|
return hsBounds3::ClosestPoint(p, inner, outer); |
|
|
|
if( !(fExtFlags & kDistsSet) ) |
|
IMakeDists(); |
|
|
|
int nSect = 0; |
|
inner = outer = fCorner; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float dist = fAxes[i].InnerProduct(p); |
|
if( dist < fDists[i].fX ) |
|
{ |
|
outer += fAxes[i]; |
|
} |
|
else if( dist > fDists[i].fY ) |
|
{ |
|
inner += fAxes[i]; |
|
} |
|
else |
|
{ |
|
float t = (dist - fDists[i].fX) / (fDists[i].fY - fDists[i].fX); |
|
inner += t * fAxes[i]; |
|
if( t > 0.5f ) |
|
outer += fAxes[i]; |
|
|
|
nSect++; |
|
} |
|
} |
|
return nSect == 3; |
|
} |
|
|
|
bool hsBounds3Ext::ISectBB(const hsBounds3Ext &other, const hsVector3 &myVel) const |
|
{ |
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
if( other.fExtFlags & kAxisAligned ) |
|
return ISectABB(other, myVel); |
|
|
|
return other.ISectBB(*this, -myVel); |
|
} |
|
hsAssert(!(fExtFlags & kAxisAligned), "Other can be axis-aligned, but not me!"); |
|
hsPoint2 depth; |
|
|
|
if( !(fExtFlags & kDistsSet) ) |
|
IMakeDists(); |
|
if( !(other.fExtFlags & (kDistsSet|kAxisAligned)) ) |
|
other.IMakeDists(); |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
other.TestPlane(fAxes[i], -myVel, depth); |
|
if( (depth.fX > fDists[i].fY) |
|
||(depth.fY < fDists[i].fX) ) |
|
return false; |
|
|
|
if( other.fExtFlags & kAxisAligned ) |
|
{ |
|
float myMin = fMins[i]; |
|
float myMax = fMaxs[i]; |
|
if( myVel[i] < 0 ) |
|
myMin += myVel[i]; |
|
else |
|
myMax += myVel[i]; |
|
if( (other.fMins[i] > myMax) |
|
||(other.fMaxs[i] < myMin) ) |
|
return false; |
|
} |
|
else |
|
{ |
|
TestPlane(other.fAxes[i], myVel, depth); |
|
if( (depth.fX > other.fDists[i].fY) |
|
||(depth.fY < other.fDists[i].fX) ) |
|
return false; |
|
} |
|
|
|
// still leaves the 3 axes of origAxis.Cross(myVel) |
|
hsVector3 ax = fAxes[i] % myVel; |
|
float dmax = fCorner.InnerProduct(ax); |
|
float dmin = dmax; |
|
int j = i+1; |
|
if( 3 == j )j = 0; |
|
float d; |
|
d = fAxes[j].InnerProduct(ax); |
|
if( d < 0 ) |
|
dmin += d; |
|
else |
|
dmax += d; |
|
j = ( j == 2 ? 0 : j+1 ); |
|
d = fAxes[j].InnerProduct(ax); |
|
if( d < 0 ) |
|
dmin += d; |
|
else |
|
dmax += d; |
|
other.TestPlane(ax, depth); |
|
if( (depth.fX > dmax) |
|
||(depth.fY < dmin) ) |
|
return false; |
|
} |
|
|
|
|
|
return true; |
|
} |
|
|
|
|
|
static bool ISectInterval(const hsPoint2& other, const hsPoint2& mine) |
|
{ |
|
if( other.fY - mine.fX <= 0 ) |
|
return false; |
|
if( mine.fY - other.fX <= 0 ) |
|
return false; |
|
|
|
return true; |
|
} |
|
|
|
static bool ITestDepth(const hsPoint2& other, const hsPoint2& mine, |
|
const hsVector3& inAx, |
|
hsVector3 &outAx, float& depth) |
|
{ |
|
depth = 0; |
|
float d0, d1; |
|
d0 = other.fY - mine.fX; |
|
if( d0 <= 0 ) |
|
return false; |
|
d1 = mine.fY - other.fX; |
|
if( d1 <= 0 ) |
|
return false; |
|
|
|
// if one interval is proper subset of other, skip |
|
if( (mine.fX < other.fX)^(mine.fY < other.fY) ) |
|
{ |
|
depth = 0; |
|
return true; |
|
} |
|
if( d0 < d1 ) |
|
{ |
|
outAx = inAx; |
|
depth = d0; |
|
return true; |
|
} |
|
|
|
outAx = -inAx; |
|
depth = d1; |
|
return true; |
|
} |
|
|
|
int32_t hsBounds3Ext::IClosestISect(const hsBounds3Ext& other, const hsVector3& myVel, |
|
float* tClose, float* tImpact) const |
|
{ |
|
// Should assert both have their spheres set. |
|
|
|
hsVector3 meToOt(&other.GetCenter(), &GetCenter()); |
|
|
|
// cTerm = (myCenter - otCenter)^2 - (myRad + otRad)^2 |
|
float cTerm; |
|
|
|
cTerm = GetRadius() + other.GetRadius(); |
|
cTerm *= -cTerm; |
|
|
|
float meToOtLen = meToOt.MagnitudeSquared(); |
|
cTerm += meToOtLen; |
|
if( cTerm <= 0 ) |
|
{ |
|
*tClose = *tImpact = 0; |
|
return -1; // started off in contact |
|
} |
|
|
|
float ooATerm = myVel.InnerProduct(myVel); |
|
if( ooATerm < hsBounds::kRealSmall ) |
|
{ |
|
*tClose = *tImpact = 0; |
|
return 0; |
|
} |
|
ooATerm = hsInvert(ooATerm); |
|
|
|
float bTerm = myVel.InnerProduct(meToOt); |
|
bTerm *= ooATerm; |
|
float bSqTerm = bTerm * bTerm; |
|
// bTerm is t for closest point to line |
|
|
|
float det = bSqTerm - ooATerm * cTerm; |
|
if( det < 0 ) |
|
{ |
|
*tClose = *tImpact = bTerm; |
|
return 0; |
|
} |
|
|
|
det = sqrt(det); |
|
*tClose = bTerm; |
|
*tImpact = bTerm - det; |
|
|
|
return 1; |
|
} |
|
|
|
void hsBounds3Ext::Unalign() |
|
{ |
|
fExtFlags = 0; |
|
|
|
fCorner = fMins; |
|
hsVector3 v; |
|
float span; |
|
span = fMaxs.fX - fMins.fX; |
|
if( span < kRealSmall ) |
|
{ |
|
fExtFlags |= kAxisZeroZero; |
|
span = 1.f; |
|
} |
|
fAxes[0].Set(span, 0, 0); |
|
span = fMaxs.fY - fMins.fY; |
|
if( span < kRealSmall ) |
|
{ |
|
fExtFlags |= kAxisOneZero; |
|
span = 1.f; |
|
} |
|
fAxes[1].Set(0, span, 0); |
|
span = fMaxs.fZ - fMins.fZ; |
|
if( span < kRealSmall ) |
|
{ |
|
fExtFlags |= kAxisTwoZero; |
|
span = 1.f; |
|
} |
|
fAxes[2].Set(0, 0, span); |
|
} |
|
|
|
bool hsBounds3Ext::ISectBB(const hsBounds3Ext &other, const hsVector3 &myVel, hsHitInfoExt *hit) const |
|
{ |
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
hsBounds3Ext meUnalign(*this); |
|
meUnalign.Unalign(); |
|
return meUnalign.ISectBB(other, myVel, hit); |
|
} |
|
hsAssert(!(fExtFlags & kAxisAligned), "Other can be axis-aligned, but not me!"); |
|
hsPoint2 depth; |
|
|
|
if( !(fExtFlags & kDistsSet) ) |
|
IMakeDists(); |
|
if( !(other.fExtFlags & (kDistsSet|kAxisAligned)) ) |
|
other.IMakeDists(); |
|
|
|
const float kRealBig = 1.e30f; |
|
float tstDepths[9]; |
|
hsVector3 tstAxes[9]; |
|
float totDepth = 0; |
|
int nDeep = 0; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
const float kFavorConstant = 0.01f; // smaller is favored |
|
|
|
other.TestPlane(fAxes[i], -myVel, depth); |
|
|
|
if( !ITestDepth(depth, fDists[i], fAxes[i], tstAxes[i], tstDepths[i]) ) |
|
return false; |
|
|
|
other.TestPlane(fAxes[i], depth); |
|
if( !ISectInterval(depth, fDists[i]) ) |
|
tstDepths[i] *= kFavorConstant; |
|
|
|
if( tstDepths[i] > 0 ) |
|
{ |
|
totDepth += tstDepths[i]; |
|
nDeep++; |
|
} |
|
|
|
if( other.fExtFlags & kAxisAligned ) |
|
{ |
|
hsPoint2 mine; |
|
mine.fX = fMins[i]; |
|
mine.fY = fMaxs[i]; |
|
if( myVel[i] > 0 )mine.fY += myVel[i]; |
|
else mine.fX += myVel[i]; |
|
depth.fX = other.fMins[i]; |
|
depth.fY = other.fMaxs[i]; |
|
|
|
hsVector3 ax; |
|
ax.Set( 0 == i ? 1.f : 0, |
|
1 == i ? 1.f : 0, |
|
2 == i ? 1.f : 0); |
|
|
|
if( !ITestDepth(depth, mine, ax, tstAxes[i+3], tstDepths[i+3]) ) |
|
return false; |
|
|
|
mine.fX = fMins[i]; |
|
mine.fY = fMaxs[i]; |
|
if( !ISectInterval(depth, mine) ) |
|
tstDepths[i+3] *= kFavorConstant; |
|
|
|
if( tstDepths[i+3] ) |
|
{ |
|
totDepth += tstDepths[i+3]; |
|
nDeep++; |
|
} |
|
} |
|
else |
|
{ |
|
TestPlane(other.fAxes[i], myVel, depth); |
|
|
|
if( !ITestDepth(other.fDists[i], depth, other.fAxes[i], tstAxes[i+3], tstDepths[i+3]) ) |
|
return false; |
|
|
|
TestPlane(other.fAxes[i], depth); |
|
|
|
if( !ISectInterval(other.fDists[i], depth) ) |
|
tstDepths[i+3] *= kFavorConstant; |
|
|
|
if( tstDepths[i+3] ) |
|
{ |
|
totDepth += tstDepths[i+3]; |
|
nDeep++; |
|
} |
|
} |
|
|
|
#if 0 |
|
// still leaves the 3 axes of origAxis.Cross(myVel) |
|
hsVector3 ax = fAxes[i] % myVel; |
|
if( ax.MagnitudeSquared() > kRealSmall ) |
|
{ |
|
hsPoint2 myDepth; |
|
myDepth.fX = myDepth.fY = fCorner.InnerProduct(ax); |
|
float d; |
|
int j = i == 2 ? 0 : i+1; |
|
if( !IAxisIsZero(j) ) |
|
{ |
|
d = fAxes[j].InnerProduct(ax); |
|
if( d < 0 ) |
|
myDepth.fX += d; |
|
else |
|
myDepth.fY += d; |
|
} |
|
j = ( j == 2 ? 0 : j+1 ); |
|
if( !IAxisIsZero(j) ) |
|
{ |
|
d = fAxes[j].InnerProduct(ax); |
|
if( d < 0 ) |
|
myDepth.fX += d; |
|
else |
|
myDepth.fY += d; |
|
} |
|
other.TestPlane(ax, depth); |
|
|
|
if( !ITestDepth(depth, myDepth, ax, tstAxes[i+6], tstDepths[i+6]) ) |
|
return false; |
|
totDepth += tstDepths[i+6]; |
|
} |
|
else |
|
tstDepths[i+6] = 0; |
|
#endif |
|
} |
|
|
|
hsVector3 norm; |
|
if( totDepth <= 0 ) |
|
{ |
|
float t, tIgnore; |
|
IClosestISect(other, myVel, &tIgnore, &t); |
|
if( t < 0 ) |
|
t = 0; |
|
else if( t > 1.f ) |
|
t = 1.f; |
|
hsPoint3 hitPt = GetCenter() + myVel * t; |
|
norm.Set(&hitPt, &other.GetCenter()); |
|
} |
|
else |
|
{ |
|
// now do a weighted average of the axes |
|
hsAssert(totDepth > 0, "nobody home"); |
|
norm.Set(0,0,0); |
|
for( i =0; i < 6; i++ ) |
|
{ |
|
if( tstDepths[i] > 0 ) |
|
norm += tstAxes[i] / tstDepths[i]; |
|
// norm += tstAxes[i] * (1.f - tstDepths[i] / totDepth); |
|
} |
|
} |
|
hsPoint2 otherDepth; |
|
norm.Normalize(); |
|
other.TestPlane(norm, otherDepth); |
|
TestPlane(norm, myVel, depth); |
|
|
|
hit->Set(this, &other, norm, otherDepth.fY - depth.fX); |
|
|
|
// mf horse hack test |
|
if( hit->fDepth < 0 ) |
|
return false; |
|
hsAssert(hit->fDepth >= 0, "Negative Depth"); |
|
|
|
return true; |
|
} |
|
|
|
bool hsBounds3Ext::ISectABB(const hsBounds3Ext &other, const hsVector3 &myVel) const |
|
{ |
|
hsPoint3 effMaxs = fMaxs; |
|
hsPoint3 effMins = fMins; |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float effMax = fMaxs[i]; |
|
float effMin = fMins[i]; |
|
if( myVel[i] > 0 ) |
|
effMax += myVel[i]; |
|
else |
|
effMin += myVel[i]; |
|
|
|
if( (effMax < other.fMins[i]) |
|
||(effMin > other.fMaxs[i]) ) |
|
return false; |
|
} |
|
return true; |
|
} |
|
|
|
bool hsBounds3Ext::ISectBS(const hsBounds3Ext &other, const hsVector3 &myVel) const |
|
{ |
|
if( !(fExtFlags & kSphereSet) ) |
|
IMakeSphere(); |
|
if( !(other.fExtFlags & kSphereSet) ) |
|
other.IMakeSphere(); |
|
|
|
hsPoint3 closestPt = GetCenter(); |
|
// we should know whether we have a useful velocity or not... |
|
// having the speed cached away would get rid of several |
|
// such uglies... |
|
if( myVel.MagnitudeSquared() > 0 ) |
|
{ |
|
float parm = hsVector3(&other.GetCenter(), &fCenter).InnerProduct(myVel) |
|
/ myVel.InnerProduct(myVel); |
|
if( parm > 0 ) |
|
{ |
|
if( parm > 1.f ) |
|
parm = 1.f; |
|
closestPt += myVel * parm; |
|
} |
|
} |
|
|
|
float combRad = fRadius + other.fRadius; |
|
|
|
return hsVector3(&closestPt, &other.GetCenter()).MagnitudeSquared() < combRad*combRad; |
|
} |
|
|
|
#if 0 // Commenting out this which will be made redundant and/or obsolete by Havok integration |
|
bool hsBounds3Ext::ISectTriABB(hsBounds3Tri &tri, const hsVector3 &myVel) const |
|
{ |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float effMax = fMaxs[i]; |
|
float effMin = fMins[i]; |
|
if( myVel[i] < 0 ) |
|
effMin += myVel[i]; |
|
else |
|
effMax += myVel[i]; |
|
|
|
int j; |
|
const uint32_t low = 0x1, hi = 0x2; |
|
uint32_t mask = low | hi; |
|
for( j = 0; j < 3; j++ ) |
|
{ |
|
if( tri.fVerts[j][i] > effMin ) |
|
mask &= ~low; |
|
if( tri.fVerts[j][i] < effMax ) |
|
mask &= ~hi; |
|
} |
|
if( mask ) |
|
return false; |
|
} |
|
return true; |
|
} |
|
|
|
bool hsBounds3Ext::TriBSHitInfo(hsBounds3Tri& tri, const hsVector3& myVel, hsHitInfoExt* hit) const |
|
{ |
|
hsPoint3 myPt = GetCenter(); |
|
myPt += myVel; |
|
|
|
hsPoint3 closePt; |
|
bool onTri = tri.ClosestTriPoint(&myPt, &closePt); |
|
|
|
hsVector3 repel; |
|
repel.Set(&myPt, &closePt); |
|
|
|
float myDepth; |
|
float repelMagSq = repel.MagnitudeSquared(); |
|
if( repelMagSq < hsBounds::kRealSmall ) |
|
{ |
|
repel = tri.fNormal; |
|
myDepth = GetRadius(); |
|
} |
|
else |
|
{ |
|
myDepth = hsFastMath::InvSqrt(repelMagSq); |
|
repel *= myDepth; |
|
myDepth = 1.f / myDepth; |
|
myDepth = GetRadius() - myDepth; |
|
if( myDepth < 0 ) |
|
myDepth = 0; |
|
} |
|
|
|
if( tri.fNormal.InnerProduct(myPt) < tri.fDist ) |
|
{ |
|
repel += tri.fNormal * (-2.f * repel.InnerProduct(tri.fNormal)); |
|
myDepth = GetRadius() * 2.f - myDepth; |
|
if( myDepth < 0 ) |
|
myDepth = 0; |
|
} |
|
|
|
hit->Set(this, &tri, &repel, myDepth); |
|
|
|
return true; |
|
} |
|
|
|
#if 0 // TOCENTER |
|
bool hsBounds3Ext::TriBBHitInfo(hsBounds3Tri& tri, const hsVector3& myVel, hsHitInfoExt* hit) const |
|
{ |
|
// Find our closest point (after movement) |
|
hsPoint3 myPt = fCorner; |
|
myPt += myVel; |
|
|
|
const float kMinDist = 1.f; // Huge min dist because world is really big right now. mf horse |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float axDot = fAxes[i].InnerProduct(tri.fNormal); |
|
if( axDot < -kMinDist ) |
|
{ |
|
// moving towards |
|
myPt += fAxes[i]; |
|
} |
|
else if( axDot < kMinDist ) |
|
{ |
|
// need to interp |
|
axDot /= -(kMinDist*2.f); |
|
axDot += 0.5f; |
|
myPt += fAxes[i] * axDot; |
|
} |
|
// else moving away, skip it |
|
} |
|
|
|
// Find closest point on tri to our closest corner |
|
hsPoint3 closePt; |
|
bool onTri = tri.ClosestTriPoint(&myPt, &closePt); |
|
|
|
// Repel vector is from closest corner to closest point on tri |
|
hsVector3 repel; |
|
repel.Set(&myPt, &closePt); |
|
repel += (-2.f * repel.InnerProduct(tri.fNormal)) * tri.fNormal; |
|
|
|
float repelMag = hsFastMath::InvSqrt(repel.MagnitudeSquared()); |
|
|
|
if( repelMag < hsBounds::kRealSmall ) |
|
{ |
|
hsPoint2 faceDepth; |
|
TestPlane(tri.fNormal, myVel, faceDepth); |
|
hit->Set(this, &tri, &tri.fNormal, tri.fDist - faceDepth.fX); |
|
return true; |
|
} |
|
|
|
repel *= repelMag; |
|
repelMag = 1.f / repelMag; |
|
|
|
hit->Set(this, &tri, &repel, repelMag); |
|
|
|
// Return true of our closest corner projects on to tri (along normal or myVel?) |
|
return onTri; |
|
} |
|
#else // TOCENTER |
|
|
|
bool hsBounds3Ext::TriBBHitInfo(hsBounds3Tri& tri, const hsVector3& myVel, hsHitInfoExt* hit) const |
|
{ |
|
hsPoint3 myPt = GetCenter(); |
|
myPt += myVel; |
|
|
|
hsPoint3 closePt; |
|
bool onTri = tri.ClosestTriPoint(&myPt, &closePt); |
|
|
|
hsVector3 repel; |
|
repel.Set(&myPt, &closePt); |
|
float repelDotNorm = repel.InnerProduct(tri.fNormal); |
|
if( repelDotNorm < 0 ) |
|
{ |
|
repel += (-2.f * repelDotNorm) * tri.fNormal; |
|
} |
|
|
|
float repelMagSq = repel.MagnitudeSquared(); |
|
if( repelMagSq < hsBounds::kRealSmall ) |
|
repel = tri.fNormal; |
|
else |
|
{ |
|
float repelMag = hsFastMath::InvSqrt(repelMagSq); |
|
repel *= repelMag; |
|
} |
|
|
|
|
|
hsPoint2 triDepth; |
|
tri.TestPlane(repel, triDepth); |
|
|
|
hsPoint2 myDepth; |
|
TestPlane(repel, myVel, myDepth); |
|
|
|
hit->Set(this, &tri, &repel, triDepth.fY - myDepth.fX); |
|
|
|
return true; |
|
} |
|
|
|
#endif // TOCENTER |
|
|
|
bool hsBounds3Ext::ISectTriBB(hsBounds3Tri &tri, const hsVector3 &myVel) const |
|
{ |
|
hsPoint2 faceDepth; |
|
// first test box against the triangle plane |
|
TestPlane(tri.fNormal, myVel, faceDepth); |
|
|
|
if( (tri.fDist > faceDepth.fY) |
|
||(tri.fDist < faceDepth.fX) ) |
|
return false; |
|
|
|
// now test tri against box planes |
|
if( TestPoints(3, tri.fVerts, -myVel) > 0 ) |
|
return false; |
|
|
|
if( !(tri.fTriFlags & hsBounds3Tri::kAxesSet) ) |
|
tri.SetAxes(); |
|
|
|
float depth = tri.fDist - faceDepth.fX; |
|
hsVector3 norm = tri.fNormal; |
|
|
|
// that only leaves the planes of triEdge.Cross(vel) |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
hsPoint2 depths; |
|
TestPlane(tri.fPerpAxes[i], myVel, depths); |
|
if( (tri.fPerpDists[i].fY < depths.fX) |
|
||(tri.fPerpDists[i].fX > depths.fY) ) |
|
return false; |
|
|
|
#if 0 |
|
float testDepth = tri.fPerpDists[i].fY - depths.fX; |
|
if( testDepth < depth ) |
|
{ |
|
depth = testDepth; |
|
norm = tri.fPerpAxes[i]; |
|
} |
|
#endif |
|
} |
|
float vDotN = myVel.InnerProduct(tri.fNormal); |
|
if( vDotN > 0 ) |
|
depth -= vDotN; |
|
|
|
if( depth <= 0 ) |
|
return false; |
|
|
|
return true; |
|
} |
|
|
|
bool hsBounds3Ext::ISectTriBB(hsBounds3Tri &tri, const hsVector3 &myVel, hsHitInfoExt *hit) const |
|
{ |
|
hsPoint2 faceDepth; |
|
// first test box against the triangle plane |
|
TestPlane(tri.fNormal, myVel, faceDepth); |
|
|
|
if( (tri.fDist > faceDepth.fY) |
|
||(tri.fDist < faceDepth.fX) ) |
|
return false; |
|
|
|
float centDist = tri.fNormal.InnerProduct(hit->fRootCenter); |
|
if( centDist < tri.fDist ) |
|
return false; |
|
|
|
// now test tri against box planes |
|
if( TestPoints(3, tri.fVerts, -myVel) > 0 ) |
|
return false; |
|
|
|
if( !(tri.fTriFlags & hsBounds3Tri::kAxesSet) ) |
|
tri.SetAxes(); |
|
|
|
float depth = tri.fDist - faceDepth.fX; |
|
hsVector3 norm = tri.fNormal; |
|
|
|
// that only leaves the planes of triEdge.Cross(vel) |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
hsPoint2 depths; |
|
TestPlane(tri.fPerpAxes[i], myVel, depths); |
|
if( (tri.fPerpDists[i].fY < depths.fX) |
|
||(tri.fPerpDists[i].fX > depths.fY) ) |
|
return false; |
|
|
|
#if 0 |
|
float testDepth = tri.fPerpDists[i].fY - depths.fX; |
|
if( testDepth < depth ) |
|
{ |
|
depth = testDepth; |
|
norm = tri.fPerpAxes[i]; |
|
} |
|
#endif |
|
} |
|
float vDotN = myVel.InnerProduct(tri.fNormal); |
|
if( vDotN > 0 ) |
|
depth -= vDotN; |
|
|
|
if( (tri.fTriFlags & hsBounds3Tri::kDoubleSide) ) |
|
{ |
|
if( tri.fNormal.InnerProduct(hit->fRootCenter) - tri.fDist < 0 ) |
|
{ |
|
depth = -tri.fDist + faceDepth.fY; |
|
if( vDotN < 0 ) |
|
depth += vDotN; |
|
|
|
tri.fNormal = -tri.fNormal; |
|
tri.fDist = -tri.fDist; |
|
} |
|
} |
|
if( depth <= 0 ) |
|
return false; |
|
|
|
// printf("ATTRIBUTE triBnd addr %x\n",&tri.fNormal); /* Takashi Nakata TEST Add */ |
|
hit->Set(this, &tri, &norm, depth); |
|
|
|
return hit->fDepth > hsBounds::kRealSmall; |
|
} |
|
|
|
bool hsBounds3Ext::ISectTriBS(hsBounds3Tri &tri, const hsVector3 &myVel) const |
|
{ |
|
if( !(fExtFlags & kSphereSet) ) |
|
IMakeSphere(); |
|
|
|
hsAssert(fBounds3Flags & kCenterValid, "Sphere set but not center (TriBS)"); |
|
float radScaled = fRadius * tri.fNormal.Magnitude(); |
|
float centerDist = tri.fNormal.InnerProduct(fCenter); |
|
float velDist = tri.fNormal.InnerProduct(myVel); |
|
float effMin = centerDist; |
|
float effMax = centerDist; |
|
|
|
if( velDist > 0 ) |
|
effMax += velDist; |
|
else |
|
effMin += velDist; |
|
effMax += radScaled; |
|
effMin -= radScaled; |
|
|
|
if( tri.fDist <= effMin ) |
|
return false; |
|
if( tri.fDist >= effMax ) |
|
return false; |
|
|
|
// mf horse |
|
float normDepth = tri.fDist - (centerDist - radScaled + velDist); |
|
if( normDepth <= 0 ) |
|
{ |
|
// we'll report a depth of zero to (hopefully) neutralize any effects |
|
if( tri.fTriFlags & hsBounds3Tri::kDoubleSide ) |
|
{ |
|
normDepth = -tri.fDist + (centerDist + radScaled + velDist); |
|
if( normDepth > 0 ) |
|
{ |
|
tri.fDist = -tri.fDist; |
|
tri.fNormal = -tri.fNormal; |
|
} |
|
else |
|
normDepth = 0; |
|
} |
|
else |
|
normDepth = 0; |
|
} |
|
hsAssert(normDepth >= 0, "NegativeDepth"); |
|
|
|
if( !(tri.fTriFlags & hsBounds3Tri::kAxesSet) ) |
|
tri.SetAxes(); |
|
|
|
hsAssert(fBounds3Flags & kCenterValid, "Sphere set but not center (TriBS)"); |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
centerDist = tri.fPerpAxes[i].InnerProduct(fCenter); |
|
velDist = tri.fPerpAxes[i].InnerProduct(myVel); |
|
effMin = centerDist; |
|
effMax = centerDist; |
|
|
|
if( velDist > 0 ) |
|
effMax += velDist; |
|
else |
|
effMin += velDist; |
|
|
|
float radScale = fRadius * tri.fPerpAxes[i].Magnitude(); |
|
effMax += radScale; |
|
effMin -= radScale; |
|
if( tri.fPerpDists[i].fY <= effMin ) |
|
return false; |
|
if( tri.fPerpDists[i].fX >= effMax ) |
|
return false; |
|
|
|
} |
|
|
|
return true; |
|
} |
|
|
|
bool hsBounds3Ext::ISectTriBS(hsBounds3Tri &tri, const hsVector3 &myVel, hsHitInfoExt *hit) const |
|
{ |
|
if( !(fExtFlags & kSphereSet) ) |
|
IMakeSphere(); |
|
|
|
hsAssert(fBounds3Flags & kCenterValid, "Sphere set but not center (TriBS)"); |
|
float radScaled = fRadius * tri.fNormal.Magnitude(); |
|
float centerDist = tri.fNormal.InnerProduct(fCenter); |
|
float velDist = tri.fNormal.InnerProduct(myVel); |
|
float effMin = centerDist; |
|
float effMax = centerDist; |
|
|
|
if( velDist > 0 ) |
|
effMax += velDist; |
|
else |
|
effMin += velDist; |
|
effMax += radScaled; |
|
effMin -= radScaled; |
|
|
|
if( tri.fDist <= effMin ) |
|
return false; |
|
if( tri.fDist >= effMax ) |
|
return false; |
|
|
|
// mf horse |
|
float normDepth = tri.fDist - (centerDist - radScaled + velDist); |
|
if( normDepth <= 0 ) |
|
{ |
|
#if 0 // need to report the collision even if the object is leaving the tri |
|
// we'll report a depth of zero to (hopefully) neutralize any effects |
|
if(!(tri.fTriFlags & hsBounds3Tri::kDoubleSide) ) |
|
return false; |
|
normDepth = -tri.fDist + (centerDist + radScaled + velDist); |
|
if( normDepth <= 0 ) |
|
return false; |
|
tri.fDist = -tri.fDist; |
|
tri.fNormal = -tri.fNormal; |
|
#else |
|
// we'll report a depth of zero to (hopefully) neutralize any effects |
|
if( tri.fTriFlags & hsBounds3Tri::kDoubleSide ) |
|
{ |
|
normDepth = -tri.fDist + (centerDist + radScaled + velDist); |
|
if( normDepth > 0 ) |
|
{ |
|
tri.fDist = -tri.fDist; |
|
tri.fNormal = -tri.fNormal; |
|
} |
|
else |
|
normDepth = 0; |
|
} |
|
else |
|
normDepth = 0; |
|
#endif |
|
} |
|
hsAssert(normDepth >= 0, "NegativeDepth"); |
|
|
|
if( !(tri.fTriFlags & hsBounds3Tri::kAxesSet) ) |
|
tri.SetAxes(); |
|
|
|
hsAssert(fBounds3Flags & kCenterValid, "Sphere set but not center (TriBS)"); |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
centerDist = tri.fPerpAxes[i].InnerProduct(fCenter); |
|
velDist = tri.fPerpAxes[i].InnerProduct(myVel); |
|
effMin = centerDist; |
|
effMax = centerDist; |
|
|
|
if( velDist > 0 ) |
|
effMax += velDist; |
|
else |
|
effMin += velDist; |
|
|
|
float radScale = fRadius * tri.fPerpAxes[i].Magnitude(); |
|
effMax += radScale; |
|
effMin -= radScale; |
|
if( tri.fPerpDists[i].fY <= effMin ) |
|
return false; |
|
if( tri.fPerpDists[i].fX >= effMax ) |
|
return false; |
|
|
|
} |
|
|
|
float invLen = hsInvert(tri.fNormal.Magnitude()); |
|
hit->Set(this, &tri, &tri.fNormal, normDepth); |
|
|
|
// mf horse - move this into Set()? |
|
hit->fNormal *= invLen; |
|
hit->fDepth *= invLen; |
|
|
|
return true; |
|
} |
|
|
|
#endif // Commenting out this which will be made redundant and/or obsolete by Havok integration |
|
|
|
bool hsBounds3Ext::ISectBSBS(const hsBounds3Ext& other, const hsVector3& myVel, hsHitInfoExt *hit) const |
|
{ |
|
if(!(fExtFlags & kSphereSet) ) |
|
IMakeSphere(); |
|
if(!(other.fExtFlags & kSphereSet) ) |
|
other.IMakeSphere(); |
|
|
|
float tClose, tImpact; |
|
if( !IClosestISect(other, myVel, &tClose, &tImpact) ) |
|
return false; |
|
if( (tImpact < 0) || (tImpact > 1.f) ) |
|
return false; |
|
if( tClose < 0 ) |
|
tClose = 0; |
|
if( tClose > 1.f ) |
|
tClose = 1.f; |
|
|
|
hsPoint3 closePt = GetCenter() + myVel * tClose; |
|
hsVector3 del; |
|
del.Set(&closePt, &other.GetCenter()); |
|
|
|
float mag = del.Magnitude(); |
|
float depth = GetRadius() + other.GetRadius() - mag; |
|
if( depth <= 0 ) |
|
return false; |
|
|
|
hsPoint3 hitPt = GetCenter() + myVel * tImpact; |
|
hsVector3 norm; |
|
norm.Set(&hitPt, &other.GetCenter()); |
|
norm.Normalize(); |
|
|
|
hit->Set(this, &other, norm, depth); |
|
return true; |
|
} |
|
|
|
bool hsBounds3Ext::ISectBSBox(const hsBounds3Ext &other, const hsVector3 &myVel, hsHitInfoExt *hit) const |
|
{ |
|
hit->fDelPos = -myVel; |
|
if( other.ISectBoxBS(*this, hit->fDelPos, hit) ) |
|
{ |
|
hit->fNormal = -hit->fNormal; |
|
hit->fBoxBnd = this; |
|
hit->fOtherBoxBnd = &other; |
|
hit->fDelPos = myVel; |
|
|
|
return true; |
|
} |
|
|
|
hit->fDelPos = myVel; |
|
return false; |
|
} |
|
|
|
bool hsBounds3Ext::ISectBoxBS(const hsBounds3Ext &other, const hsVector3 &myVel, hsHitInfoExt *hit) const |
|
{ |
|
if(!(fExtFlags & kSphereSet) ) |
|
IMakeSphere(); |
|
hsAssert(fBounds3Flags & kCenterValid, "Sphere set but not center (BoxBS(vel))"); |
|
|
|
hsVector3 minAxis; |
|
float minDepth; |
|
bool haveAxis = false; |
|
hsVector3 tstAxis; |
|
float tstDepth; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
bool tryAxis; |
|
if( other.fExtFlags & kAxisAligned ) |
|
{ |
|
// first try the other box axes |
|
float effMin = fCenter[i]; |
|
float effMax = effMin; |
|
float velDist = myVel[i]; |
|
if( velDist > 0 ) |
|
effMax += velDist; |
|
else |
|
effMin += velDist; |
|
effMax += fRadius; |
|
effMin -= fRadius; |
|
|
|
if( effMax < other.fMins[i] ) |
|
return false; |
|
if( effMin > other.fMaxs[i] ) |
|
return false; |
|
|
|
if( (other.fMins[i] <= effMin) |
|
&&(other.fMaxs[i] <= effMax) ) |
|
{ |
|
tstDepth = other.fMaxs[i] - effMin; |
|
hsAssert(tstDepth > -kRealSmall, "Late to be finding sep axis"); |
|
tstAxis.Set(i == 0 ? 1.f : 0, i & 1 ? 1.f : 0, i & 2 ? 1.f : 0); |
|
tryAxis = true; |
|
} |
|
else |
|
if( (other.fMins[i] >= effMin) |
|
&&(other.fMaxs[i] >= effMax) ) |
|
{ |
|
tstDepth = effMax - other.fMins[i]; |
|
hsAssert(tstDepth > -kRealSmall, "Late to be finding sep axis"); |
|
tstAxis.Set(i == 0 ? -1.f : 0, i & 1 ? -1.f : 0, i & 2 ? -1.f : 0); |
|
tryAxis = true; |
|
} |
|
else |
|
tryAxis = false; |
|
} |
|
else |
|
{ |
|
// first try the other box axes |
|
float radScaled = fRadius * other.fAxes[i].Magnitude(); |
|
float centerDist = other.fAxes[i].InnerProduct(fCenter); |
|
float effMin = centerDist; |
|
float effMax = centerDist; |
|
float velDist = other.fAxes[i].InnerProduct(myVel); |
|
if( velDist > 0 ) |
|
effMax += velDist; |
|
else |
|
effMin += velDist; |
|
effMax += radScaled; |
|
effMin -= radScaled; |
|
|
|
if( !(other.fExtFlags & kDistsSet) ) |
|
other.IMakeDists(); |
|
|
|
if( effMax < other.fDists[i].fX ) |
|
return false; |
|
if( effMin > other.fDists[i].fY ) |
|
return false; |
|
|
|
if( centerDist <= other.fDists[i].fX ) |
|
{ |
|
tstDepth = effMax - other.fDists[i].fX; |
|
tstAxis = -other.fAxes[i]; |
|
hsAssert(tstDepth > -kRealSmall, "Late to be finding sep axis"); |
|
} |
|
else |
|
if( centerDist >= other.fDists[i].fY ) |
|
{ |
|
tstDepth = other.fDists[i].fY - effMin; |
|
tstAxis = other.fAxes[i]; |
|
hsAssert(tstDepth > -kRealSmall, "Late to be finding sep axis"); |
|
} |
|
else |
|
tryAxis = false; |
|
|
|
} |
|
if( tryAxis ) |
|
{ |
|
float magSq = tstAxis.MagnitudeSquared(); |
|
if( magSq > kRealSmall ) |
|
{ |
|
tstDepth *= tstDepth * hsInvert(magSq); |
|
if( !haveAxis||(tstDepth < minDepth) ) |
|
{ |
|
minDepth = tstDepth; |
|
minAxis = tstAxis; |
|
haveAxis = true; |
|
} |
|
hsAssert(!haveAxis || (minAxis.MagnitudeSquared() > kRealSmall), "Bogus"); |
|
} |
|
} |
|
} |
|
// now try the axis between the center of sphere and center of other box |
|
hsVector3 diag(&fCenter, &other.GetCenter()); |
|
if( !haveAxis && (diag.MagnitudeSquared() < kRealSmall) ) |
|
diag.Set(1.f, 0, 0); |
|
float effMin = diag.InnerProduct(fCenter); |
|
float effMax = effMin; |
|
float velDist = diag.InnerProduct(myVel); |
|
if( velDist > 0 ) |
|
effMax += velDist; |
|
else |
|
effMin += velDist; |
|
float radDist = fRadius * diag.Magnitude(); |
|
effMax += radDist; |
|
effMin -= radDist; |
|
hsPoint2 otherDepth; |
|
other.TestPlane(diag, otherDepth); |
|
if( effMax < otherDepth.fX ) |
|
return false; |
|
if( effMin > otherDepth.fY ) |
|
return false; |
|
|
|
tstAxis = diag; |
|
tstDepth = otherDepth.fY - effMin; |
|
float magSq = tstAxis.MagnitudeSquared(); |
|
if( magSq > 0 ) |
|
{ |
|
tstDepth *= tstDepth * hsInvert(magSq); |
|
if( !haveAxis ||(tstDepth < minDepth) ) |
|
{ |
|
minDepth = tstDepth; |
|
minAxis = tstAxis; |
|
} |
|
} |
|
|
|
float invMag = hsInvert(minAxis.Magnitude()); |
|
minAxis *= invMag; |
|
hsAssert(minDepth >= 0, "Late to find sep plane"); |
|
minDepth = sqrt(minDepth); |
|
hit->Set(this, &other, minAxis, minDepth); |
|
|
|
return true; |
|
} |
|
|
|
bool hsBounds3Ext::ISectBoxBS(const hsBounds3Ext &other, const hsVector3 &myVel) const |
|
{ |
|
if( !(fExtFlags & kSphereSet) ) |
|
IMakeSphere(); |
|
hsAssert(fBounds3Flags & kCenterValid, "Sphere set but not center (BoxBS)"); |
|
|
|
if( other.fExtFlags & kAxisAligned ) |
|
{ |
|
// first try the other box axes |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float effMin = fCenter[i]; |
|
float effMax = effMin; |
|
float velDist = myVel[i]; |
|
if( velDist > 0 ) |
|
effMax += velDist; |
|
else |
|
effMin += velDist; |
|
effMax += fRadius; |
|
effMin -= fRadius; |
|
|
|
if( effMax < other.fMins[i] ) |
|
return false; |
|
if( effMin > other.fMaxs[i] ) |
|
return false; |
|
} |
|
} |
|
else |
|
{ |
|
// first try the other box axes |
|
if( !(other.fExtFlags & kDistsSet) ) |
|
other.IMakeDists(); |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float effMin = other.fAxes[i].InnerProduct(fCenter); |
|
float effMax = effMin; |
|
float velDist = other.fAxes[i].InnerProduct(myVel); |
|
if( velDist > 0 ) |
|
effMax += velDist; |
|
else |
|
effMin += velDist; |
|
float radScaled = fRadius * other.fAxes[i].Magnitude(); |
|
effMax += radScaled; |
|
effMin -= radScaled; |
|
|
|
if( effMax < other.fDists[i].fX ) |
|
return false; |
|
if( effMin > other.fDists[i].fY ) |
|
return false; |
|
} |
|
} |
|
|
|
// now try the axis between the center of sphere and center of other box |
|
hsVector3 diag(&fCenter, &other.GetCenter()); |
|
float effMin = diag.InnerProduct(fCenter); |
|
float effMax = effMin; |
|
float velDist = diag.InnerProduct(myVel); |
|
if( velDist > 0 ) |
|
effMax += velDist; |
|
else |
|
effMin += velDist; |
|
float radDist = fRadius * diag.Magnitude(); |
|
effMax += radDist; |
|
effMin -= radDist; |
|
hsPoint2 otherDepth; |
|
other.TestPlane(diag, otherDepth); |
|
if( effMax < otherDepth.fX ) |
|
return false; |
|
if( effMin > otherDepth.fY ) |
|
return false; |
|
|
|
return true; |
|
} |
|
|
|
bool hsBounds3Ext::ISectLine(const hsPoint3* from, const hsPoint3* at) const |
|
{ |
|
if( !(fExtFlags & kSphereSet) ) |
|
IMakeSphere(); |
|
|
|
hsPoint3 onLine; |
|
float z = ClosestPointToLine(&fCenter, from, at, &onLine); |
|
|
|
float distSq = hsVector3(&onLine, &fCenter).MagnitudeSquared(); |
|
if( distSq >= fRadius*fRadius ) |
|
return false; |
|
|
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
if( ((*from)[i] < fMins[i])&&((*at)[i] < fMins[i]) ) |
|
return false; |
|
if( ((*from)[i] > fMaxs[i])&&((*at)[i] > fMaxs[i]) ) |
|
return false; |
|
} |
|
} |
|
else |
|
{ |
|
if( !(fExtFlags & kDistsSet) ) |
|
IMakeDists(); |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float d0 = fAxes[i].InnerProduct(from); |
|
float d1 = fAxes[i].InnerProduct(at); |
|
if( d0 < d1 ) |
|
{ |
|
if( d1 < fDists[i].fX ) |
|
return false; |
|
if( d0 > fDists[i].fY ) |
|
return false; |
|
} |
|
else |
|
{ |
|
if( d0 < fDists[i].fX ) |
|
return false; |
|
if( d1 > fDists[i].fY ) |
|
return false; |
|
} |
|
} |
|
} |
|
return true; |
|
} |
|
|
|
bool hsBounds3Ext::ISectCone(const hsPoint3* from, const hsPoint3* at, float radius) const |
|
{ |
|
if( !(fExtFlags & kSphereSet) ) |
|
IMakeSphere(); |
|
|
|
// expensive |
|
hsPoint3 onLine; |
|
ClosestPointToLine(&fCenter, from, at, &onLine); |
|
|
|
float distSq = hsVector3(&onLine, &fCenter).MagnitudeSquared(); |
|
float radiusSq = fRadius * fRadius; |
|
if (distSq - radius*radius >= radiusSq) |
|
return false; |
|
|
|
float dist = hsVector3(from, &onLine).Magnitude(); |
|
float len = hsVector3(from, at).Magnitude(); |
|
float partRadius = radius/len * dist; |
|
if (distSq - fRadius*fRadius - partRadius*partRadius >= 0) |
|
{ |
|
hsVector3 rayToCenter(&fCenter,&onLine); |
|
rayToCenter.Normalize(); |
|
|
|
hsPoint3 atEdge = *at + rayToCenter*radius; |
|
|
|
ClosestPointToLine(&fCenter, from, &atEdge, &onLine); |
|
|
|
distSq = hsVector3(&onLine, &fCenter).MagnitudeSquared(); |
|
if( distSq >= radiusSq ) |
|
return false; |
|
} |
|
|
|
// incorrect |
|
if( fExtFlags & kAxisAligned ) |
|
{ |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
if( ((*from)[i] < fMins[i])&&((*at)[i]+radius < fMins[i]) ) |
|
return false; |
|
if( ((*from)[i] > fMaxs[i])&&((*at)[i]-radius > fMaxs[i]) ) |
|
return false; |
|
} |
|
} |
|
else |
|
{ |
|
if( !(fExtFlags & kDistsSet) ) |
|
IMakeDists(); |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
ClosestPointToInfiniteLine(at, &fAxes[i], &onLine); |
|
hsVector3 atLine(&onLine,at); |
|
atLine.Normalize(); |
|
hsPoint3 atEdge = *at + atLine * radius; |
|
|
|
float d0 = fAxes[i].InnerProduct(*from); |
|
float d1 = fAxes[i].InnerProduct(atEdge); |
|
if( d0 < d1 ) |
|
{ |
|
if( d1 < fDists[i].fX ) |
|
return false; |
|
if( d0 > fDists[i].fY ) |
|
return false; |
|
} |
|
else |
|
{ |
|
if( d0 < fDists[i].fX ) |
|
return false; |
|
if( d1 > fDists[i].fY ) |
|
return false; |
|
} |
|
} |
|
} |
|
return true; |
|
} |
|
|
|
|
|
bool hsBounds3Ext::ISectRayBS(const hsPoint3& from, const hsPoint3& to, hsPoint3& at) const |
|
{ |
|
hsVector3 c2f(&from,&GetCenter()); |
|
hsVector3 f2t(&to,&from); |
|
float a = f2t.MagnitudeSquared(); |
|
float b = 2 * (c2f.InnerProduct(f2t)); |
|
float c = c2f.MagnitudeSquared() - GetRadius()*GetRadius(); |
|
|
|
float disc = b*b - 4*a*c; |
|
if (disc < 0) |
|
return false; |
|
else |
|
{ |
|
float discSqrt = sqrt(disc); |
|
float denom = 1.f/(2*a); |
|
float t = (-b - discSqrt) * denom; |
|
|
|
if (t<1 && t>0) |
|
at = from + (f2t * t); |
|
else |
|
return false; |
|
#if 0 |
|
{ |
|
t = (-b + discSqrt) * denom; |
|
if (t > 1) |
|
return false; |
|
at = from + (f2t * t); |
|
} |
|
#endif |
|
return true; |
|
} |
|
} |
|
|
|
void hsBounds3Ext::Read(hsStream *s) |
|
{ |
|
fExtFlags = s->ReadLE32(); |
|
hsBounds3::Read(s); |
|
if( !(fExtFlags & kAxisAligned) ) |
|
{ |
|
fCorner.Read(s); |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
fAxes[i].Read(s); |
|
fDists[i].fX = s->ReadLEScalar(); |
|
fDists[i].fY = s->ReadLEScalar(); |
|
} |
|
IMakeMinsMaxs(); |
|
IMakeDists(); |
|
} |
|
IMakeSphere(); |
|
} |
|
void hsBounds3Ext::Write(hsStream *s) |
|
{ |
|
s->WriteLE32(fExtFlags); |
|
hsBounds3::Write(s); |
|
if( !(fExtFlags & kAxisAligned) ) |
|
{ |
|
fCorner.Write(s); |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
fAxes[i].Write(s); |
|
if( fExtFlags & kDistsSet ) |
|
{ |
|
s->WriteLEScalar(fDists[i].fX); |
|
s->WriteLEScalar(fDists[i].fY); |
|
} |
|
else |
|
{ |
|
// Playing nice with binary patches--writing uninited values BAD! |
|
s->WriteLEScalar( 0.f ); |
|
s->WriteLEScalar( 0.f ); |
|
} |
|
} |
|
} |
|
} |
|
|
|
#if 0 // Commenting out this which will be made redundant and/or obsolete by Havok integration |
|
//////////////////////////////////////////////////////////////////////////////////////////////////// |
|
//////////////////////////////////////////////////////////////////////////////////////////////////// |
|
//////////////////////////////////////////////////////////////////////////////////////////////////// |
|
|
|
void hsBounds3Tri::TestPlane(const hsVector3 &n, hsPoint2 &depth) const |
|
{ |
|
depth.fX = depth.fY = n.InnerProduct(fVerts[0]); |
|
|
|
float d1, d2; |
|
|
|
d1 = n.InnerProduct(fVerts[1]); |
|
d2 = n.InnerProduct(fVerts[2]); |
|
|
|
if( d1 > d2 ) |
|
{ |
|
if( d1 > depth.fY ) |
|
depth.fY = d1; |
|
if( d2 < depth.fX ) |
|
depth.fX = d2; |
|
} |
|
else |
|
{ |
|
if( d2 > depth.fY ) |
|
depth.fY = d2; |
|
if( d1 < depth.fX ) |
|
depth.fX = d1; |
|
} |
|
} |
|
bool hsBounds3Tri::ClosestTriPoint(const hsPoint3 *p, hsPoint3 *out, const hsVector3 *ax) const |
|
{ |
|
// project point onto tri plane |
|
hsPoint3 pPln; |
|
if( ax ) |
|
{ |
|
float t; |
|
|
|
t = fNormal.InnerProduct(fVerts[0] - *p); |
|
float s = fNormal.InnerProduct(ax); |
|
if( (s > hsBounds::kRealSmall)||(s < -hsBounds::kRealSmall) ) |
|
{ |
|
t /= s; |
|
|
|
pPln = *p; |
|
pPln += *ax * t; |
|
} |
|
else |
|
{ |
|
return ClosestTriPoint(p, out); |
|
} |
|
} |
|
else |
|
{ |
|
float t; |
|
|
|
t = fNormal.InnerProduct(fVerts[0] - *p); |
|
t /= fNormal.MagnitudeSquared(); |
|
|
|
pPln = *p; |
|
pPln += fNormal * t; |
|
} |
|
|
|
if( !(fTriFlags & kAxesSet) ) |
|
SetAxes(); |
|
|
|
int nIn = 0; |
|
int firstIn, secondIn; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
float tst = fPerpAxes[i].InnerProduct(pPln); |
|
bool in = false; |
|
if( fOnIsMax & (1 << i) ) |
|
{ |
|
if( tst <= fPerpDists[i].fY ) |
|
in = true; |
|
} |
|
else |
|
{ |
|
if( tst >= fPerpDists[i].fX ) |
|
in = true; |
|
} |
|
if( in ) |
|
{ |
|
if( nIn++ ) |
|
secondIn = i; |
|
else |
|
firstIn = i; |
|
} |
|
} |
|
switch( nIn ) |
|
{ |
|
case 3: |
|
*out = pPln; |
|
break; |
|
case 1: |
|
{ |
|
int k, kPlus; |
|
k = firstIn == 2 ? 0 : firstIn+1; |
|
kPlus = k == 2 ? 0 : k+1; |
|
|
|
hsPoint3 pTmp; |
|
float z; |
|
z = hsBounds3::ClosestPointToLine(&pPln, fVerts+k, fVerts+kPlus, &pTmp); |
|
if( z <= 1.f ) |
|
*out = pTmp; |
|
else |
|
{ |
|
k = kPlus; |
|
kPlus = k == 2 ? 0 : k+1; |
|
z = hsBounds3::ClosestPointToLine(&pPln, fVerts+k, fVerts+kPlus, out); |
|
} |
|
} |
|
break; |
|
case 2: |
|
{ |
|
int k, kPlus; |
|
k = secondIn == 2 ? 0 : secondIn+1; |
|
if( k == firstIn ) |
|
k++; |
|
kPlus = k == 2 ? 0 : k+1; |
|
|
|
hsBounds3::ClosestPointToLine(&pPln, fVerts+k, fVerts+kPlus, out); |
|
break; |
|
} |
|
|
|
case 0: |
|
hsAssert(false, "Extreme bogosity, inverted tri?!?"); |
|
*out = pPln; |
|
return false; |
|
} |
|
|
|
#ifdef HS_DEBUGGING // mf horse testing |
|
#if 0 |
|
if( 0 ) |
|
{ |
|
hsVector3 ndeb = hsVector3(fVerts+1, fVerts) % hsVector3(fVerts+2, fVerts); |
|
float dis; |
|
dis = fNormal.InnerProduct(pPln) - fDist; |
|
if( (fDist > hsBounds::kRealSmall)||(fDist < -hsBounds::kRealSmall) ) |
|
dis /= fDist; |
|
hsAssert((dis < hsBounds::kRealSmall)&&(dis > -hsBounds::kRealSmall), "Non-planar pPln"); |
|
dis = hsVector3(&pPln, out).MagnitudeSquared(); |
|
float vDis; |
|
vDis = hsVector3(&pPln, fVerts+0).MagnitudeSquared(); |
|
hsAssert( vDis - dis > -hsBounds::kRealSmall, "Bad closest point"); |
|
vDis = hsVector3(&pPln, fVerts+1).MagnitudeSquared(); |
|
hsAssert( vDis - dis > -hsBounds::kRealSmall, "Bad closest point"); |
|
vDis = hsVector3(&pPln, fVerts+2).MagnitudeSquared(); |
|
hsAssert( vDis - dis > -hsBounds::kRealSmall, "Bad closest point"); |
|
bool dork = false; |
|
if( dork ) |
|
{ |
|
float zn[3]; |
|
float zf[3]; |
|
float z[3]; |
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
z[i] = fPerpAxes[i].InnerProduct(fVerts[i]); |
|
int j; |
|
j = i == 0 ? 2 : i-1; |
|
zf[i] = fPerpAxes[i].InnerProduct(fVerts[j]); |
|
j = i == 2 ? 0 : i+1; |
|
zn[i] = fPerpAxes[i].InnerProduct(fVerts[j]); |
|
} |
|
return ClosestTriPoint(p, out, ax); |
|
} |
|
} |
|
#endif |
|
#endif |
|
|
|
return 3 == nIn; |
|
} |
|
|
|
void hsBounds3Tri::SetAxes() const |
|
{ |
|
fOnIsMax = 0; |
|
|
|
hsVector3 edge[3]; |
|
edge[0].Set(fVerts, fVerts+1); |
|
edge[1].Set(fVerts+1, fVerts+2); |
|
edge[2].Set(fVerts+2, fVerts); |
|
hsVector3 perp = edge[2] % edge[0]; |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
int j = i == 2 ? 0 : i+1; |
|
int k = j == 2 ? 0 : j+1; |
|
fPerpAxes[i] = edge[i] % perp; |
|
fPerpAxes[i].Normalize(); |
|
|
|
fPerpDists[i].fX = fPerpAxes[i].InnerProduct(fVerts[i]); |
|
fPerpDists[i].fY = fPerpAxes[i].InnerProduct(fVerts[k]); |
|
if( fPerpDists[i].fX > fPerpDists[i].fY ) |
|
{ |
|
fOnIsMax |= 1 << i; |
|
float d = fPerpDists[i].fX; |
|
fPerpDists[i].fX = fPerpDists[i].fY; |
|
fPerpDists[i].fY = d; |
|
} |
|
} |
|
fTriFlags |= kAxesSet; |
|
} |
|
|
|
hsBounds3Tri* hsBounds3Tri::Transform(const hsMatrix44& x) |
|
{ |
|
#if 0 // IDENT |
|
if( x.fFlags & hsMatrix44::kIsIdent ) |
|
return this; |
|
#endif // IDENT |
|
|
|
fVerts[0] = x * fVerts[0]; |
|
fVerts[1] = x * fVerts[1]; |
|
fVerts[2] = x * fVerts[2]; |
|
|
|
hsVector3 v1, v2; |
|
v1.Set(&fVerts[1], &fVerts[0]); |
|
v2.Set(&fVerts[2], &fVerts[0]); |
|
fNormal = v1 % v2; |
|
// mf horse - do we need to normalize here? |
|
// fNormal.Normalize(); |
|
|
|
fDist = fNormal.InnerProduct(fVerts[0]); |
|
|
|
fTriFlags &= ~kAxesSet; |
|
|
|
SetAxes(); |
|
|
|
return this; |
|
} |
|
|
|
hsBounds3Tri* hsBounds3Tri::Translate(const hsVector3& v) |
|
{ |
|
fVerts[0] += v; |
|
fVerts[1] += v; |
|
fVerts[2] += v; |
|
|
|
fDist = fNormal.InnerProduct(fVerts[0]); |
|
|
|
int i; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
int j = i == 2 ? 0 : i+1; |
|
int k = j == 2 ? 0 : j+1; |
|
|
|
float del = fPerpAxes[i].InnerProduct(v); |
|
fPerpDists[i].fX += del; |
|
fPerpDists[i].fY += del; |
|
} |
|
|
|
return this; |
|
} |
|
|
|
void hsBounds3Tri::Set(const hsPoint3& v0, |
|
const hsPoint3& v1, |
|
const hsPoint3& v2, |
|
hsTriangle3* t, |
|
const hsMatrix44& x) |
|
{ |
|
fVerts[0] = v0; |
|
fVerts[1] = v1; |
|
fVerts[2] = v2; |
|
|
|
fOnIsMax = 0; |
|
fTriangle = t; |
|
|
|
if( t->fFlags & hsTriangle3::kTwoSided ) |
|
fTriFlags |= kDoubleSide; |
|
|
|
#if 0 // IDENT |
|
if( x.fFlags & hsMatrix44::kIsIdent ) |
|
{ |
|
hsVector3 v1, v2; |
|
v1.Set(&fVerts[1], &fVerts[0]); |
|
v2.Set(&fVerts[2], &fVerts[0]); |
|
fNormal = v1 % v2; |
|
// mf horse - do we need to normalize here? |
|
// fNormal.Normalize(); |
|
|
|
fDist = fNormal.InnerProduct(fVerts[0]); |
|
|
|
fTriFlags &= ~kAxesSet; |
|
|
|
SetAxes(); |
|
} |
|
else |
|
#endif // IDENT |
|
Transform(x); |
|
} |
|
|
|
hsBounds3Tri::hsBounds3Tri(const hsPoint3& v0, |
|
const hsPoint3& v1, |
|
const hsPoint3& v2, |
|
hsTriangle3* t, |
|
const hsMatrix44& x) |
|
{ |
|
Set(v0, v1, v2, t, x); |
|
} |
|
|
|
hsBounds3Tri::hsBounds3Tri(hsTriangle3* t, const hsMatrix44& x) |
|
{ |
|
Set(t->fVert[0]->fVtx->fLocalPos, |
|
t->fVert[2]->fVtx->fLocalPos, |
|
t->fVert[2]->fVtx->fLocalPos, |
|
t, x); |
|
} |
|
|
|
void hsBounds3Tri::Set(hsPoint3 *v0, hsPoint3 *v1, hsPoint3 *v2, hsVector3 *n, uint32_t triFlags, hsTriangle3 *t) |
|
{ |
|
fTriFlags = 0; |
|
|
|
if( triFlags & hsTriangle3::kTwoSided ) |
|
fTriFlags |= kDoubleSide; |
|
|
|
fNormal = *n; |
|
fVerts[0] = *v0; |
|
fVerts[1] = *v1; |
|
fVerts[2] = *v2; |
|
|
|
fOnIsMax = 0; |
|
fTriangle = t; |
|
|
|
fDist = fNormal.InnerProduct(fVerts[0]); |
|
} |
|
|
|
hsBounds3Tri::hsBounds3Tri(hsPoint3 *v0, hsPoint3 *v1, hsPoint3 *v2, hsVector3 *n, uint32_t triFlags, hsTriangle3 *t) |
|
{ |
|
Set(v0, v1, v2, n, triFlags, t); |
|
} |
|
|
|
hsBounds3Tri::hsBounds3Tri(hsTriangle3* t) |
|
{ |
|
Set(&t->fVert[0]->fVtx->fLocalPos, |
|
&t->fVert[1]->fVtx->fLocalPos, |
|
&t->fVert[2]->fVtx->fLocalPos, |
|
&t->fNormal, t->fFlags, t); |
|
} |
|
|
|
hsBounds3Tri::~hsBounds3Tri() |
|
{ |
|
} |
|
|
|
|
|
// Finds closest intersection vertex or triangle/center-line intersection |
|
bool hsBounds3Tri::ISectCone(const hsPoint3& from, const hsPoint3& to, float cosThetaSq, bool ignoreFacing, hsPoint3& at, bool& backSide) const |
|
{ |
|
float d0 = from.InnerProduct(fNormal); |
|
float d1 = at.InnerProduct(fNormal); |
|
float dt = fNormal.InnerProduct(fVerts[0]); |
|
backSide = d0 < dt; |
|
if( !ignoreFacing && backSide ) |
|
return false; |
|
if ( (d0 < dt || d1 < dt) && |
|
(d0 > dt || d1 > dt) && |
|
ClosestTriPoint(&from, &at, &hsVector3(&to,&from)) ) |
|
return true; |
|
|
|
hsVector3 av(&to,&from); |
|
float distASq = av.MagnitudeSquared(); |
|
float radiusSq = distASq * (1-cosThetaSq)/cosThetaSq; |
|
|
|
float minDistSq = 0; |
|
int32_t minVert = 0; |
|
bool sect = false; |
|
for (int32_t i=0; i<3; i++) |
|
{ |
|
hsPoint3 onLine; |
|
float t = hsBounds3::ClosestPointToLine(&fVerts[i], &from, &to, &onLine); |
|
|
|
// outside the cap of the cylinder |
|
if (t<0 || t>1) |
|
continue; |
|
|
|
// outside the edge of the cylinder |
|
if (hsVector3(&onLine, &fVerts[i]).MagnitudeSquared() >= radiusSq) |
|
continue; |
|
|
|
hsVector3 bv(&fVerts[i],&from); |
|
|
|
float distBSq = bv.MagnitudeSquared(); |
|
|
|
float cosMuSquared = (av * bv) / (distASq * distBSq); |
|
|
|
// outside the angle of the cone |
|
if (cosMuSquared > cosThetaSq) |
|
continue; |
|
|
|
if (!sect || distBSq < minDistSq) |
|
{ |
|
minVert = i; |
|
minDistSq = distBSq; |
|
sect = true; |
|
} |
|
} |
|
at = fVerts[minVert]; |
|
return sect; |
|
} |
|
#endif // Commenting out this which will be made redundant and/or obsolete by Havok integration
|
|
|