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.
 
 
 
 
 

3033 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"
#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;
}
hsBool 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;
}
hsBool 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
//
hsBool 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 = (hsBool)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;
}
}
}
hsBool 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);
hsBool someLow = false;
hsBool someHi = false;
hsBool 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
{
hsBool someIn = false;
hsBool 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;
}
hsBool 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;
}
hsBool 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 hsBool 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 hsBool 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);
}
hsBool 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;
}
hsBool 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;
}
hsBool 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
hsBool 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;
}
hsBool hsBounds3Ext::TriBSHitInfo(hsBounds3Tri& tri, const hsVector3& myVel, hsHitInfoExt* hit) const
{
hsPoint3 myPt = GetCenter();
myPt += myVel;
hsPoint3 closePt;
hsBool 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
hsBool 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;
hsBool 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
hsBool hsBounds3Ext::TriBBHitInfo(hsBounds3Tri& tri, const hsVector3& myVel, hsHitInfoExt* hit) const
{
hsPoint3 myPt = GetCenter();
myPt += myVel;
hsPoint3 closePt;
hsBool 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
hsBool 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;
}
hsBool 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;
}
hsBool 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;
}
hsBool 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
hsBool 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;
}
hsBool 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;
}
hsBool 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;
hsBool haveAxis = false;
hsVector3 tstAxis;
float tstDepth;
int i;
for( i = 0; i < 3; i++ )
{
hsBool 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;
}
hsBool 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;
}
hsBool 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;
}
hsBool 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;
}
hsBool 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;
}
}
hsBool 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);
hsBool 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");
hsBool 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
hsBool hsBounds3Tri::ISectCone(const hsPoint3& from, const hsPoint3& to, float cosThetaSq, hsBool ignoreFacing, hsPoint3& at, hsBool& 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;
hsBool 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