gjk.cpp

Public Variables

S32

num_irregularities 

S32

num_iterations 

F32

rel_error 

F32

sEpsilon2 

U32

sIteration 

F32

sTolerance 

Public Variables

S32 num_irregularities 

S32 num_iterations 

F32 rel_error 

F32 sEpsilon2 

U32 sIteration 

F32 sTolerance 

  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// The core algorithms in this file are inspired by public papers written
 25// by G. van den Bergen for his interference detection library,
 26// "SOLID 2.0"
 27
 28#include "core/dataChunker.h"
 29#include "collision/collision.h"
 30#include "scene/sceneObject.h"
 31#include "collision/convex.h"
 32#include "collision/gjk.h"
 33
 34
 35//----------------------------------------------------------------------------
 36
 37static F32 rel_error = 1E-5f;     // relative error in the computed distance
 38static F32 sTolerance = 1E-3f;    // Distance tolerance
 39static F32 sEpsilon2 = 1E-20f;    // Zero length vector
 40static U32 sIteration = 15;       // Stuck in a loop?
 41
 42S32 num_iterations = 0;
 43S32 num_irregularities = 0;
 44
 45
 46//----------------------------------------------------------------------------
 47
 48GjkCollisionState::GjkCollisionState()
 49{
 50   mBits = 0;
 51   mAll_bits = 0;
 52   U32 x, y;
 53   for (x = 0; x < 16; x++)
 54      for (y = 0; y < 4; y++)
 55         mDet[x][y] = 0.0f;
 56
 57   for (x = 0; x < 4; x++)
 58      for (y = 0; y < 4; y++)
 59         mDP[x][y] = 0.0f;
 60
 61   mLast = 0;
 62   mLast_bit = 0;
 63   mA = mB = 0;
 64}
 65
 66GjkCollisionState::~GjkCollisionState()
 67{
 68}
 69
 70
 71//----------------------------------------------------------------------------
 72
 73void GjkCollisionState::swap()
 74{
 75   Convex* t = mA; mA = mB; mB = t;
 76   CollisionStateList* l = mLista; mLista = mListb; mListb = l;
 77   mDistvec.neg();
 78}
 79
 80
 81//----------------------------------------------------------------------------
 82
 83void GjkCollisionState::compute_det()
 84{
 85   // Dot new point with current set
 86   for (S32 i = 0, bit = 1; i < 4; ++i, bit <<=1)
 87      if (mBits & bit)
 88        mDP[i][mLast] = mDP[mLast][i] = mDot(mY[i], mY[mLast]);
 89   mDP[mLast][mLast] = mDot(mY[mLast], mY[mLast]);
 90
 91   // Calulate the determinent
 92   mDet[mLast_bit][mLast] = 1;
 93   for (S32 j = 0, sj = 1; j < 4; ++j, sj <<= 1) {
 94      if (mBits & sj) {
 95         S32 s2 = sj | mLast_bit;
 96       mDet[s2][j] = mDP[mLast][mLast] - mDP[mLast][j];
 97       mDet[s2][mLast] = mDP[j][j] - mDP[j][mLast];
 98         for (S32 k = 0, sk = 1; k < j; ++k, sk <<= 1) {
 99            if (mBits & sk) {
100              S32 s3 = sk | s2;
101           mDet[s3][k] = mDet[s2][j] * (mDP[j][j] - mDP[j][k]) +
102                            mDet[s2][mLast] * (mDP[mLast][j] - mDP[mLast][k]);
103           mDet[s3][j] = mDet[sk | mLast_bit][k] * (mDP[k][k] - mDP[k][j]) +
104                            mDet[sk | mLast_bit][mLast] * (mDP[mLast][k] - mDP[mLast][j]);
105           mDet[s3][mLast] = mDet[sk | sj][k] * (mDP[k][k] - mDP[k][mLast]) +
106                                mDet[sk | sj][j] * (mDP[j][k] - mDP[j][mLast]);
107            }
108         }
109      }
110   }
111
112   if (mAll_bits == 15) {
113     mDet[15][0] = mDet[14][1] * (mDP[1][1] - mDP[1][0]) +
114                   mDet[14][2] * (mDP[2][1] - mDP[2][0]) +
115                   mDet[14][3] * (mDP[3][1] - mDP[3][0]);
116    mDet[15][1] = mDet[13][0] * (mDP[0][0] - mDP[0][1]) +
117                   mDet[13][2] * (mDP[2][0] - mDP[2][1]) +
118                   mDet[13][3] * (mDP[3][0] - mDP[3][1]);
119    mDet[15][2] = mDet[11][0] * (mDP[0][0] - mDP[0][2]) +
120                   mDet[11][1] * (mDP[1][0] - mDP[1][2]) +
121                   mDet[11][3] * (mDP[3][0] - mDP[3][2]);
122    mDet[15][3] = mDet[7][0] * (mDP[0][0] - mDP[0][3]) +
123                   mDet[7][1] * (mDP[1][0] - mDP[1][3]) +
124                   mDet[7][2] * (mDP[2][0] - mDP[2][3]);
125   }
126}
127
128
129//----------------------------------------------------------------------------
130
131inline void GjkCollisionState::compute_vector(S32 bits, VectorF& v)
132{
133   F32 sum = 0;
134   v.set(0, 0, 0);
135   for (S32 i = 0, bit = 1; i < 4; ++i, bit <<= 1) {
136      if (bits & bit) {
137         sum += mDet[bits][i];
138         v += mY[i] * mDet[bits][i];
139      }
140   }
141   v *= 1 / sum;
142}
143
144
145//----------------------------------------------------------------------------
146
147inline bool GjkCollisionState::valid(S32 s)
148{
149   for (S32 i = 0, bit = 1; i < 4; ++i, bit <<= 1) {
150      if (mAll_bits & bit) {
151         if (s & bit) {
152            if (mDet[s][i] <= 0)
153               return false;
154         }
155         else
156            if (mDet[s | bit][i] > 0)
157               return false;
158      }
159   }
160   return true;
161}
162
163
164//----------------------------------------------------------------------------
165
166inline bool GjkCollisionState::closest(VectorF& v)
167{
168   compute_det();
169   for (S32 s = mBits; s; --s) {
170      if ((s & mBits) == s) {
171         if (valid(s | mLast_bit)) {
172          mBits = s | mLast_bit;
173            if (mBits != 15)
174               compute_vector(mBits, v);
175            return true;
176         }
177      }
178   }
179   if (valid(mLast_bit)) {
180      mBits = mLast_bit;
181      v = mY[mLast];
182      return true;
183   }
184   return false;
185}
186
187
188//----------------------------------------------------------------------------
189
190inline bool GjkCollisionState::degenerate(const VectorF& w)
191{
192  for (S32 i = 0, bit = 1; i < 4; ++i, bit <<= 1)
193   if ((mAll_bits & bit) && mY[i] == w)
194      return true;
195  return false;
196}
197
198
199//----------------------------------------------------------------------------
200
201inline void GjkCollisionState::nextBit()
202{
203   mLast = 0;
204   mLast_bit = 1;
205   while (mBits & mLast_bit) {
206      ++mLast;
207     mLast_bit <<= 1;
208   }
209}
210
211
212//----------------------------------------------------------------------------
213//----------------------------------------------------------------------------
214
215//----------------------------------------------------------------------------
216
217void GjkCollisionState::set(Convex* aa, Convex* bb,
218   const MatrixF& a2w, const MatrixF& b2w)
219{
220   mA = aa;
221   mB = bb;
222
223   mBits = 0;
224   mAll_bits = 0;
225   reset(a2w,b2w);
226
227   // link
228   mLista = CollisionStateList::alloc();
229   mLista->mState = this;
230   mListb = CollisionStateList::alloc();
231   mListb->mState = this;
232}
233
234
235//----------------------------------------------------------------------------
236
237void GjkCollisionState::reset(const MatrixF& a2w, const MatrixF& b2w)
238{
239   VectorF zero(0,0,0),sa,sb;
240   a2w.mulP(mA->support(zero),&sa);
241   b2w.mulP(mB->support(zero),&sb);
242   mDistvec = sa - sb;
243   mDist = mDistvec.len();
244}
245
246
247//----------------------------------------------------------------------------
248
249void GjkCollisionState::getCollisionInfo(const MatrixF& mat, Collision* info)
250{
251   AssertFatal(false, "GjkCollisionState::getCollisionInfo() - There remain scaling problems here.");
252   // This assumes that the shapes do not intersect
253   Point3F pa,pb;
254   if (mBits) {
255      getClosestPoints(pa,pb);
256      mat.mulP(pa,&info->point);
257     mB->getTransform().mulP(pb,&pa);
258      info->normal = info->point - pa;
259   }
260   else {
261      mat.mulP(mP[mLast],&info->point);
262      info->normal = mDistvec;
263   }
264   info->normal.normalize();
265   info->object = mB->getObject();
266}
267
268void GjkCollisionState::getClosestPoints(Point3F& p1, Point3F& p2)
269{
270   F32 sum = 0;
271   p1.set(0, 0, 0);
272   p2.set(0, 0, 0);
273   for (S32 i = 0, bit = 1; i < 4; ++i, bit <<= 1) {
274      if (mBits & bit) {
275         sum += mDet[mBits][i];
276         p1 += mP[i] * mDet[mBits][i];
277         p2 += mQ[i] * mDet[mBits][i];
278      }
279   }
280   F32 s = 1 / sum;
281   p1 *= s;
282   p2 *= s;
283}
284
285
286//----------------------------------------------------------------------------
287
288bool GjkCollisionState::intersect(const MatrixF& a2w, const MatrixF& b2w)
289{
290   num_iterations = 0;
291   MatrixF w2a,w2b;
292
293   w2a = a2w;
294   w2b = b2w;
295   w2a.inverse();
296   w2b.inverse();
297   reset(a2w,b2w);
298
299   mBits = 0;
300   mAll_bits = 0;
301
302   do {
303      nextBit();
304
305      VectorF va,sa;
306      w2a.mulV(-mDistvec,&va);
307     mP[mLast] = mA->support(va);
308      a2w.mulP(mP[mLast],&sa);
309
310      VectorF vb,sb;
311      w2b.mulV(mDistvec,&vb);
312     mQ[mLast] = mB->support(vb);
313      b2w.mulP(mQ[mLast],&sb);
314
315      VectorF w = sa - sb;
316      if (mDot(mDistvec,w) > 0)
317         return false;
318      if (degenerate(w)) {
319         ++num_irregularities;
320         return false;
321      }
322
323     mY[mLast] = w;
324     mAll_bits = mBits | mLast_bit;
325
326      ++num_iterations;
327      if (!closest(mDistvec) || num_iterations > sIteration) {
328         ++num_irregularities;
329         return false;
330      }
331   }
332   while (mBits < 15 && mDistvec.lenSquared() > sEpsilon2);
333   return true;
334}
335
336F32 GjkCollisionState::distance(const MatrixF& a2w, const MatrixF& b2w,
337   const F32 dontCareDist, const MatrixF* _w2a, const MatrixF* _w2b)
338{
339   num_iterations = 0;
340   MatrixF w2a,w2b;
341
342   if (_w2a == NULL || _w2b == NULL) {
343      w2a = a2w;
344      w2b = b2w;
345      w2a.inverse();
346      w2b.inverse();
347   }
348   else {
349      w2a = *_w2a;
350      w2b = *_w2b;
351   }
352
353   reset(a2w,b2w);
354   mBits = 0;
355   mAll_bits = 0;
356   F32 mu = 0;
357
358   do {
359      nextBit();
360
361      VectorF va,sa;
362      w2a.mulV(-mDistvec,&va);
363     mP[mLast] = mA->support(va);
364      a2w.mulP(mP[mLast],&sa);
365
366      VectorF vb,sb;
367      w2b.mulV(mDistvec,&vb);
368     mQ[mLast] = mB->support(vb);
369      b2w.mulP(mQ[mLast],&sb);
370
371      VectorF w = sa - sb;
372      F32 nm = mDot(mDistvec, w) / mDist;
373      if (nm > mu)
374         mu = nm;
375      if (mu > dontCareDist)
376         return mu;
377      if (mFabs(mDist - mu) <= mDist * rel_error)
378         return mDist;
379
380      ++num_iterations;
381      if (degenerate(w) || num_iterations > sIteration) {
382         ++num_irregularities;
383         return mDist;
384      }
385
386     mY[mLast] = w;
387     mAll_bits = mBits | mLast_bit;
388
389      if (!closest(mDistvec)) {
390         ++num_irregularities;
391         return mDist;
392      }
393
394     mDist = mDistvec.len();
395   }
396   while (mBits < 15 && mDist > sTolerance) ;
397
398   if (mBits == 15 && mu <= 0)
399      mDist = 0;
400   return mDist;
401}
402