| 285 | //---------------------------------------------------------------------------- |
| 286 | |
| 287 | bool GjkCollisionState::intersect(const MatrixF& a2w, const MatrixF& b2w) |
| 288 | { |
| 289 | num_iterations = 0; |
| 290 | MatrixF w2a,w2b; |
| 291 | |
| 292 | w2a = a2w; |
| 293 | w2b = b2w; |
| 294 | w2a.inverse(); |
| 295 | w2b.inverse(); |
| 296 | reset(a2w,b2w); |
| 297 | |
| 298 | mBits = 0; |
| 299 | mAll_bits = 0; |
| 300 | |
| 301 | do { |
| 302 | nextBit(); |
| 303 | |
| 304 | VectorF va,sa; |
| 305 | w2a.mulV(-mDistvec,&va); |
| 306 | mP[mLast] = mA->support(va); |
| 307 | a2w.mulP(mP[mLast],&sa); |
| 308 | |
| 309 | VectorF vb,sb; |
| 310 | w2b.mulV(mDistvec,&vb); |
| 311 | mQ[mLast] = mB->support(vb); |
| 312 | b2w.mulP(mQ[mLast],&sb); |
| 313 | |
| 314 | VectorF w = sa - sb; |
| 315 | if (mDot(mDistvec,w) > 0) |
| 316 | return false; |
| 317 | if (degenerate(w)) { |
| 318 | ++num_irregularities; |
| 319 | return false; |
| 320 | } |
| 321 | |
| 322 | mY[mLast] = w; |
| 323 | mAll_bits = mBits | mLast_bit; |
| 324 | |
| 325 | ++num_iterations; |
| 326 | if (!closest(mDistvec) || num_iterations > sIteration) { |
| 327 | ++num_irregularities; |
| 328 | return false; |
| 329 | } |
| 330 | } |
| 331 | while (mBits < 15 && mDistvec.lenSquared() > sEpsilon2); |
| 332 | return true; |
| 333 | } |
| 334 | |
| 335 | F32 GjkCollisionState::distance(const MatrixF& a2w, const MatrixF& b2w, |
| 336 | const F32 dontCareDist, const MatrixF* _w2a, const MatrixF* _w2b) |