convex.cpp

Public Functions

isInside(const Point3F & p, const Point3F & a, const Point3F & b, const VectorF & n)

sqrDistanceEdges(const Point3F & start0, const Point3F & end0, const Point3F & start1, const Point3F & end1, Point3F * is, Point3F * it)

  1
  2//-----------------------------------------------------------------------------
  3// Copyright (c) 2012 GarageGames, LLC
  4//
  5// Permission is hereby granted, free of charge, to any person obtaining a copy
  6// of this software and associated documentation files (the "Software"), to
  7// deal in the Software without restriction, including without limitation the
  8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
  9// sell copies of the Software, and to permit persons to whom the Software is
 10// furnished to do so, subject to the following conditions:
 11//
 12// The above copyright notice and this permission notice shall be included in
 13// all copies or substantial portions of the Software.
 14//
 15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 21// IN THE SOFTWARE.
 22//-----------------------------------------------------------------------------
 23
 24#include "platform/platform.h"
 25#include "collision/convex.h"
 26
 27#include "platform/types.h"
 28#include "core/dataChunker.h"
 29#include "collision/collision.h"
 30#include "scene/sceneObject.h"
 31#include "collision/gjk.h"
 32#include "collision/concretePolyList.h"
 33#include "platform/profiler.h"
 34
 35//----------------------------------------------------------------------------
 36//----------------------------------------------------------------------------
 37
 38static DataChunker sChunker;
 39
 40CollisionStateList CollisionStateList::sFreeList;
 41CollisionWorkingList CollisionWorkingList::sFreeList;
 42F32 sqrDistanceEdges(const Point3F& start0,
 43                     const Point3F& end0,
 44                     const Point3F& start1,
 45                     const Point3F& end1,
 46                     Point3F*       is,
 47                     Point3F*       it);
 48
 49
 50//----------------------------------------------------------------------------
 51// Collision State
 52//----------------------------------------------------------------------------
 53
 54CollisionState::CollisionState()
 55{
 56   mB = mA = NULL;
 57   mDist = 0.0f;
 58   mListb = mLista = 0;
 59}
 60
 61CollisionState::~CollisionState()
 62{
 63   if (mLista)
 64      mLista->free();
 65   if (mListb)
 66      mListb->free();
 67}
 68
 69void CollisionState::swap()
 70{
 71}
 72
 73void CollisionState::set(Convex* a,Convex* b,const MatrixF& a2w, const MatrixF& b2w)
 74{
 75}
 76
 77
 78F32 CollisionState::distance(const MatrixF& a2w, const MatrixF& b2w, const F32 dontCareDist,
 79                       const MatrixF* w2a, const MatrixF* _w2b)
 80{
 81   return 0;
 82}
 83
 84//----------------------------------------------------------------------------
 85// Feature Collision
 86//----------------------------------------------------------------------------
 87
 88void ConvexFeature::reset()
 89{
 90   material = NULL;
 91   mObject = NULL;
 92   mVertexList.clear();
 93   mEdgeList.clear();
 94   mFaceList.clear();
 95}
 96
 97bool ConvexFeature::collide(ConvexFeature& cf,CollisionList* cList, F32 tol)
 98{
 99   // Our vertices vs. other faces
100   const Point3F* vert = mVertexList.begin();
101   const Point3F* vend = mVertexList.end();
102   while (vert != vend) {
103      cf.testVertex(*vert,cList,false, tol);
104      vert++;
105   }
106
107   // Other vertices vs. our faces
108   vert = cf.mVertexList.begin();
109   vend = cf.mVertexList.end();
110   while (vert != vend) {
111      U32 storeCount = cList->getCount();
112      testVertex(*vert,cList,true, tol);
113
114      // Fix up last reference.  material and object are copied from this rather
115      //  than the object we're colliding against.
116      if (storeCount != cList->getCount()) 
117      {
118         Collision &col = (*cList)[cList->getCount() - 1];
119         col.material = cf.material;
120         col.object   = cf.mObject;
121      }
122      vert++;
123   }
124
125   // Edge vs. Edge
126   const Edge* edge = mEdgeList.begin();
127   const Edge* eend = mEdgeList.end();
128   while (edge != eend) {
129      cf.testEdge(this,mVertexList[edge->vertex[0]],
130         mVertexList[edge->vertex[1]],cList, tol);
131      edge++;
132   }
133
134   return true;
135}
136
137inline bool isInside(const Point3F& p, const Point3F& a, const Point3F& b, const VectorF& n)
138{
139   VectorF v;
140   mCross(n,b - a,&v);
141   return mDot(v,p - a) < 0.0f;
142}
143
144void ConvexFeature::testVertex(const Point3F& v,CollisionList* cList,bool flip, F32 tol)
145{
146   // Test vertex against all faces
147   const Face* face = mFaceList.begin();
148   const Face* end  = mFaceList.end();
149   for (; face != end; face++) {
150      if (cList->getCount() >= CollisionList::MaxCollisions)
151         return;
152
153      const Point3F& p0 = mVertexList[face->vertex[0]];
154      const Point3F& p1 = mVertexList[face->vertex[1]];
155      const Point3F& p2 = mVertexList[face->vertex[2]];
156
157      // Point near the plane?
158      F32 distance = mDot(face->normal,v - p0);
159      if (distance > tol || distance < -tol)
160         continue;
161
162      // Make sure it's within the bounding edges
163      if (isInside(v,p0,p1,face->normal) && isInside(v,p1,p2,face->normal) &&
164            isInside(v,p2,p0,face->normal)) {
165
166         // Add collision to this face
167         Collision& info = cList->increment();
168         info.point = v;
169         info.normal = face->normal;
170         if (flip)
171            info.normal.neg();
172         info.material = material;
173         info.object = mObject;
174         info.distance = distance;
175      }
176   }
177}
178
179void ConvexFeature::testEdge(ConvexFeature* cf,const Point3F& s1, const Point3F& e1, CollisionList* cList, F32 tol)
180{
181   F32 tolSquared = tol*tol;
182
183   // Test edges against edges
184   const Edge* edge = mEdgeList.begin();
185   const Edge* end  = mEdgeList.end();
186   for (; edge != end; edge++) {
187      if (cList->getCount() >= CollisionList::MaxCollisions)
188         return;
189
190      const Point3F& s2 = mVertexList[edge->vertex[0]];
191      const Point3F& e2 = mVertexList[edge->vertex[1]];
192
193      // Get the distance and closest points
194      Point3F i1,i2;
195      F32 distance = sqrDistanceEdges(s1, e1, s2, e2, &i1, &i2);
196      if (distance > tolSquared)
197         continue;
198      distance = mSqrt(distance);
199
200      // Need to figure out how to orient the collision normal.
201      // The current test involves checking to see whether the collision
202      // points are contained within the convex volumes, which is slow.
203      if (inVolume(i1) || cf->inVolume(i2))
204         distance = -distance;
205
206      // Contact normal
207      VectorF normal = i1 - i2;
208      if ( mIsZero( distance ) )
209         normal.zero();
210      else
211         normal *= 1 / distance;
212
213      // Return a collision
214      Collision& info = cList->increment();
215      info.point    = i1;
216      info.normal   = normal;
217      info.distance = distance;
218      info.material = material;
219      info.object   = mObject;
220   }
221}
222
223bool ConvexFeature::inVolume(const Point3F& v)
224{
225   // Test the point to see if it's inside the volume
226   const Face* face = mFaceList.begin();
227   const Face* end  = mFaceList.end();
228   for (; face != end; face++) {
229      const Point3F& p0 = mVertexList[face->vertex[0]];
230      if (mDot(face->normal,v - p0) > 0)
231         return false;
232   }
233   return true;
234}
235
236
237//----------------------------------------------------------------------------
238// Collision State management
239//----------------------------------------------------------------------------
240
241//----------------------------------------------------------------------------
242
243CollisionStateList::CollisionStateList()
244{
245   mPrev = mNext = this;
246   mState = NULL;
247}
248
249void CollisionStateList::linkAfter(CollisionStateList* ptr)
250{
251   mPrev = ptr;
252   mNext = ptr->mNext;
253   ptr->mNext = this;
254   mNext->mPrev = this;
255}
256
257void CollisionStateList::unlink()
258{
259   mPrev->mNext = mNext;
260   mNext->mPrev = mPrev;
261   mPrev = mNext = this;
262}
263
264CollisionStateList* CollisionStateList::alloc()
265{
266   if (!sFreeList.isEmpty()) {
267      CollisionStateList* nxt = sFreeList.mNext;
268      nxt->unlink();
269      nxt->mState = NULL;
270      return nxt;
271   }
272   return constructInPlace((CollisionStateList*)sChunker.alloc(sizeof(CollisionStateList)));
273}
274
275void CollisionStateList::free()
276{
277   unlink();
278   linkAfter(&sFreeList);
279}
280
281
282//----------------------------------------------------------------------------
283
284CollisionWorkingList::CollisionWorkingList()
285{
286   wLink.mPrev = wLink.mNext = this;
287   rLink.mPrev = rLink.mNext = this;
288   mConvex = NULL;
289}
290
291void CollisionWorkingList::wLinkAfter(CollisionWorkingList* ptr)
292{
293   wLink.mPrev = ptr;
294   wLink.mNext = ptr->wLink.mNext;
295   ptr->wLink.mNext = this;
296   wLink.mNext->wLink.mPrev = this;
297}
298
299void CollisionWorkingList::rLinkAfter(CollisionWorkingList* ptr)
300{
301   rLink.mPrev = ptr;
302   rLink.mNext = ptr->rLink.mNext;
303   ptr->rLink.mNext = this;
304   rLink.mNext->rLink.mPrev = this;
305}
306
307void CollisionWorkingList::unlink()
308{
309   wLink.mPrev->wLink.mNext = wLink.mNext;
310   wLink.mNext->wLink.mPrev = wLink.mPrev;
311   wLink.mPrev = wLink.mNext = this;
312
313   rLink.mPrev->rLink.mNext = rLink.mNext;
314   rLink.mNext->rLink.mPrev = rLink.mPrev;
315   rLink.mPrev = rLink.mNext = this;
316}
317
318CollisionWorkingList* CollisionWorkingList::alloc()
319{
320   if (sFreeList.wLink.mNext != &sFreeList) {
321      CollisionWorkingList* nxt = sFreeList.wLink.mNext;
322      nxt->unlink();
323      return nxt;
324   }
325   return constructInPlace((CollisionWorkingList*)sChunker.alloc(sizeof(CollisionWorkingList)));
326}
327
328void CollisionWorkingList::free()
329{
330   unlink();
331   wLinkAfter(&sFreeList);
332}
333
334
335//----------------------------------------------------------------------------
336// Convex Base Class
337//----------------------------------------------------------------------------
338
339U32 Convex::sTag = (U32)-1;
340
341//----------------------------------------------------------------------------
342
343Convex::Convex()
344{
345   mNext = mPrev = this;
346   mTag = 0;
347   mObject = NULL;
348   mType = ConvexType::BoxConvexType;
349}
350
351Convex::~Convex()
352{
353   // Unlink from Convex Database
354   unlink();
355
356   // Delete collision states
357   while (mList.mNext != &mList)
358      delete mList.mNext->mState;
359
360   // Free up working list
361   while (mWorking.wLink.mNext != &mWorking)
362      mWorking.wLink.mNext->free();
363
364   // Free up references
365   while (mReference.rLink.mNext != &mReference)
366      mReference.rLink.mNext->free();
367}
368
369
370//----------------------------------------------------------------------------
371
372void Convex::collectGarbage()
373{
374   // Delete unreferenced Convex Objects
375   for (Convex* itr = mNext; itr != this; itr = itr->mNext) {
376      if (itr->mReference.rLink.mNext == &itr->mReference) {
377         Convex* ptr = itr;
378         itr = itr->mPrev;
379         delete ptr;
380      }
381   }
382}
383
384void Convex::nukeList()
385{
386   // Delete all Convex Objects
387   for (Convex* itr = mNext; itr != this; itr = itr->mNext) {
388      Convex* ptr = itr;
389      itr = itr->mPrev;
390      delete ptr;
391   }
392}
393
394void Convex::registerObject(Convex *convex)
395{
396   convex->linkAfter(this);
397}
398
399
400//----------------------------------------------------------------------------
401
402void Convex::linkAfter(Convex* ptr)
403{
404   mPrev = ptr;
405   mNext = ptr->mNext;
406   ptr->mNext = this;
407   mNext->mPrev = this;
408}
409
410void Convex::unlink()
411{
412   mPrev->mNext = mNext;
413   mNext->mPrev = mPrev;
414   mPrev = mNext = this;
415}
416
417
418//----------------------------------------------------------------------------
419
420Point3F Convex::support(const VectorF&) const
421{
422   return Point3F(0,0,0);
423}
424
425void Convex::getFeatures(const MatrixF&,const VectorF&,ConvexFeature* f)
426{
427   f->mObject = NULL;
428}
429
430const MatrixF& Convex::getTransform() const
431{
432   return mObject->getTransform();
433}
434
435const Point3F& Convex::getScale() const
436{
437   return mObject->getScale();
438}
439
440Box3F Convex::getBoundingBox() const
441{
442   return mObject->getWorldBox();
443}
444
445Box3F Convex::getBoundingBox(const MatrixF& mat, const Point3F& scale) const
446{
447   Box3F wBox = mObject->getObjBox();
448   wBox.minExtents.convolve(scale);
449   wBox.maxExtents.convolve(scale);
450   mat.mul(wBox);
451   return wBox;
452}
453
454//----------------------------------------------------------------------------
455
456void Convex::addToWorkingList(Convex* ptr)
457{
458   CollisionWorkingList* cl = CollisionWorkingList::alloc();
459   cl->wLinkAfter(&mWorking);
460   cl->rLinkAfter(&ptr->mReference);
461   cl->mConvex = ptr;
462};
463
464
465//----------------------------------------------------------------------------
466
467void Convex::updateWorkingList(const Box3F& box, const U32 colMask)
468{
469   PROFILE_SCOPE( Convex_UpdateWorkingList );
470
471   sTag++;
472
473   // Clear objects off the working list that are no longer intersecting
474   for (CollisionWorkingList* itr = mWorking.wLink.mNext; itr != &mWorking; itr = itr->wLink.mNext) {
475      itr->mConvex->mTag = sTag;
476      if ((!box.isOverlapped(itr->mConvex->getBoundingBox())) || (!itr->mConvex->getObject()->isCollisionEnabled())) {
477         CollisionWorkingList* cl = itr;
478         itr = itr->wLink.mPrev;
479         cl->free();
480      }
481   }
482
483   // Special processing for the terrain and interiors...
484   AssertFatal(mObject->getContainer(), "Must be in a container!");
485
486   SimpleQueryList sql;
487   mObject->getContainer()->findObjects(box, colMask,SimpleQueryList::insertionCallback, &sql);
488   for (U32 i = 0; i < sql.mList.size(); i++)
489      sql.mList[i]->buildConvex(box, this);
490}
491
492void Convex::clearWorkingList()
493{
494   PROFILE_SCOPE( Convex_ClearWorkingList );
495
496   sTag++;
497
498   for (CollisionWorkingList* itr = mWorking.wLink.mNext; itr != &mWorking; itr = itr->wLink.mNext)
499   {
500      itr->mConvex->mTag = sTag;
501      CollisionWorkingList* cl = itr;
502      itr = itr->wLink.mPrev;
503      cl->free();
504   }
505}
506
507// ---------------------------------------------------------------------------
508
509void Convex::updateStateList(const MatrixF& mat, const Point3F& scale, const Point3F* displacement)
510{
511   PROFILE_SCOPE( Convex_UpdateStateList );
512
513   Box3F box1 = getBoundingBox(mat, scale);
514   box1.minExtents -= Point3F(1, 1, 1);
515   box1.maxExtents += Point3F(1, 1, 1);
516   if (displacement) {
517      Point3F oldMin = box1.minExtents;
518      Point3F oldMax = box1.maxExtents;
519
520      box1.minExtents.setMin(oldMin + *displacement);
521      box1.minExtents.setMin(oldMax + *displacement);
522      box1.maxExtents.setMax(oldMin + *displacement);
523      box1.maxExtents.setMax(oldMax + *displacement);
524   }
525   sTag++;
526
527   // Destroy states which are no longer intersecting
528   for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext) {
529      Convex* cv = (itr->mState->mA == this)? itr->mState->mB: itr->mState->mA;
530      cv->mTag = sTag;
531      if (!box1.isOverlapped(cv->getBoundingBox())) {
532         CollisionState* cs = itr->mState;
533         itr = itr->mPrev;
534         delete cs;
535      }
536   }
537
538   // Add collision states for new overlapping objects
539   for (CollisionWorkingList* itr0 = mWorking.wLink.mNext; itr0 != &mWorking; itr0 = itr0->wLink.mNext) {
540      Convex* cv = itr0->mConvex;
541      if (cv->mTag != sTag && box1.isOverlapped(cv->getBoundingBox())) {
542         CollisionState* state = new GjkCollisionState;
543         state->set(this,cv,mat,cv->getTransform());
544         state->mLista->linkAfter(&mList);
545         state->mListb->linkAfter(&cv->mList);
546      }
547   }
548}
549
550
551//----------------------------------------------------------------------------
552
553CollisionState* Convex::findClosestState(const MatrixF& mat, const Point3F& scale, const F32 dontCareDist)
554{
555   PROFILE_SCOPE( Convex_FindClosestState );
556
557   updateStateList(mat, scale);
558   F32 dist = +1E30f;
559   CollisionState *st = 0;
560
561   // Prepare scaled version of transform
562   MatrixF axform = mat;
563   axform.scale(scale);
564   MatrixF axforminv(true);
565   MatrixF temp(mat);
566   axforminv.scale(Point3F(1.0f/scale.x, 1.0f/scale.y, 1.0f/scale.z));
567   temp.affineInverse();
568   axforminv.mul(temp);
569
570   for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext) 
571   {
572      CollisionState* state = itr->mState;
573      if (state->mLista != itr)
574         state->swap();
575
576      // Prepare scaled version of transform
577      MatrixF bxform = state->mB->getTransform();
578      temp = bxform;
579      Point3F bscale = state->mB->getScale();
580      bxform.scale(bscale);
581      MatrixF bxforminv(true);
582      bxforminv.scale(Point3F(1.0f/bscale.x, 1.0f/bscale.y, 1.0f/bscale.z));
583      temp.affineInverse();
584      bxforminv.mul(temp);
585
586      //
587      F32 dd = state->distance(axform, bxform, dontCareDist, &axforminv, &bxforminv);
588      if (dd < dist) 
589      {
590         dist = dd;
591         st = state;
592      }
593   }
594   if (dist < dontCareDist)
595      return st;
596   else
597      return NULL;
598}
599
600
601//----------------------------------------------------------------------------
602
603bool Convex::getCollisionInfo(const MatrixF& mat, const Point3F& scale, CollisionList* cList,F32 tol)
604{
605   PROFILE_SCOPE( Convex_GetCollisionInfo );
606
607   // Making these static prevents needless Vector resizing that occurs
608   // in the ConvexFeature constructor.
609   static ConvexFeature fa;
610   static ConvexFeature fb;
611
612   for ( CollisionStateList* itr = mList.mNext; 
613         itr != &mList; 
614         itr = itr->mNext) 
615   {
616
617      CollisionState* state = itr->mState;
618
619      if (state->mLista != itr)
620         state->swap();
621
622      if (state->mDist <= tol) 
623      {
624         fa.reset();
625         fb.reset();
626         VectorF v;
627
628         // The idea is that we need to scale the matrix, so we need to
629         // make a copy of it, before we can pass it in to getFeatures.
630         // This is used to scale us for comparison against the other
631         // convex, which is correctly scaled.
632         MatrixF omat = mat;
633         omat.scale(scale);
634
635         MatrixF imat = omat;
636         imat.inverse();
637         imat.mulV(-state->mDistvec,&v);
638
639         getFeatures(omat,v,&fa);
640
641         imat = state->mB->getTransform();
642         imat.scale(state->mB->getScale());
643
644         MatrixF bxform = imat;
645         imat.inverse();
646         imat.mulV(state->mDistvec,&v);
647
648         state->mB->getFeatures(bxform,v,&fb);
649
650         fa.collide(fb,cList,tol);
651      }
652   }
653
654   return (cList->getCount() != 0);
655}
656
657void Convex::getPolyList(AbstractPolyList*)
658{
659
660}
661
662void Convex::renderWorkingList()
663{
664   //bool rendered = false;
665
666   CollisionWorkingList& rList = getWorkingList();
667   CollisionWorkingList* pList = rList.wLink.mNext;
668   while (pList != &rList) {
669      Convex* pConvex = pList->mConvex;
670      pConvex->render();
671      //rendered = true;
672      pList = pList->wLink.mNext;
673   }
674
675   //Con::warnf( "convex rendered - %s", rendered ? "YES" : "NO" );
676}
677
678void Convex::render()
679{
680   ConcretePolyList polyList;
681   getPolyList( &polyList );
682   polyList.render();
683}
684
685//-----------------------------------------------------------------------------
686// This function based on code originally written for the book:
687// 3D Game Engine Design, by David H. Eberly
688//
689F32 sqrDistanceEdges(const Point3F& start0, const Point3F& end0,
690   const Point3F& start1, const Point3F& end1,
691   Point3F* is, Point3F* it)
692{
693   Point3F direction0 = end0 - start0;
694   F32 fA00 = direction0.lenSquared();
695
696   Point3F direction1 = end1 - start1;
697   F32 fA11 = direction1.lenSquared();
698   F32 fA01 = -mDot(direction0, direction1);
699
700   Point3F kDiff = start0 - start1;
701   F32 fC   = kDiff.lenSquared();
702   F32 fB0  = mDot(kDiff, direction0);
703   F32 fDet = mFabs(fA00*fA11 - fA01*fA01);
704
705   // Since the endpoints are tested as vertices, we're not interested
706   // in parallel lines, and intersections that don't involve end-points.
707   if (fDet >= 0.00001) {
708
709      // Calculate time of intersection for each line
710      F32 fB1 = -mDot(kDiff, direction1);
711      F32 fS  = fA01*fB1-fA11*fB0;
712      F32 fT  = fA01*fB0-fA00*fB1;
713
714      // Only interested in collisions that don't involve the end points
715      if (fS >= 0.0 && fS <= fDet && fT >= 0.0 && fT <= fDet) {
716         F32 fInvDet = 1.0 / fDet;
717         fS *= fInvDet;
718         fT *= fInvDet;
719         F32 fSqrDist = (fS*(fA00*fS + fA01*fT + 2.0*fB0) +
720            fT*(fA01*fS + fA11*fT + 2.0*fB1) + fC);
721
722         // Intersection points.
723         *is = start0 + direction0 * fS;
724         *it = start1 + direction1 * fT;
725         return mFabs(fSqrDist);
726      }
727   }
728
729   // Return a large number in the cases where endpoints are involved.
730   return 1e10f;
731}
732