mirror of
https://github.com/tribes2/engine.git
synced 2026-01-20 03:34:48 +00:00
937 lines
25 KiB
C++
937 lines
25 KiB
C++
//-----------------------------------------------------------------------------
|
|
// V12 Engine
|
|
//
|
|
// Copyright (c) 2001 GarageGames.Com
|
|
// Portions Copyright (c) 2001 by Sierra Online, Inc.
|
|
//-----------------------------------------------------------------------------
|
|
|
|
#include "dgl/dgl.h"
|
|
#include "Core/dataChunker.h"
|
|
#include "Collision/collision.h"
|
|
#include "sceneGraph/sceneGraph.h"
|
|
#include "Sim/sceneObject.h"
|
|
#include "terrain/terrData.h"
|
|
#include "Collision/convex.h"
|
|
|
|
//----------------------------------------------------------------------------
|
|
//----------------------------------------------------------------------------
|
|
|
|
static DataChunker sChunker;
|
|
|
|
CollisionStateList CollisionStateList::sFreeList;
|
|
CollisionWorkingList CollisionWorkingList::sFreeList;
|
|
F32 sqrDistanceEdges(const Point3F& start0,
|
|
const Point3F& end0,
|
|
const Point3F& start1,
|
|
const Point3F& end1,
|
|
Point3F* is,
|
|
Point3F* it);
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Feature Collision
|
|
//----------------------------------------------------------------------------
|
|
|
|
bool ConvexFeature::collide(ConvexFeature& cf,CollisionList* cList, F32 tol)
|
|
{
|
|
// Our vertices vs. CF faces
|
|
const Point3F* vert = mVertexList.begin();
|
|
const Point3F* vend = mVertexList.end();
|
|
while (vert != vend)
|
|
{
|
|
cf.testVertex(*vert,cList,false, tol);
|
|
vert++;
|
|
}
|
|
|
|
// CF vertices vs. our faces
|
|
vert = cf.mVertexList.begin();
|
|
vend = cf.mVertexList.end();
|
|
while (vert != vend)
|
|
{
|
|
U32 storeCount = cList->count;
|
|
testVertex(*vert,cList,true, tol);
|
|
|
|
// Fix up last reference. material and object are copied from this rather
|
|
// than the object we're colliding against.
|
|
if (storeCount != cList->count)
|
|
{
|
|
cList->collision[cList->count - 1].material = cf.material;
|
|
cList->collision[cList->count - 1].object = cf.object;
|
|
}
|
|
vert++;
|
|
}
|
|
|
|
// Edge vs. Edge
|
|
const Edge* edge = mEdgeList.begin();
|
|
const Edge* eend = mEdgeList.end();
|
|
while (edge != eend)
|
|
{
|
|
cf.testEdge(mVertexList[edge->vertex[0]],
|
|
mVertexList[edge->vertex[1]],cList, tol);
|
|
edge++;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
inline bool isInside(const Point3F& p, const Point3F& a, const Point3F& b, const VectorF& n)
|
|
{
|
|
VectorF v;
|
|
mCross(n,b - a,&v);
|
|
F32 result = mDot(v,p - a);
|
|
return result <= 0.0f;
|
|
}
|
|
|
|
void ConvexFeature::testVertex(const Point3F& v,CollisionList* cList,bool flip, F32 tol)
|
|
{
|
|
// Test vertex against all faces
|
|
const Face* face = mFaceList.begin();
|
|
const Face* end = mFaceList.end();
|
|
for (; face != end; face++) {
|
|
if (cList->count >= CollisionList::MaxCollisions)
|
|
return;
|
|
|
|
const Point3F& p0 = mVertexList[face->vertex[0]];
|
|
const Point3F& p1 = mVertexList[face->vertex[1]];
|
|
const Point3F& p2 = mVertexList[face->vertex[2]];
|
|
F32 distance = mFabs(mDot(face->normal,v - p0));
|
|
|
|
if (distance > tol)
|
|
continue;
|
|
|
|
if (isInside(v,p0,p1,face->normal) &&
|
|
isInside(v,p1,p2,face->normal) &&
|
|
isInside(v,p2,p0,face->normal)) {
|
|
// Add collision to this face
|
|
Collision& info = cList->collision[cList->count++];
|
|
AssertFatal(cList->count <= CollisionList::MaxCollisions, "Error, exceeded max collisions!");
|
|
info.point = v;
|
|
info.normal = face->normal;
|
|
if (flip)
|
|
info.normal.neg();
|
|
info.material = material;
|
|
info.object = object;
|
|
info.distance = distance;
|
|
}
|
|
}
|
|
}
|
|
|
|
void ConvexFeature::testEdge(const Point3F& s1, const Point3F& e1, CollisionList* cList, F32 tol)
|
|
{
|
|
|
|
F32 tolSquared = tol*tol;
|
|
|
|
// Test edges against edges
|
|
VectorF v1 = e1 - s1;
|
|
const Edge* edge = mEdgeList.begin();
|
|
const Edge* end = mEdgeList.end();
|
|
for (; edge != end; edge++) {
|
|
if (cList->count >= CollisionList::MaxCollisions)
|
|
return;
|
|
|
|
const Point3F& s2 = mVertexList[edge->vertex[0]];
|
|
const Point3F& e2 = mVertexList[edge->vertex[1]];
|
|
VectorF n,v2 = e2 - s2;
|
|
|
|
// Normal to both lines
|
|
mCross(v1,v2,&n);
|
|
if (n.lenSquared() < 1e-9) {
|
|
continue;
|
|
}
|
|
|
|
Point3F is;
|
|
Point3F it;
|
|
F32 distance = sqrDistanceEdges(s1, e1,
|
|
s2, e2,
|
|
&is, &it);
|
|
|
|
if (distance > tolSquared)
|
|
continue;
|
|
distance = mSqrt(distance);
|
|
|
|
// Return a collision
|
|
Collision& info = cList->collision[cList->count++];
|
|
AssertFatal(cList->count <= CollisionList::MaxCollisions, "Error, exceeded max collisions!");
|
|
info.point = is;
|
|
info.normal = is - it;
|
|
if (info.normal.isZero())
|
|
info.normal.set(0, 0, 1);
|
|
else
|
|
info.normal.normalize();
|
|
info.distance = distance;
|
|
info.material = material;
|
|
info.object = object;
|
|
}
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Collision State management
|
|
//----------------------------------------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
CollisionStateList::CollisionStateList()
|
|
{
|
|
mPrev = mNext = this;
|
|
mState = NULL;
|
|
}
|
|
|
|
void CollisionStateList::linkAfter(CollisionStateList* ptr)
|
|
{
|
|
mPrev = ptr;
|
|
mNext = ptr->mNext;
|
|
ptr->mNext = this;
|
|
mNext->mPrev = this;
|
|
}
|
|
|
|
void CollisionStateList::unlink()
|
|
{
|
|
mPrev->mNext = mNext;
|
|
mNext->mPrev = mPrev;
|
|
mPrev = mNext = this;
|
|
}
|
|
|
|
CollisionStateList* CollisionStateList::alloc()
|
|
{
|
|
if (!sFreeList.isEmpty()) {
|
|
CollisionStateList* nxt = sFreeList.mNext;
|
|
nxt->unlink();
|
|
nxt->mState = NULL;
|
|
return nxt;
|
|
}
|
|
return constructInPlace((CollisionStateList*)sChunker.alloc(sizeof(CollisionStateList)));
|
|
}
|
|
|
|
void CollisionStateList::free()
|
|
{
|
|
unlink();
|
|
linkAfter(&sFreeList);
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
CollisionWorkingList::CollisionWorkingList()
|
|
{
|
|
wLink.mPrev = wLink.mNext = this;
|
|
rLink.mPrev = rLink.mNext = this;
|
|
}
|
|
|
|
void CollisionWorkingList::wLinkAfter(CollisionWorkingList* ptr)
|
|
{
|
|
wLink.mPrev = ptr;
|
|
wLink.mNext = ptr->wLink.mNext;
|
|
ptr->wLink.mNext = this;
|
|
wLink.mNext->wLink.mPrev = this;
|
|
}
|
|
|
|
void CollisionWorkingList::rLinkAfter(CollisionWorkingList* ptr)
|
|
{
|
|
rLink.mPrev = ptr;
|
|
rLink.mNext = ptr->rLink.mNext;
|
|
ptr->rLink.mNext = this;
|
|
rLink.mNext->rLink.mPrev = this;
|
|
}
|
|
|
|
void CollisionWorkingList::unlink()
|
|
{
|
|
wLink.mPrev->wLink.mNext = wLink.mNext;
|
|
wLink.mNext->wLink.mPrev = wLink.mPrev;
|
|
wLink.mPrev = wLink.mNext = this;
|
|
|
|
rLink.mPrev->rLink.mNext = rLink.mNext;
|
|
rLink.mNext->rLink.mPrev = rLink.mPrev;
|
|
rLink.mPrev = rLink.mNext = this;
|
|
}
|
|
|
|
CollisionWorkingList* CollisionWorkingList::alloc()
|
|
{
|
|
if (sFreeList.wLink.mNext != &sFreeList) {
|
|
CollisionWorkingList* nxt = sFreeList.wLink.mNext;
|
|
nxt->unlink();
|
|
return nxt;
|
|
}
|
|
return constructInPlace((CollisionWorkingList*)sChunker.alloc(sizeof(CollisionWorkingList)));
|
|
}
|
|
|
|
void CollisionWorkingList::free()
|
|
{
|
|
unlink();
|
|
wLinkAfter(&sFreeList);
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Convex Base Class
|
|
//----------------------------------------------------------------------------
|
|
|
|
U32 Convex::sTag = -1;
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
Convex::Convex()
|
|
{
|
|
mNext = mPrev = this;
|
|
mTag = 0;
|
|
}
|
|
|
|
Convex::~Convex()
|
|
{
|
|
// Unlink from Convex Database
|
|
unlink();
|
|
|
|
// Delete collision states
|
|
while (mList.mNext != &mList)
|
|
delete mList.mNext->mState;
|
|
|
|
// Free up working list
|
|
while (mWorking.wLink.mNext != &mWorking)
|
|
mWorking.wLink.mNext->free();
|
|
|
|
// Free up references
|
|
while (mReference.rLink.mNext != &mReference)
|
|
mReference.rLink.mNext->free();
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
void Convex::collectGarbage()
|
|
{
|
|
// Delete unreferenced Convex Objects
|
|
for (Convex* itr = mNext; itr != this; itr = itr->mNext) {
|
|
if (itr->mReference.rLink.mNext == &itr->mReference) {
|
|
Convex* ptr = itr;
|
|
itr = itr->mPrev;
|
|
delete ptr;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Convex::nukeList()
|
|
{
|
|
// Delete all Convex Objects
|
|
for (Convex* itr = mNext; itr != this; itr = itr->mNext) {
|
|
Convex* ptr = itr;
|
|
itr = itr->mPrev;
|
|
delete ptr;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
void Convex::registerObject(Convex *convex)
|
|
{
|
|
convex->linkAfter(this);
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
void Convex::linkAfter(Convex* ptr)
|
|
{
|
|
mPrev = ptr;
|
|
mNext = ptr->mNext;
|
|
ptr->mNext = this;
|
|
mNext->mPrev = this;
|
|
}
|
|
|
|
void Convex::unlink()
|
|
{
|
|
mPrev->mNext = mNext;
|
|
mNext->mPrev = mPrev;
|
|
mPrev = mNext = this;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
Point3F Convex::support(const VectorF&) const
|
|
{
|
|
return Point3F(0,0,0);
|
|
}
|
|
|
|
void Convex::getFeatures(const MatrixF&,const VectorF&,ConvexFeature* f)
|
|
{
|
|
f->object = NULL;
|
|
}
|
|
|
|
const MatrixF& Convex::getTransform() const
|
|
{
|
|
return mObject->getTransform();
|
|
}
|
|
|
|
const Point3F& Convex::getScale() const
|
|
{
|
|
return mObject->getScale();
|
|
}
|
|
|
|
Box3F Convex::getBoundingBox() const
|
|
{
|
|
return mObject->getWorldBox();
|
|
}
|
|
|
|
Box3F Convex::getBoundingBox(const MatrixF& mat, const Point3F& scale) const
|
|
{
|
|
Box3F wBox = mObject->getObjBox();
|
|
wBox.min.convolve(scale);
|
|
wBox.max.convolve(scale);
|
|
mat.mul(wBox);
|
|
return wBox;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
void Convex::addToWorkingList(Convex* ptr)
|
|
{
|
|
CollisionWorkingList* cl = CollisionWorkingList::alloc();
|
|
cl->wLinkAfter(&mWorking);
|
|
cl->rLinkAfter(&ptr->mReference);
|
|
cl->mConvex = ptr;
|
|
};
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
void Convex::updateWorkingList(const Box3F& box, const U32 colMask)
|
|
{
|
|
sTag++;
|
|
|
|
// Clear objects off the working list that are no longer intersecting
|
|
for (CollisionWorkingList* itr = mWorking.wLink.mNext; itr != &mWorking; itr = itr->wLink.mNext) {
|
|
itr->mConvex->mTag = sTag;
|
|
if (!box.isOverlapped(itr->mConvex->getBoundingBox())) {
|
|
CollisionWorkingList* cl = itr;
|
|
itr = itr->wLink.mPrev;
|
|
cl->free();
|
|
}
|
|
}
|
|
|
|
// Special processing for the terrain and interiors...
|
|
AssertFatal(mObject->getContainer(), "Must be in a container!");
|
|
|
|
SimpleQueryList sql;
|
|
mObject->getContainer()->findObjects(box, colMask,
|
|
SimpleQueryList::insertionCallback, U32(&sql));
|
|
for (U32 i = 0; i < sql.mList.size(); i++)
|
|
sql.mList[i]->buildConvex(box, this);
|
|
}
|
|
|
|
// ---------------------------------------------------------------------------
|
|
|
|
void Convex::updateStateList(const MatrixF& mat, const Point3F& scale, const Point3F* displacement)
|
|
{
|
|
Box3F box1 = getBoundingBox(mat, scale);
|
|
box1.min -= Point3F(1, 1, 1);
|
|
box1.max += Point3F(1, 1, 1);
|
|
if (displacement) {
|
|
Point3F oldMin = box1.min;
|
|
Point3F oldMax = box1.max;
|
|
|
|
box1.min.setMin(oldMin + *displacement);
|
|
box1.min.setMin(oldMax + *displacement);
|
|
box1.max.setMax(oldMin + *displacement);
|
|
box1.max.setMax(oldMax + *displacement);
|
|
}
|
|
sTag++;
|
|
|
|
// Destroy states which are no longer intersecting
|
|
for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext) {
|
|
Convex* cv = (itr->mState->a == this)? itr->mState->b: itr->mState->a;
|
|
cv->mTag = sTag;
|
|
if (!box1.isOverlapped(cv->getBoundingBox())) {
|
|
CollisionState* cs = itr->mState;
|
|
itr = itr->mPrev;
|
|
delete cs;
|
|
}
|
|
}
|
|
|
|
// Add collision states for new overlapping objects
|
|
for (CollisionWorkingList* itr0 = mWorking.wLink.mNext; itr0 != &mWorking; itr0 = itr0->wLink.mNext) {
|
|
register Convex* cv = itr0->mConvex;
|
|
if (cv->mTag != sTag && box1.isOverlapped(cv->getBoundingBox())) {
|
|
CollisionState* state = new CollisionState;
|
|
state->set(this,cv,mat,cv->getTransform());
|
|
state->mLista->linkAfter(&mList);
|
|
state->mListb->linkAfter(&cv->mList);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
CollisionState* Convex::findClosestState(const MatrixF& mat, const Point3F& scale)
|
|
{
|
|
updateStateList(mat, scale);
|
|
F32 dist = +1E30;
|
|
CollisionState *st = 0;
|
|
|
|
MatrixF axform = mat;
|
|
axform.scale(scale);
|
|
|
|
for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext) {
|
|
CollisionState* state = itr->mState;
|
|
if (state->mLista != itr)
|
|
state->swap();
|
|
|
|
MatrixF bxform = state->b->getTransform();
|
|
bxform.scale(state->b->getScale());
|
|
F32 dd = state->distance(axform, bxform);
|
|
if (dd < dist) {
|
|
dist = dd;
|
|
st = state;
|
|
}
|
|
}
|
|
return st;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
CollisionState* Convex::findClosestStateBounded(const MatrixF& mat, const Point3F& scale, const F32 dontCareDist)
|
|
{
|
|
updateStateList(mat, scale);
|
|
F32 dist = +1E30;
|
|
CollisionState *st = 0;
|
|
|
|
MatrixF axform = mat;
|
|
axform.scale(scale);
|
|
MatrixF axforminv(true);
|
|
MatrixF temp(mat);
|
|
axforminv.scale(Point3F(1.0f/scale.x,
|
|
1.0f/scale.y,
|
|
1.0f/scale.z));
|
|
temp.affineInverse();
|
|
axforminv.mul(temp);
|
|
|
|
for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext) {
|
|
CollisionState* state = itr->mState;
|
|
if (state->mLista != itr)
|
|
state->swap();
|
|
|
|
MatrixF bxform = state->b->getTransform();
|
|
temp = bxform;
|
|
Point3F bscale = state->b->getScale();
|
|
bxform.scale(bscale);
|
|
MatrixF bxforminv(true);
|
|
bxforminv.scale(Point3F(1.0f/bscale.x,
|
|
1.0f/bscale.y,
|
|
1.0f/bscale.z));
|
|
temp.affineInverse();
|
|
bxforminv.mul(temp);
|
|
F32 dd = state->distanceBounded(axform, bxform, dontCareDist, &axforminv, &bxforminv);
|
|
if (dd < dist) {
|
|
dist = dd;
|
|
st = state;
|
|
}
|
|
}
|
|
if (dist < dontCareDist)
|
|
return st;
|
|
else
|
|
return NULL;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
bool Convex::getCollisionInfo(const MatrixF& mat, const Point3F& scale, CollisionList* cList,F32 tol)
|
|
{
|
|
for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext) {
|
|
CollisionState* state = itr->mState;
|
|
if (state->mLista != itr)
|
|
state->swap();
|
|
if (state->dist < tol) {
|
|
ConvexFeature fa,fb;
|
|
VectorF v;
|
|
|
|
MatrixF imat = mat;
|
|
imat.scale(scale);
|
|
imat.inverse();
|
|
imat.mulV(-state->v,&v);
|
|
getFeatures(mat,v,&fa);
|
|
|
|
imat = state->b->getTransform();
|
|
imat.scale(state->b->getScale());
|
|
MatrixF bxform = imat;
|
|
imat.inverse();
|
|
imat.mulV(state->v,&v);
|
|
state->b->getFeatures(bxform,v,&fb);
|
|
|
|
fa.collide(fb,cList,tol);
|
|
}
|
|
}
|
|
return (cList->count != 0);
|
|
}
|
|
|
|
void Convex::getPolyList(AbstractPolyList*)
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
// This function taken from:
|
|
// 3D Game Engine Design, David H. Eberly
|
|
// Ref page 43. Code from included cd.
|
|
//
|
|
F32 sqrDistanceEdges(const Point3F& start0,
|
|
const Point3F& end0,
|
|
const Point3F& start1,
|
|
const Point3F& end1,
|
|
Point3F* is,
|
|
Point3F* it)
|
|
{
|
|
Point3F direction0 = end0 - start0;
|
|
Point3F direction1 = end1 - start1;
|
|
|
|
Point3F kDiff = start0 - start1;
|
|
F32 fA00 = direction0.lenSquared();
|
|
F32 fA01 = -mDot(direction0, direction1);
|
|
F32 fA11 = direction1.lenSquared();
|
|
F32 fB0 = mDot(kDiff, direction0);
|
|
F32 fC = kDiff.lenSquared();
|
|
F32 fDet = mAbs(fA00*fA11 - fA01*fA01);
|
|
F32 fB1, fS, fT, fSqrDist, fTmp;
|
|
|
|
if ( fDet >= 0.00001 )
|
|
{
|
|
// line segments are not parallel
|
|
fB1 = -mDot(kDiff, direction1);
|
|
fS = fA01*fB1-fA11*fB0;
|
|
fT = fA01*fB0-fA00*fB1;
|
|
|
|
if ( fS >= 0.0 )
|
|
{
|
|
if ( fS <= fDet )
|
|
{
|
|
if ( fT >= 0.0 )
|
|
{
|
|
if ( fT <= fDet ) // region 0 (interior)
|
|
{
|
|
// minimum at two interior points of 3D lines
|
|
F32 fInvDet = 1.0/fDet;
|
|
fS *= fInvDet;
|
|
fT *= fInvDet;
|
|
fSqrDist = (fS*(fA00*fS + fA01*fT + 2.0*fB0) +
|
|
fT*(fA01*fS + fA11*fT + 2.0*fB1) + fC);
|
|
}
|
|
else // region 3 (side)
|
|
{
|
|
fT = 1.0;
|
|
fTmp = fA01 + fB0;
|
|
if ( fTmp >= 0.0 )
|
|
{
|
|
fS = 0.0;
|
|
fSqrDist = fA11 + 2.0*fB1 + fC;
|
|
}
|
|
else if ( -fTmp >= fA00 )
|
|
{
|
|
fS = 1.0;
|
|
fSqrDist = fA00 + fA11 + fC + 2.0*(fB1 + fTmp);
|
|
}
|
|
else
|
|
{
|
|
fS = -fTmp/fA00;
|
|
fSqrDist = fTmp*fS+fA11+2.0*fB1+fC;
|
|
}
|
|
}
|
|
}
|
|
else // region 7 (side)
|
|
{
|
|
fT = 0.0;
|
|
if ( fB0 >= 0.0 )
|
|
{
|
|
fS = 0.0;
|
|
fSqrDist = fC;
|
|
}
|
|
else if ( -fB0 >= fA00 )
|
|
{
|
|
fS = 1.0;
|
|
fSqrDist = fA00+2.0*fB0+fC;
|
|
}
|
|
else
|
|
{
|
|
fS = -fB0/fA00;
|
|
fSqrDist = fB0*fS+fC;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ( fT >= 0.0 )
|
|
{
|
|
if ( fT <= fDet ) // region 1 (side)
|
|
{
|
|
fS = 1.0;
|
|
fTmp = fA01+fB1;
|
|
if ( fTmp >= 0.0 )
|
|
{
|
|
fT = 0.0;
|
|
fSqrDist = fA00+2.0*fB0+fC;
|
|
}
|
|
else if ( -fTmp >= fA11 )
|
|
{
|
|
fT = 1.0;
|
|
fSqrDist = fA00+fA11+fC+2.0*(fB0+fTmp);
|
|
}
|
|
else
|
|
{
|
|
fT = -fTmp/fA11;
|
|
fSqrDist = fTmp*fT+fA00+2.0*fB0+fC;
|
|
}
|
|
}
|
|
else // region 2 (corner)
|
|
{
|
|
fTmp = fA01+fB0;
|
|
if ( -fTmp <= fA00 )
|
|
{
|
|
fT = 1.0;
|
|
if ( fTmp >= 0.0 )
|
|
{
|
|
fS = 0.0;
|
|
fSqrDist = fA11+2.0*fB1+fC;
|
|
}
|
|
else
|
|
{
|
|
fS = -fTmp/fA00;
|
|
fSqrDist = fTmp*fS+fA11+2.0*fB1+fC;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fS = 1.0;
|
|
fTmp = fA01+fB1;
|
|
if ( fTmp >= 0.0 )
|
|
{
|
|
fT = 0.0;
|
|
fSqrDist = fA00+2.0*fB0+fC;
|
|
}
|
|
else if ( -fTmp >= fA11 )
|
|
{
|
|
fT = 1.0;
|
|
fSqrDist = fA00+fA11+fC+2.0*(fB0+fTmp);
|
|
}
|
|
else
|
|
{
|
|
fT = -fTmp/fA11;
|
|
fSqrDist = fTmp*fT+fA00+2.0*fB0+fC;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else // region 8 (corner)
|
|
{
|
|
if ( -fB0 < fA00 )
|
|
{
|
|
fT = 0.0;
|
|
if ( fB0 >= 0.0 )
|
|
{
|
|
fS = 0.0;
|
|
fSqrDist = fC;
|
|
}
|
|
else
|
|
{
|
|
fS = -fB0/fA00;
|
|
fSqrDist = fB0*fS+fC;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fS = 1.0;
|
|
fTmp = fA01+fB1;
|
|
if ( fTmp >= 0.0 )
|
|
{
|
|
fT = 0.0;
|
|
fSqrDist = fA00+2.0*fB0+fC;
|
|
}
|
|
else if ( -fTmp >= fA11 )
|
|
{
|
|
fT = 1.0;
|
|
fSqrDist = fA00+fA11+fC+2.0*(fB0+fTmp);
|
|
}
|
|
else
|
|
{
|
|
fT = -fTmp/fA11;
|
|
fSqrDist = fTmp*fT+fA00+2.0*fB0+fC;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ( fT >= 0.0 )
|
|
{
|
|
if ( fT <= fDet ) // region 5 (side)
|
|
{
|
|
fS = 0.0;
|
|
if ( fB1 >= 0.0 )
|
|
{
|
|
fT = 0.0;
|
|
fSqrDist = fC;
|
|
}
|
|
else if ( -fB1 >= fA11 )
|
|
{
|
|
fT = 1.0;
|
|
fSqrDist = fA11+2.0*fB1+fC;
|
|
}
|
|
else
|
|
{
|
|
fT = -fB1/fA11;
|
|
fSqrDist = fB1*fT+fC;
|
|
}
|
|
}
|
|
else // region 4 (corner)
|
|
{
|
|
fTmp = fA01+fB0;
|
|
if ( fTmp < 0.0 )
|
|
{
|
|
fT = 1.0;
|
|
if ( -fTmp >= fA00 )
|
|
{
|
|
fS = 1.0;
|
|
fSqrDist = fA00+fA11+fC+2.0*(fB1+fTmp);
|
|
}
|
|
else
|
|
{
|
|
fS = -fTmp/fA00;
|
|
fSqrDist = fTmp*fS+fA11+2.0*fB1+fC;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fS = 0.0;
|
|
if ( fB1 >= 0.0 )
|
|
{
|
|
fT = 0.0;
|
|
fSqrDist = fC;
|
|
}
|
|
else if ( -fB1 >= fA11 )
|
|
{
|
|
fT = 1.0;
|
|
fSqrDist = fA11+2.0*fB1+fC;
|
|
}
|
|
else
|
|
{
|
|
fT = -fB1/fA11;
|
|
fSqrDist = fB1*fT+fC;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else // region 6 (corner)
|
|
{
|
|
if ( fB0 < 0.0 )
|
|
{
|
|
fT = 0.0;
|
|
if ( -fB0 >= fA00 )
|
|
{
|
|
fS = 1.0;
|
|
fSqrDist = fA00+2.0*fB0+fC;
|
|
}
|
|
else
|
|
{
|
|
fS = -fB0/fA00;
|
|
fSqrDist = fB0*fS+fC;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fS = 0.0;
|
|
if ( fB1 >= 0.0 )
|
|
{
|
|
fT = 0.0;
|
|
fSqrDist = fC;
|
|
}
|
|
else if ( -fB1 >= fA11 )
|
|
{
|
|
fT = 1.0;
|
|
fSqrDist = fA11+2.0*fB1+fC;
|
|
}
|
|
else
|
|
{
|
|
fT = -fB1/fA11;
|
|
fSqrDist = fB1*fT+fC;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// line segments are parallel
|
|
if ( fA01 > 0.0 )
|
|
{
|
|
// direction vectors form an obtuse angle
|
|
if ( fB0 >= 0.0 )
|
|
{
|
|
fS = 0.0;
|
|
fT = 0.0;
|
|
fSqrDist = fC;
|
|
}
|
|
else if ( -fB0 <= fA00 )
|
|
{
|
|
fS = -fB0/fA00;
|
|
fT = 0.0;
|
|
fSqrDist = fB0*fS+fC;
|
|
}
|
|
else
|
|
{
|
|
fB1 = -mDot(kDiff, direction1);
|
|
fS = 1.0;
|
|
fTmp = fA00+fB0;
|
|
if ( -fTmp >= fA01 )
|
|
{
|
|
fT = 1.0;
|
|
fSqrDist = fA00+fA11+fC+2.0*(fA01+fB0+fB1);
|
|
}
|
|
else
|
|
{
|
|
fT = -fTmp/fA01;
|
|
fSqrDist = fA00+2.0*fB0+fC+fT*(fA11*fT+2.0*(fA01+fB1));
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// direction vectors form an acute angle
|
|
if ( -fB0 >= fA00 )
|
|
{
|
|
fS = 1.0;
|
|
fT = 0.0;
|
|
fSqrDist = fA00+2.0*fB0+fC;
|
|
}
|
|
else if ( fB0 <= 0.0 )
|
|
{
|
|
fS = -fB0/fA00;
|
|
fT = 0.0;
|
|
fSqrDist = fB0*fS+fC;
|
|
}
|
|
else
|
|
{
|
|
fB1 = -mDot(kDiff, direction1);
|
|
fS = 0.0;
|
|
if ( fB0 >= -fA01 )
|
|
{
|
|
fT = 1.0;
|
|
fSqrDist = fA11+2.0*fB1+fC;
|
|
}
|
|
else
|
|
{
|
|
fT = -fB0/fA01;
|
|
fSqrDist = fC+fT*(2.0*fB1+fA11*fT);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
*is = start0 + direction0 * fS;
|
|
*it = start1 + direction1 * fT;
|
|
|
|
return mFabs(fSqrDist);
|
|
}
|