| 325 | |
| 326 | |
| 327 | bool TetModel::pointInTriangle(const Vector3r& p0, const Vector3r& p1, const Vector3r& p2, const Vector3r& p, |
| 328 | Vector3r& inter, Vector3r &bary) |
| 329 | { |
| 330 | // see Bridson: Robust treatment of collisions contact and friction for cloth animation |
| 331 | const Vector3r x43 = p - p2; |
| 332 | const Vector3r x13 = p0 - p2; |
| 333 | const Vector3r x23 = p1 - p2; |
| 334 | |
| 335 | // compute inv matrix a,b,b,c |
| 336 | Real a = x13.dot(x13); |
| 337 | Real b = x13.dot(x23); |
| 338 | Real c = x23.dot(x23); |
| 339 | const Real det = a*c - b*b; |
| 340 | if (fabs(det) < 1.0e-9) |
| 341 | return false; |
| 342 | |
| 343 | Real d1 = x13.dot(x43); |
| 344 | Real d2 = x23.dot(x43); |
| 345 | |
| 346 | Real w1 = (c*d1 - b*d2) / det; |
| 347 | Real w2 = (a*d2 - b*d1) / det; |
| 348 | |
| 349 | // this clamping gives not an exact orthogonal point to the edge!! |
| 350 | if (w1 < 0) w1 = 0; |
| 351 | if (w1 > 1) w1 = 1; |
| 352 | if (w2 < 0) w2 = 0; |
| 353 | if (w2 > 1) w2 = 1; |
| 354 | |
| 355 | bary[0] = w1; |
| 356 | bary[1] = w2; |
| 357 | bary[2] = (Real)1 - w1 - w2; |
| 358 | |
| 359 | if (bary[2] < 0) |
| 360 | { |
| 361 | // this gives not an exact orthogonal point to the edge!! |
| 362 | const Real w12 = w1 + w2; |
| 363 | bary[0] -= w2 / (w12)*(w12 - 1); |
| 364 | bary[1] -= w1 / (w12)*(w12 - 1); |
| 365 | bary[2] = 0; |
| 366 | } |
| 367 | |
| 368 | inter = p2 + bary[0] * x13 + bary[1] * x23; |
| 369 | |
| 370 | return true; |
| 371 | } |