////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// * Initializes a collision query : * - reset stats & contact status * - setup matrices * * \param world0 [in] world matrix for first object * \param world1 [in] world matrix for second object *
| 232 | */ |
| 233 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
| 234 | void AABBTreeCollider::InitQuery(const Matrix4x4* world0, const Matrix4x4* world1) |
| 235 | { |
| 236 | // Reset stats & contact status |
| 237 | Collider::InitQuery(); |
| 238 | mNbBVBVTests = 0; |
| 239 | mNbPrimPrimTests = 0; |
| 240 | mNbBVPrimTests = 0; |
| 241 | mPairs.Reset(); |
| 242 | |
| 243 | // Setup matrices |
| 244 | Matrix4x4 InvWorld0, InvWorld1; |
| 245 | if(world0) InvertPRMatrix(InvWorld0, *world0); |
| 246 | else InvWorld0.Identity(); |
| 247 | |
| 248 | if(world1) InvertPRMatrix(InvWorld1, *world1); |
| 249 | else InvWorld1.Identity(); |
| 250 | |
| 251 | Matrix4x4 World0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1; |
| 252 | Matrix4x4 World1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0; |
| 253 | |
| 254 | mR0to1 = World0to1; World0to1.GetTrans(mT0to1); |
| 255 | mR1to0 = World1to0; World1to0.GetTrans(mT1to0); |
| 256 | |
| 257 | // Precompute absolute 1-to-0 rotation matrix |
| 258 | for(udword i=0;i<3;i++) |
| 259 | { |
| 260 | for(udword j=0;j<3;j++) |
| 261 | { |
| 262 | // Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID) |
| 263 | mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]); |
| 264 | } |
| 265 | } |
| 266 | } |
| 267 | |
| 268 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
| 269 | /** |