| 333 | } |
| 334 | |
| 335 | F32 GjkCollisionState::distance(const MatrixF& a2w, const MatrixF& b2w, |
| 336 | const F32 dontCareDist, const MatrixF* _w2a, const MatrixF* _w2b) |
| 337 | { |
| 338 | num_iterations = 0; |
| 339 | MatrixF w2a,w2b; |
| 340 | |
| 341 | if (_w2a == NULL || _w2b == NULL) { |
| 342 | w2a = a2w; |
| 343 | w2b = b2w; |
| 344 | w2a.inverse(); |
| 345 | w2b.inverse(); |
| 346 | } |
| 347 | else { |
| 348 | w2a = *_w2a; |
| 349 | w2b = *_w2b; |
| 350 | } |
| 351 | |
| 352 | reset(a2w,b2w); |
| 353 | mBits = 0; |
| 354 | mAll_bits = 0; |
| 355 | F32 mu = 0; |
| 356 | |
| 357 | do { |
| 358 | nextBit(); |
| 359 | |
| 360 | VectorF va,sa; |
| 361 | w2a.mulV(-mDistvec,&va); |
| 362 | mP[mLast] = mA->support(va); |
| 363 | a2w.mulP(mP[mLast],&sa); |
| 364 | |
| 365 | VectorF vb,sb; |
| 366 | w2b.mulV(mDistvec,&vb); |
| 367 | mQ[mLast] = mB->support(vb); |
| 368 | b2w.mulP(mQ[mLast],&sb); |
| 369 | |
| 370 | VectorF w = sa - sb; |
| 371 | F32 nm = mDot(mDistvec, w) / mDist; |
| 372 | if (nm > mu) |
| 373 | mu = nm; |
| 374 | if (mu > dontCareDist) |
| 375 | return mu; |
| 376 | if (mFabs(mDist - mu) <= mDist * rel_error) |
| 377 | return mDist; |
| 378 | |
| 379 | ++num_iterations; |
| 380 | if (degenerate(w) || num_iterations > sIteration) { |
| 381 | ++num_irregularities; |
| 382 | return mDist; |
| 383 | } |
| 384 | |
| 385 | mY[mLast] = w; |
| 386 | mAll_bits = mBits | mLast_bit; |
| 387 | |
| 388 | if (!closest(mDistvec)) { |
| 389 | ++num_irregularities; |
| 390 | return mDist; |
| 391 | } |
| 392 | |