Initial commit

Torque/SDK/engine/collision/convex.cc

@@ -0,0 +1,641 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, 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"
+#include "collision/gjk.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);
+
+
+//----------------------------------------------------------------------------
+// Collision State
+//----------------------------------------------------------------------------
+
+CollisionState::CollisionState()
+{
+   mLista = mListb = 0;
+}
+
+CollisionState::~CollisionState()
+{
+   if (mLista)
+      mLista->free();
+   if (mListb)
+      mListb->free();
+}
+
+void CollisionState::swap()
+{
+}
+
+void CollisionState::set(Convex* a,Convex* b,const MatrixF& a2w, const MatrixF& b2w)
+{
+}
+
+
+F32 CollisionState::distance(const MatrixF& a2w, const MatrixF& b2w, const F32 dontCareDist,
+                       const MatrixF* w2a, const MatrixF* _w2b)
+{
+   return 0;
+}
+
+void CollisionState::render()
+{
+}
+
+
+
+//----------------------------------------------------------------------------
+// Feature Collision
+//----------------------------------------------------------------------------
+
+bool ConvexFeature::collide(ConvexFeature& cf,CollisionList* cList, F32 tol)
+{
+   // Our vertices vs. other faces
+   const Point3F* vert = mVertexList.begin();
+   const Point3F* vend = mVertexList.end();
+   while (vert != vend) {
+      cf.testVertex(*vert,cList,false, tol);
+      vert++;
+   }
+
+   // Other 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(this,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);
+   return mDot(v,p - a) < 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]];
+
+      // Point near the plane?
+      F32 distance = mDot(face->normal,v - p0);
+      if (distance > tol || distance < -tol)
+         continue;
+
+      // Make sure it's within the bounding edges
+      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++];
+         info.point = v;
+         info.normal = face->normal;
+         if (flip)
+            info.normal.neg();
+         info.material = material;
+         info.object = object;
+         info.distance = distance;
+      }
+   }
+}
+
+void ConvexFeature::testEdge(ConvexFeature* cf,const Point3F& s1, const Point3F& e1, CollisionList* cList, F32 tol)
+{
+   F32 tolSquared = tol*tol;
+
+   // Test edges against edges
+   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]];
+
+      // Get the distance and closest points
+      Point3F i1,i2;
+      F32 distance = sqrDistanceEdges(s1, e1, s2, e2, &i1, &i2);
+      if (distance > tolSquared)
+         continue;
+      distance = mSqrt(distance);
+
+      // Need to figure out how to orient the collision normal.
+      // The current test involves checking to see whether the collision
+      // points are contained within the convex volumes, which is slow.
+      if (inVolume(i1) || cf->inVolume(i2))
+         distance = -distance;
+
+      // Contact normal
+      VectorF normal = i1 - i2;
+      normal *= 1 / distance;
+
+      // Return a collision
+      Collision& info = cList->collision[cList->count++];
+      info.point    = i1;
+      info.normal   = normal;
+      info.distance = distance;
+      info.material = material;
+      info.object   = object;
+   }
+}
+
+bool ConvexFeature::inVolume(const Point3F& v)
+{
+   // Test the point to see if it's inside the volume
+   const Face* face = mFaceList.begin();
+   const Face* end  = mFaceList.end();
+   for (; face != end; face++) {
+      const Point3F& p0 = mVertexList[face->vertex[0]];
+      if (mDot(face->normal,v - p0) > 0)
+         return false;
+   }
+   return true;
+}
+
+
+//----------------------------------------------------------------------------
+// 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 = (U32)-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::render()
+{
+   for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext)
+      if (itr->mState->mLista == itr)
+         itr->mState->render();
+}
+
+
+//----------------------------------------------------------------------------
+
+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())) || (!itr->mConvex->getObject()->isCollisionEnabled())) {
+         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, &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 GjkCollisionState;
+         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, const F32 dontCareDist)
+{
+   updateStateList(mat, scale);
+   F32 dist = +1E30;
+   CollisionState *st = 0;
+
+   // Prepare scaled version of transform
+   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();
+
+      // Prepare scaled version of transform
+      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->distance(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;
+
+         // The idea is that we need to scale the matrix, so we need to
+         // make a copy of it, before we can pass it in to getFeatures.
+         // This is used to scale us for comparison against the other
+         // convex, which is correctly scaled.
+         MatrixF omat = mat;
+         omat.scale(scale);
+         MatrixF imat = omat;
+         imat.inverse();
+         imat.mulV(-state->v,&v);
+         getFeatures(omat,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 based on code orignally written for the book:
+// 3D Game Engine Design, by David H. Eberly
+//
+F32 sqrDistanceEdges(const Point3F& start0, const Point3F& end0,
+   const Point3F& start1, const Point3F& end1,
+   Point3F* is, Point3F* it)
+{
+   Point3F direction0 = end0 - start0;
+   F32 fA00 = direction0.lenSquared();
+
+   Point3F direction1 = end1 - start1;
+   F32 fA11 = direction1.lenSquared();
+   F32 fA01 = -mDot(direction0, direction1);
+
+   Point3F kDiff = start0 - start1;
+   F32 fC   = kDiff.lenSquared();
+   F32 fB0  = mDot(kDiff, direction0);
+   F32 fDet = mAbs((S32)(fA00*fA11 - fA01*fA01));
+
+   // Since the endpoints are tested as vertices, we're not interested
+   // in parallel lines, and intersections that don't involve end-points.
+   if (fDet >= 0.00001) {
+
+      // Calculate time of intersection for each line
+      F32 fB1 = -mDot(kDiff, direction1);
+      F32 fS  = fA01*fB1-fA11*fB0;
+      F32 fT  = fA01*fB0-fA00*fB1;
+
+      // Only interested in collisions that don't involve the end points
+      if (fS >= 0.0 && fS <= fDet && fT >= 0.0 && fT <= fDet) {
+         F32 fInvDet = 1.0 / fDet;
+         fS *= fInvDet;
+         fT *= fInvDet;
+         F32 fSqrDist = (fS*(fA00*fS + fA01*fT + 2.0*fB0) +
+            fT*(fA01*fS + fA11*fT + 2.0*fB1) + fC);
+
+         // Intersection points.
+         *is = start0 + direction0 * fS;
+         *it = start1 + direction1 * fT;
+         return mFabs(fSqrDist);
+      }
+   }
+
+   // Return a large number in the cases where endpoints are involved.
+   return 1e10f;
+}