00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include "fcl/intersect.h"
00038 #include <iostream>
00039 #include <limits>
00040 #include <vector>
00041 #include <boost/math/special_functions/fpclassify.hpp>
00042
00043 namespace fcl
00044 {
00045 const FCL_REAL PolySolver::NEAR_ZERO_THRESHOLD = 1e-9;
00046
00047
00048 bool PolySolver::isZero(FCL_REAL v)
00049 {
00050 return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD);
00051 }
00052
00053 bool PolySolver::cbrt(FCL_REAL v)
00054 {
00055 return powf(v, 1.0 / 3.0);
00056 }
00057
00058 int PolySolver::solveLinear(FCL_REAL c[2], FCL_REAL s[1])
00059 {
00060 if(isZero(c[1]))
00061 return 0;
00062 s[0] = - c[0] / c[1];
00063 return 1;
00064 }
00065
00066 int PolySolver::solveQuadric(FCL_REAL c[3], FCL_REAL s[2])
00067 {
00068 FCL_REAL p, q, D;
00069
00070
00071
00072 if(isZero(c[2]))
00073 return solveLinear(c, s);
00074
00075
00076 p = c[1] / (2.0 * c[2]);
00077 q = c[0] / c[2];
00078 D = p * p - q;
00079
00080 if(isZero(D))
00081 {
00082
00083 s[0] = s[1] = -p;
00084 return 1;
00085 }
00086
00087 if(D < 0.0)
00088
00089 return 0;
00090 else
00091 {
00092
00093 FCL_REAL sqrt_D = sqrt(D);
00094 s[0] = sqrt_D - p;
00095 s[1] = -sqrt_D - p;
00096 return 2;
00097 }
00098 }
00099
00100 int PolySolver::solveCubic(FCL_REAL c[4], FCL_REAL s[3])
00101 {
00102 int i, num;
00103 FCL_REAL sub, A, B, C, sq_A, p, q, cb_p, D;
00104 const FCL_REAL ONE_OVER_THREE = 1 / 3.0;
00105 const FCL_REAL PI = 3.14159265358979323846;
00106
00107
00108 if(isZero(c[3]))
00109 return solveQuadric(c, s);
00110
00111
00112 A = c[2] / c[3];
00113 B = c[1] / c[3];
00114 C = c[0] / c[3];
00115
00116
00117 sq_A = A * A;
00118 p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE;
00119 q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C);
00120
00121
00122 cb_p = p * p * p;
00123 D = q * q + cb_p;
00124
00125 if(isZero(D))
00126 {
00127 if(isZero(q))
00128 {
00129
00130 s[0] = 0.0;
00131 num = 1;
00132 }
00133 else
00134 {
00135
00136 FCL_REAL u = cbrt(-q);
00137 s[0] = 2.0 * u;
00138 s[1] = -u;
00139 num = 2;
00140 }
00141 }
00142 else
00143 {
00144 if(D < 0.0)
00145 {
00146
00147 FCL_REAL phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p));
00148 FCL_REAL t = 2.0 * sqrt(-p);
00149 s[0] = t * cos(phi);
00150 s[1] = -t * cos(phi + PI / 3.0);
00151 s[2] = -t * cos(phi - PI / 3.0);
00152 num = 3;
00153 }
00154 else
00155 {
00156
00157 FCL_REAL sqrt_D = sqrt(D);
00158 FCL_REAL u = cbrt(sqrt_D + fabs(q));
00159 if(q > 0.0)
00160 s[0] = - u + p / u ;
00161 else
00162 s[0] = u - p / u;
00163 num = 1;
00164 }
00165 }
00166
00167
00168 sub = ONE_OVER_THREE * A;
00169 for(i = 0; i < num; i++)
00170 s[i] -= sub;
00171 return num;
00172 }
00173
00174
00175
00176 const FCL_REAL Intersect::EPSILON = 1e-5;
00177 const FCL_REAL Intersect::NEAR_ZERO_THRESHOLD = 1e-7;
00178 const FCL_REAL Intersect::CCD_RESOLUTION = 1e-7;
00179
00180
00181 bool Intersect::isZero(FCL_REAL v)
00182 {
00183 return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD);
00184 }
00185
00187 bool Intersect::solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00188 const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00189 FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data)
00190 {
00191 FCL_REAL v2[2]= {l*l,r*r};
00192 FCL_REAL v[2]= {l,r};
00193 FCL_REAL r_backup;
00194
00195 unsigned char min3, min2, min1, max3, max2, max1;
00196
00197 min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1;
00198 min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1;
00199 min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1;
00200
00201
00202
00203 FCL_REAL minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0];
00204 FCL_REAL major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0];
00205
00206 if(major<0) return false;
00207 if(minor>0) return false;
00208
00209
00210 FCL_REAL m = 0.5 * (r + l);
00211
00212
00213 FCL_REAL dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1];
00214 FCL_REAL dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1];
00215
00216 if((dminor > 0)||(dmajor < 0))
00217 {
00218 FCL_REAL m2 = m*m;
00219 FCL_REAL fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0];
00220 FCL_REAL nl = m;
00221 FCL_REAL nu = m;
00222 if(fm>0)
00223 {
00224 nl-=(fm/dminor);
00225 nu-=(fm/dmajor);
00226 }
00227 else
00228 {
00229 nu-=(fm/dminor);
00230 nl-=(fm/dmajor);
00231 }
00232
00233
00234
00235 if(nl>r) return false;
00236 if(nu<l) return false;
00237 if(nl>l)
00238 {
00239 if(nu<r) { l=nl; r=nu; m=0.5*(l+r); }
00240 else { l=nl; m=0.5*(l+r); }
00241 }
00242 else
00243 {
00244 if(nu<r) { r=nu; m=0.5*(l+r); }
00245 }
00246 }
00247
00248
00249 if((r-l)< CCD_RESOLUTION)
00250 {
00251 if(bVF)
00252 return checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r);
00253 else
00254 return checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, data);
00255 }
00256
00257 r_backup = r, r = m;
00258 if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, bVF, coeffs, data))
00259 return true;
00260
00261 l = m, r = r_backup;
00262 return solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, bVF, coeffs, data);
00263 }
00264
00265
00266
00267 bool Intersect::insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p)
00268 {
00269 Vec3f ab = b - a;
00270 Vec3f ac = c - a;
00271 Vec3f n = ab.cross(ac);
00272
00273 Vec3f pa = a - p;
00274 Vec3f pb = b - p;
00275 Vec3f pc = c - p;
00276
00277 if((pb.cross(pc)).dot(n) < -EPSILON) return false;
00278 if((pc.cross(pa)).dot(n) < -EPSILON) return false;
00279 if((pa.cross(pb)).dot(n) < -EPSILON) return false;
00280
00281 return true;
00282 }
00283
00284 bool Intersect::insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p)
00285 {
00286 return (p - a).dot(p - b) <= 0;
00287 }
00288
00294 bool Intersect::linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
00295 Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub)
00296 {
00297 Vec3f p31 = p1 - p3;
00298 Vec3f p34 = p4 - p3;
00299 if(fabs(p34[0]) < EPSILON && fabs(p34[1]) < EPSILON && fabs(p34[2]) < EPSILON)
00300 return false;
00301
00302 Vec3f p12 = p2 - p1;
00303 if(fabs(p12[0]) < EPSILON && fabs(p12[1]) < EPSILON && fabs(p12[2]) < EPSILON)
00304 return false;
00305
00306 FCL_REAL d3134 = p31.dot(p34);
00307 FCL_REAL d3412 = p34.dot(p12);
00308 FCL_REAL d3112 = p31.dot(p12);
00309 FCL_REAL d3434 = p34.dot(p34);
00310 FCL_REAL d1212 = p12.dot(p12);
00311
00312 FCL_REAL denom = d1212 * d3434 - d3412 * d3412;
00313 if(fabs(denom) < EPSILON)
00314 return false;
00315 FCL_REAL numer = d3134 * d3412 - d3112 * d3434;
00316
00317 *mua = numer / denom;
00318 if(*mua < 0 || *mua > 1)
00319 return false;
00320
00321 *mub = (d3134 + d3412 * (*mua)) / d3434;
00322 if(*mub < 0 || *mub > 1)
00323 return false;
00324
00325 *pa = p1 + p12 * (*mua);
00326 *pb = p3 + p34 * (*mub);
00327 return true;
00328 }
00329
00330 bool Intersect::checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00331 const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
00332 FCL_REAL t)
00333 {
00334 return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t);
00335 }
00336
00337 bool Intersect::checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00338 const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00339 FCL_REAL t, Vec3f* q_i)
00340 {
00341 Vec3f a = a0 + va * t;
00342 Vec3f b = b0 + vb * t;
00343 Vec3f c = c0 + vc * t;
00344 Vec3f d = d0 + vd * t;
00345 Vec3f p1, p2;
00346 FCL_REAL t_ab, t_cd;
00347 if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd))
00348 {
00349 if(q_i) *q_i = p1;
00350 return true;
00351 }
00352
00353 return false;
00354 }
00355
00356 bool Intersect::checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00357 const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
00358 FCL_REAL t)
00359 {
00360 return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t);
00361 }
00362
00363 bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
00364 const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00365 const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00366 bool bVF,
00367 FCL_REAL* ret)
00368 {
00369 FCL_REAL discriminant = b * b - 4 * a * c;
00370 if(discriminant < 0)
00371 return false;
00372
00373 FCL_REAL sqrt_dis = sqrt(discriminant);
00374 FCL_REAL r1 = (-b + sqrt_dis) / (2 * a);
00375 bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false;
00376
00377 FCL_REAL r2 = (-b - sqrt_dis) / (2 * a);
00378 bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false;
00379
00380 if(v1 && v2)
00381 {
00382 *ret = (r1 > r2) ? r2 : r1;
00383 return true;
00384 }
00385 if(v1)
00386 {
00387 *ret = r1;
00388 return true;
00389 }
00390 if(v2)
00391 {
00392 *ret = r2;
00393 return true;
00394 }
00395
00396 return false;
00397 }
00398
00399 bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
00400 const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00401 const Vec3f& va, const Vec3f& vb, const Vec3f& vp)
00402 {
00403 if(isZero(a))
00404 {
00405 FCL_REAL t = -c/b;
00406 return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false;
00407 }
00408
00409 FCL_REAL discriminant = b*b-4*a*c;
00410 if(discriminant < 0)
00411 return false;
00412
00413 FCL_REAL sqrt_dis = sqrt(discriminant);
00414
00415 FCL_REAL r1 = (-b+sqrt_dis) / (2 * a);
00416 bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false;
00417 if(v1) return true;
00418
00419 FCL_REAL r2 = (-b-sqrt_dis) / (2 * a);
00420 bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false;
00421 return v2;
00422 }
00423
00424
00425
00428 void Intersect::computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00429 const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
00430 FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d)
00431 {
00432 Vec3f vavb = vb - va;
00433 Vec3f vavc = vc - va;
00434 Vec3f vavp = vp - va;
00435 Vec3f a0b0 = b0 - a0;
00436 Vec3f a0c0 = c0 - a0;
00437 Vec3f a0p0 = p0 - a0;
00438
00439 Vec3f vavb_cross_vavc = vavb.cross(vavc);
00440 Vec3f vavb_cross_a0c0 = vavb.cross(a0c0);
00441 Vec3f a0b0_cross_vavc = a0b0.cross(vavc);
00442 Vec3f a0b0_cross_a0c0 = a0b0.cross(a0c0);
00443
00444 *a = vavp.dot(vavb_cross_vavc);
00445 *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc);
00446 *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc);
00447 *d = a0p0.dot(a0b0_cross_a0c0);
00448 }
00449
00450 void Intersect::computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00451 const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00452 FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d)
00453 {
00454 Vec3f vavb = vb - va;
00455 Vec3f vcvd = vd - vc;
00456 Vec3f vavc = vc - va;
00457 Vec3f c0d0 = d0 - c0;
00458 Vec3f a0b0 = b0 - a0;
00459 Vec3f a0c0 = c0 - a0;
00460 Vec3f vavb_cross_vcvd = vavb.cross(vcvd);
00461 Vec3f vavb_cross_c0d0 = vavb.cross(c0d0);
00462 Vec3f a0b0_cross_vcvd = a0b0.cross(vcvd);
00463 Vec3f a0b0_cross_c0d0 = a0b0.cross(c0d0);
00464
00465 *a = vavc.dot(vavb_cross_vcvd);
00466 *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd);
00467 *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd);
00468 *d = a0c0.dot(a0b0_cross_c0d0);
00469 }
00470
00471 void Intersect::computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00472 const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
00473 const Vec3f& L,
00474 FCL_REAL* a, FCL_REAL* b, FCL_REAL* c)
00475 {
00476 Vec3f vbva = va - vb;
00477 Vec3f vbvp = vp - vb;
00478 Vec3f b0a0 = a0 - b0;
00479 Vec3f b0p0 = p0 - b0;
00480
00481 Vec3f L_cross_vbvp = L.cross(vbvp);
00482 Vec3f L_cross_b0p0 = L.cross(b0p0);
00483
00484 *a = L_cross_vbvp.dot(vbva);
00485 *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva);
00486 *c = L_cross_b0p0.dot(b0a0);
00487 }
00488
00489
00490 bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00491 const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
00492 FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
00493 {
00494 *collision_time = 2.0;
00495
00496 Vec3f vp, va, vb, vc;
00497 vp = p1 - p0;
00498 va = a1 - a0;
00499 vb = b1 - b0;
00500 vc = c1 - c0;
00501
00502 FCL_REAL a, b, c, d;
00503 computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d);
00504
00505 if(isZero(a) && isZero(b) && isZero(c) && isZero(d))
00506 {
00507 return false;
00508 }
00509
00510
00515
00516 FCL_REAL coeffs[4];
00517 coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
00518
00519 if(useNewton)
00520 {
00521 FCL_REAL l = 0;
00522 FCL_REAL r = 1;
00523
00524 if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs))
00525 {
00526 *collision_time = 0.5 * (l + r);
00527 }
00528 }
00529 else
00530 {
00531 FCL_REAL roots[3];
00532 int num = PolySolver::solveCubic(coeffs, roots);
00533 for(int i = 0; i < num; ++i)
00534 {
00535 FCL_REAL r = roots[i];
00536 if(r < 0 || r > 1) continue;
00537 if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r))
00538 {
00539 *collision_time = r;
00540 break;
00541 }
00542 }
00543 }
00544
00545 if(*collision_time > 1)
00546 {
00547 return false;
00548 }
00549
00550 *p_i = vp * (*collision_time) + p0;
00551 return true;
00552 }
00553
00554 bool Intersect::intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00555 const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
00556 FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
00557 {
00558 *collision_time = 2.0;
00559
00560 Vec3f va, vb, vc, vd;
00561 va = a1 - a0;
00562 vb = b1 - b0;
00563 vc = c1 - c0;
00564 vd = d1 - d0;
00565
00566 FCL_REAL a, b, c, d;
00567 computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d);
00568
00569 if(isZero(a) && isZero(b) && isZero(c) && isZero(d))
00570 {
00571 return false;
00572 }
00573
00578
00579
00580 FCL_REAL coeffs[4];
00581 coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
00582
00583 if(useNewton)
00584 {
00585 FCL_REAL l = 0;
00586 FCL_REAL r = 1;
00587
00588 if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i))
00589 {
00590 *collision_time = (l + r) * 0.5;
00591 }
00592 }
00593 else
00594 {
00595 FCL_REAL roots[3];
00596 int num = PolySolver::solveCubic(coeffs, roots);
00597 for(int i = 0; i < num; ++i)
00598 {
00599 FCL_REAL r = roots[i];
00600 if(r < 0 || r > 1) continue;
00601
00602 if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i))
00603 {
00604 *collision_time = r;
00605 break;
00606 }
00607 }
00608 }
00609
00610 if(*collision_time > 1)
00611 {
00612 return false;
00613 }
00614
00615 return true;
00616 }
00617
00618
00619 bool Intersect::intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00620 const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
00621 const Vec3f& L)
00622 {
00623 Vec3f va, vb, vp;
00624 va = a1 - a0;
00625 vb = b1 - b0;
00626 vp = p1 - p0;
00627
00628 FCL_REAL a, b, c;
00629 computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c);
00630
00631 if(isZero(a) && isZero(b) && isZero(c))
00632 return true;
00633
00634 return solveSquare(a, b, c, a0, b0, p0, va, vb, vp);
00635
00636 }
00637
00638
00640 bool Intersect::intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00641 const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1)
00642 {
00643 Vec3f n0 = (b0 - a0).cross(c0 - a0);
00644 Vec3f n1 = (b1 - a1).cross(c1 - a1);
00645 Vec3f a0a1 = a1 - a0;
00646 Vec3f b0b1 = b1 - b0;
00647 Vec3f c0c1 = c1 - c0;
00648 Vec3f delta = (b0b1 - a0a1).cross(c0c1 - a0a1);
00649 Vec3f nx = (n0 + n1 - delta) * 0.5;
00650
00651 Vec3f a0d0 = d0 - a0;
00652 Vec3f a1d1 = d1 - a1;
00653
00654 FCL_REAL A = n0.dot(a0d0);
00655 FCL_REAL B = n1.dot(a1d1);
00656 FCL_REAL C = nx.dot(a0d0);
00657 FCL_REAL D = nx.dot(a1d1);
00658 FCL_REAL E = n1.dot(a0d0);
00659 FCL_REAL F = n0.dot(a1d1);
00660
00661 if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0)
00662 return false;
00663 if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0)
00664 return false;
00665
00666 return true;
00667 }
00668
00669 bool Intersect::intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00670 const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
00671 FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
00672 {
00673 if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1))
00674 {
00675 return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton);
00676 }
00677 else
00678 return false;
00679 }
00680
00681 bool Intersect::intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00682 const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
00683 FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
00684 {
00685 if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1))
00686 {
00687 return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton);
00688 }
00689 else
00690 return false;
00691 }
00692
00693 bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00694 const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00695 const Matrix3f& R, const Vec3f& T,
00696 Vec3f* contact_points,
00697 unsigned int* num_contact_points,
00698 FCL_REAL* penetration_depth,
00699 Vec3f* normal)
00700 {
00701 Vec3f Q1_ = R * Q1 + T;
00702 Vec3f Q2_ = R * Q2 + T;
00703 Vec3f Q3_ = R * Q3 + T;
00704
00705 return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
00706 }
00707
00708 bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00709 const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00710 const Transform3f& tf,
00711 Vec3f* contact_points,
00712 unsigned int* num_contact_points,
00713 FCL_REAL* penetration_depth,
00714 Vec3f* normal)
00715 {
00716 Vec3f Q1_ = tf.transform(Q1);
00717 Vec3f Q2_ = tf.transform(Q2);
00718 Vec3f Q3_ = tf.transform(Q3);
00719
00720 return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
00721 }
00722
00723
00724 #if ODE_STYLE
00725 bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00726 const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00727 Vec3f* contact_points,
00728 unsigned int* num_contact_points,
00729 FCL_REAL* penetration_depth,
00730 Vec3f* normal)
00731 {
00732
00733
00734 Vec3f n1;
00735 FCL_REAL t1;
00736 bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1);
00737 if(!b1) return false;
00738
00739 Vec3f n2;
00740 FCL_REAL t2;
00741 bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);
00742 if(!b2) return false;
00743
00744 if(sameSideOfPlane(P1, P2, P3, n2, t2))
00745 return false;
00746
00747 if(sameSideOfPlane(Q1, Q2, Q3, n1, t1))
00748 return false;
00749
00750 Vec3f clipped_points1[MAX_TRIANGLE_CLIPS];
00751 unsigned int num_clipped_points1 = 0;
00752 Vec3f clipped_points2[MAX_TRIANGLE_CLIPS];
00753 unsigned int num_clipped_points2 = 0;
00754
00755 Vec3f deepest_points1[MAX_TRIANGLE_CLIPS];
00756 unsigned int num_deepest_points1 = 0;
00757 Vec3f deepest_points2[MAX_TRIANGLE_CLIPS];
00758 unsigned int num_deepest_points2 = 0;
00759 FCL_REAL penetration_depth1 = -1, penetration_depth2 = -1;
00760
00761 clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2);
00762
00763 if(num_clipped_points2 == 0)
00764 return false;
00765
00766 computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2);
00767 if(num_deepest_points2 == 0)
00768 return false;
00769
00770 clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1);
00771 if(num_clipped_points1 == 0)
00772 return false;
00773
00774 computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1);
00775 if(num_deepest_points1 == 0)
00776 return false;
00777
00778
00780 if(contact_points && num_contact_points && penetration_depth && normal)
00781 {
00782 if(penetration_depth1 > penetration_depth2)
00783 {
00784 *num_contact_points = num_deepest_points2;
00785 for(unsigned int i = 0; i < num_deepest_points2; ++i)
00786 {
00787 contact_points[i] = deepest_points2[i];
00788 }
00789
00790 *normal = -n1;
00791 *penetration_depth = penetration_depth2;
00792 }
00793 else
00794 {
00795 *num_contact_points = num_deepest_points1;
00796 for(unsigned int i = 0; i < num_deepest_points1; ++i)
00797 {
00798 contact_points[i] = deepest_points1[i];
00799 }
00800
00801 *normal = n2;
00802 *penetration_depth = penetration_depth1;
00803 }
00804 }
00805
00806 return true;
00807 }
00808 #else
00809 bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00810 const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00811 Vec3f* contact_points,
00812 unsigned int* num_contact_points,
00813 FCL_REAL* penetration_depth,
00814 Vec3f* normal)
00815 {
00816 Vec3f p1 = P1 - P1;
00817 Vec3f p2 = P2 - P1;
00818 Vec3f p3 = P3 - P1;
00819 Vec3f q1 = Q1 - P1;
00820 Vec3f q2 = Q2 - P1;
00821 Vec3f q3 = Q3 - P1;
00822
00823 Vec3f e1 = p2 - p1;
00824 Vec3f e2 = p3 - p2;
00825 Vec3f n1 = e1.cross(e2);
00826 if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false;
00827
00828 Vec3f f1 = q2 - q1;
00829 Vec3f f2 = q3 - q2;
00830 Vec3f m1 = f1.cross(f2);
00831 if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false;
00832
00833 Vec3f ef11 = e1.cross(f1);
00834 if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false;
00835
00836 Vec3f ef12 = e1.cross(f2);
00837 if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false;
00838
00839 Vec3f f3 = q1 - q3;
00840 Vec3f ef13 = e1.cross(f3);
00841 if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false;
00842
00843 Vec3f ef21 = e2.cross(f1);
00844 if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false;
00845
00846 Vec3f ef22 = e2.cross(f2);
00847 if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false;
00848
00849 Vec3f ef23 = e2.cross(f3);
00850 if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false;
00851
00852 Vec3f e3 = p1 - p3;
00853 Vec3f ef31 = e3.cross(f1);
00854 if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false;
00855
00856 Vec3f ef32 = e3.cross(f2);
00857 if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false;
00858
00859 Vec3f ef33 = e3.cross(f3);
00860 if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false;
00861
00862 Vec3f g1 = e1.cross(n1);
00863 if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false;
00864
00865 Vec3f g2 = e2.cross(n1);
00866 if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false;
00867
00868 Vec3f g3 = e3.cross(n1);
00869 if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false;
00870
00871 Vec3f h1 = f1.cross(m1);
00872 if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false;
00873
00874 Vec3f h2 = f2.cross(m1);
00875 if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false;
00876
00877 Vec3f h3 = f3.cross(m1);
00878 if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false;
00879
00880 if(contact_points && num_contact_points && penetration_depth && normal)
00881 {
00882 Vec3f n1, n2;
00883 FCL_REAL t1, t2;
00884 buildTrianglePlane(P1, P2, P3, &n1, &t1);
00885 buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);
00886
00887 Vec3f deepest_points1[3];
00888 unsigned int num_deepest_points1 = 0;
00889 Vec3f deepest_points2[3];
00890 unsigned int num_deepest_points2 = 0;
00891 FCL_REAL penetration_depth1, penetration_depth2;
00892
00893 Vec3f P[3] = {P1, P2, P3};
00894 Vec3f Q[3] = {Q1, Q2, Q3};
00895
00896 computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2);
00897 computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1);
00898
00899
00900 if(penetration_depth1 > penetration_depth2)
00901 {
00902 *num_contact_points = std::min(num_deepest_points2, (unsigned int)2);
00903 for(unsigned int i = 0; i < num_deepest_points2; ++i)
00904 {
00905 contact_points[i] = deepest_points2[i];
00906 }
00907
00908 *normal = -n1;
00909 *penetration_depth = penetration_depth2;
00910 }
00911 else
00912 {
00913 *num_contact_points = std::min(num_deepest_points1, (unsigned int)2);
00914 for(unsigned int i = 0; i < num_deepest_points1; ++i)
00915 {
00916 contact_points[i] = deepest_points1[i];
00917 }
00918
00919 *normal = n2;
00920 *penetration_depth = penetration_depth1;
00921 }
00922 }
00923
00924 return true;
00925 }
00926 #endif
00927
00928
00929 void Intersect::computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, FCL_REAL t, FCL_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points)
00930 {
00931 *num_deepest_points = 0;
00932 FCL_REAL max_depth = -std::numeric_limits<FCL_REAL>::max();
00933 unsigned int num_deepest_points_ = 0;
00934 unsigned int num_neg = 0;
00935 unsigned int num_pos = 0;
00936 unsigned int num_zero = 0;
00937
00938 for(unsigned int i = 0; i < num_clipped_points; ++i)
00939 {
00940 FCL_REAL dist = -distanceToPlane(n, t, clipped_points[i]);
00941 if(dist > EPSILON) num_pos++;
00942 else if(dist < -EPSILON) num_neg++;
00943 else num_zero++;
00944 if(dist > max_depth)
00945 {
00946 max_depth = dist;
00947 num_deepest_points_ = 1;
00948 deepest_points[num_deepest_points_ - 1] = clipped_points[i];
00949 }
00950 else if(dist + 1e-6 >= max_depth)
00951 {
00952 num_deepest_points_++;
00953 deepest_points[num_deepest_points_ - 1] = clipped_points[i];
00954 }
00955 }
00956
00957 if(max_depth < -EPSILON)
00958 num_deepest_points_ = 0;
00959
00960 if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0)))
00961 num_deepest_points_ = 0;
00962
00963 *penetration_depth = max_depth;
00964 *num_deepest_points = num_deepest_points_;
00965 }
00966
00967 void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
00968 const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
00969 const Vec3f& tn, FCL_REAL to,
00970 Vec3f clipped_points[], unsigned int* num_clipped_points,
00971 bool clip_triangle)
00972 {
00973 *num_clipped_points = 0;
00974 Vec3f temp_clip[MAX_TRIANGLE_CLIPS];
00975 Vec3f temp_clip2[MAX_TRIANGLE_CLIPS];
00976 unsigned int num_temp_clip = 0;
00977 unsigned int num_temp_clip2 = 0;
00978 Vec3f v[3] = {v1, v2, v3};
00979
00980 Vec3f plane_n;
00981 FCL_REAL plane_dist;
00982
00983 if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist))
00984 {
00985 clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip);
00986 if(num_temp_clip > 0)
00987 {
00988 if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist))
00989 {
00990 clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2);
00991 if(num_temp_clip2 > 0)
00992 {
00993 if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist))
00994 {
00995 if(clip_triangle)
00996 {
00997 num_temp_clip = 0;
00998 clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip);
00999 if(num_temp_clip > 0)
01000 {
01001 clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points);
01002 }
01003 }
01004 else
01005 {
01006 clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points);
01007 }
01008 }
01009 }
01010 }
01011 }
01012 }
01013 }
01014
01015 void Intersect::clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, FCL_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points)
01016 {
01017 *num_clipped_points = 0;
01018
01019 unsigned int num_clipped_points_ = 0;
01020 unsigned int vi;
01021 unsigned int prev_classify = 2;
01022 unsigned int classify;
01023 for(unsigned int i = 0; i <= num_polygon_points; ++i)
01024 {
01025 vi = (i % num_polygon_points);
01026 FCL_REAL d = distanceToPlane(n, t, polygon_points[i]);
01027 classify = ((d > EPSILON) ? 1 : 0);
01028 if(classify == 0)
01029 {
01030 if(prev_classify == 1)
01031 {
01032 if(num_clipped_points_ < MAX_TRIANGLE_CLIPS)
01033 {
01034 Vec3f tmp;
01035 clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp);
01036 if(num_clipped_points_ > 0)
01037 {
01038 if((tmp - clipped_points[num_clipped_points_ - 1]).sqrLength() > EPSILON)
01039 {
01040 clipped_points[num_clipped_points_] = tmp;
01041 num_clipped_points_++;
01042 }
01043 }
01044 else
01045 {
01046 clipped_points[num_clipped_points_] = tmp;
01047 num_clipped_points_++;
01048 }
01049 }
01050 }
01051
01052 if(num_clipped_points_ < MAX_TRIANGLE_CLIPS && i < num_polygon_points)
01053 {
01054 clipped_points[num_clipped_points_] = polygon_points[vi];
01055 num_clipped_points_++;
01056 }
01057 }
01058 else
01059 {
01060 if(prev_classify == 0)
01061 {
01062 if(num_clipped_points_ < MAX_TRIANGLE_CLIPS)
01063 {
01064 Vec3f tmp;
01065 clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp);
01066 if(num_clipped_points_ > 0)
01067 {
01068 if((tmp - clipped_points[num_clipped_points_ - 1]).sqrLength() > EPSILON)
01069 {
01070 clipped_points[num_clipped_points_] = tmp;
01071 num_clipped_points_++;
01072 }
01073 }
01074 else
01075 {
01076 clipped_points[num_clipped_points_] = tmp;
01077 num_clipped_points_++;
01078 }
01079 }
01080 }
01081 }
01082
01083 prev_classify = classify;
01084 }
01085
01086 if(num_clipped_points_ > 2)
01087 {
01088 if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).sqrLength() < EPSILON)
01089 {
01090 num_clipped_points_--;
01091 }
01092 }
01093
01094 *num_clipped_points = num_clipped_points_;
01095 }
01096
01097 void Intersect::clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point)
01098 {
01099 FCL_REAL dist1 = distanceToPlane(n, t, v1);
01100 Vec3f tmp = v2 - v1;
01101 FCL_REAL dist2 = tmp.dot(n);
01102 *clipped_point = tmp * (-dist1 / dist2) + v1;
01103 }
01104
01105 FCL_REAL Intersect::distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v)
01106 {
01107 return n.dot(v) - t;
01108 }
01109
01110 bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t)
01111 {
01112 Vec3f n_ = (v2 - v1).cross(v3 - v1);
01113 bool can_normalize = false;
01114 n_.normalize(&can_normalize);
01115 if(can_normalize)
01116 {
01117 *n = n_;
01118 *t = n_.dot(v1);
01119 return true;
01120 }
01121
01122 return false;
01123 }
01124
01125 bool Intersect::buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t)
01126 {
01127 Vec3f n_ = (v2 - v1).cross(tn);
01128 bool can_normalize = false;
01129 n_.normalize(&can_normalize);
01130 if(can_normalize)
01131 {
01132 *n = n_;
01133 *t = n_.dot(v1);
01134 return true;
01135 }
01136
01137 return false;
01138 }
01139
01140 bool Intersect::sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t)
01141 {
01142 FCL_REAL dist1 = distanceToPlane(n, t, v1);
01143 FCL_REAL dist2 = dist1 * distanceToPlane(n, t, v2);
01144 FCL_REAL dist3 = dist1 * distanceToPlane(n, t, v3);
01145 if((dist2 > 0) && (dist3 > 0))
01146 return true;
01147 return false;
01148 }
01149
01150 int Intersect::project6(const Vec3f& ax,
01151 const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
01152 const Vec3f& q1, const Vec3f& q2, const Vec3f& q3)
01153 {
01154 FCL_REAL P1 = ax.dot(p1);
01155 FCL_REAL P2 = ax.dot(p2);
01156 FCL_REAL P3 = ax.dot(p3);
01157 FCL_REAL Q1 = ax.dot(q1);
01158 FCL_REAL Q2 = ax.dot(q2);
01159 FCL_REAL Q3 = ax.dot(q3);
01160
01161 FCL_REAL mn1 = std::min(P1, std::min(P2, P3));
01162 FCL_REAL mx2 = std::max(Q1, std::max(Q2, Q3));
01163 if(mn1 > mx2) return 0;
01164
01165 FCL_REAL mx1 = std::max(P1, std::max(P2, P3));
01166 FCL_REAL mn2 = std::min(Q1, std::min(Q2, Q3));
01167
01168 if(mn2 > mx1) return 0;
01169 return 1;
01170 }
01171
01172
01173
01174 void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
01175 Vec3f& VEC, Vec3f& X, Vec3f& Y)
01176 {
01177 Vec3f T;
01178 FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
01179 Vec3f TMP;
01180
01181 T = Q - P;
01182 A_dot_A = A.dot(A);
01183 B_dot_B = B.dot(B);
01184 A_dot_B = A.dot(B);
01185 A_dot_T = A.dot(T);
01186 B_dot_T = B.dot(T);
01187
01188
01189
01190
01191 FCL_REAL t, u;
01192
01193
01194
01195
01196 FCL_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;
01197
01198 t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;
01199
01200
01201
01202 if((t < 0) || boost::math::isnan(t)) t = 0; else if(t > 1) t = 1;
01203
01204
01205
01206 u = (t*A_dot_B - B_dot_T) / B_dot_B;
01207
01208
01209
01210
01211
01212 if((u <= 0) || boost::math::isnan(u))
01213 {
01214 Y = Q;
01215
01216 t = A_dot_T / A_dot_A;
01217
01218 if((t <= 0) || boost::math::isnan(t))
01219 {
01220 X = P;
01221 VEC = Q - P;
01222 }
01223 else if(t >= 1)
01224 {
01225 X = P + A;
01226 VEC = Q - X;
01227 }
01228 else
01229 {
01230 X = P + A * t;
01231 TMP = T.cross(A);
01232 VEC = A.cross(TMP);
01233 }
01234 }
01235 else if (u >= 1)
01236 {
01237 Y = Q + B;
01238
01239 t = (A_dot_B + A_dot_T) / A_dot_A;
01240
01241 if((t <= 0) || boost::math::isnan(t))
01242 {
01243 X = P;
01244 VEC = Y - P;
01245 }
01246 else if(t >= 1)
01247 {
01248 X = P + A;
01249 VEC = Y - X;
01250 }
01251 else
01252 {
01253 X = P + A * t;
01254 T = Y - P;
01255 TMP = T.cross(A);
01256 VEC= A.cross(TMP);
01257 }
01258 }
01259 else
01260 {
01261 Y = Q + B * u;
01262
01263 if((t <= 0) || boost::math::isnan(t))
01264 {
01265 X = P;
01266 TMP = T.cross(B);
01267 VEC = B.cross(TMP);
01268 }
01269 else if(t >= 1)
01270 {
01271 X = P + A;
01272 T = Q - X;
01273 TMP = T.cross(B);
01274 VEC = B.cross(TMP);
01275 }
01276 else
01277 {
01278 X = P + A * t;
01279 VEC = A.cross(B);
01280 if(VEC.dot(T) < 0)
01281 {
01282 VEC = VEC * (-1);
01283 }
01284 }
01285 }
01286 }
01287
01288
01289 FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q)
01290 {
01291
01292
01293 Vec3f Sv[3];
01294 Vec3f Tv[3];
01295 Vec3f VEC;
01296
01297 Sv[0] = S[1] - S[0];
01298 Sv[1] = S[2] - S[1];
01299 Sv[2] = S[0] - S[2];
01300
01301 Tv[0] = T[1] - T[0];
01302 Tv[1] = T[2] - T[1];
01303 Tv[2] = T[0] - T[2];
01304
01305
01306
01307
01308
01309
01310
01311
01312
01313 Vec3f V, Z, minP, minQ;
01314 FCL_REAL mindd;
01315 int shown_disjoint = 0;
01316
01317 mindd = (S[0] - T[0]).sqrLength() + 1;
01318
01319 for(int i = 0; i < 3; ++i)
01320 {
01321 for(int j = 0; j < 3; ++j)
01322 {
01323
01324
01325 segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q);
01326
01327 V = Q - P;
01328 FCL_REAL dd = V.dot(V);
01329
01330
01331
01332
01333 if(dd <= mindd)
01334 {
01335 minP = P;
01336 minQ = Q;
01337 mindd = dd;
01338
01339 Z = S[(i+2)%3] - P;
01340 FCL_REAL a = Z.dot(VEC);
01341 Z = T[(j+2)%3] - Q;
01342 FCL_REAL b = Z.dot(VEC);
01343
01344 if((a <= 0) && (b >= 0)) return sqrt(dd);
01345
01346 FCL_REAL p = V.dot(VEC);
01347
01348 if(a < 0) a = 0;
01349 if(b > 0) b = 0;
01350 if((p - a + b) > 0) shown_disjoint = 1;
01351 }
01352 }
01353 }
01354
01355
01356
01357
01358
01359
01360
01361
01362
01363
01364
01365
01366
01367
01368
01369
01370
01371 Vec3f Sn;
01372 FCL_REAL Snl;
01373
01374 Sn = Sv[0].cross(Sv[1]);
01375 Snl = Sn.dot(Sn);
01376
01377
01378
01379 if(Snl > 1e-15)
01380 {
01381
01382
01383 Vec3f Tp;
01384
01385 V = S[0] - T[0];
01386 Tp[0] = V.dot(Sn);
01387
01388 V = S[0] - T[1];
01389 Tp[1] = V.dot(Sn);
01390
01391 V = S[0] - T[2];
01392 Tp[2] = V.dot(Sn);
01393
01394
01395
01396
01397 int point = -1;
01398 if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0))
01399 {
01400 if(Tp[0] < Tp[1]) point = 0; else point = 1;
01401 if(Tp[2] < Tp[point]) point = 2;
01402 }
01403 else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0))
01404 {
01405 if(Tp[0] > Tp[1]) point = 0; else point = 1;
01406 if(Tp[2] > Tp[point]) point = 2;
01407 }
01408
01409
01410
01411 if(point >= 0)
01412 {
01413 shown_disjoint = 1;
01414
01415
01416
01417
01418 V = T[point] - S[0];
01419 Z = Sn.cross(Sv[0]);
01420 if(V.dot(Z) > 0)
01421 {
01422 V = T[point] - S[1];
01423 Z = Sn.cross(Sv[1]);
01424 if(V.dot(Z) > 0)
01425 {
01426 V = T[point] - S[2];
01427 Z = Sn.cross(Sv[2]);
01428 if(V.dot(Z) > 0)
01429 {
01430
01431
01432 P = T[point] + Sn * (Tp[point] / Snl);
01433 Q = T[point];
01434 return (P - Q).length();
01435 }
01436 }
01437 }
01438 }
01439 }
01440
01441 Vec3f Tn;
01442 FCL_REAL Tnl;
01443
01444 Tn = Tv[0].cross(Tv[1]);
01445 Tnl = Tn.dot(Tn);
01446
01447 if(Tnl > 1e-15)
01448 {
01449 Vec3f Sp;
01450
01451 V = T[0] - S[0];
01452 Sp[0] = V.dot(Tn);
01453
01454 V = T[0] - S[1];
01455 Sp[1] = V.dot(Tn);
01456
01457 V = T[0] - S[2];
01458 Sp[2] = V.dot(Tn);
01459
01460 int point = -1;
01461 if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0))
01462 {
01463 if(Sp[0] < Sp[1]) point = 0; else point = 1;
01464 if(Sp[2] < Sp[point]) point = 2;
01465 }
01466 else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0))
01467 {
01468 if(Sp[0] > Sp[1]) point = 0; else point = 1;
01469 if(Sp[2] > Sp[point]) point = 2;
01470 }
01471
01472 if(point >= 0)
01473 {
01474 shown_disjoint = 1;
01475
01476 V = S[point] - T[0];
01477 Z = Tn.cross(Tv[0]);
01478 if(V.dot(Z) > 0)
01479 {
01480 V = S[point] - T[1];
01481 Z = Tn.cross(Tv[1]);
01482 if(V.dot(Z) > 0)
01483 {
01484 V = S[point] - T[2];
01485 Z = Tn.cross(Tv[2]);
01486 if(V.dot(Z) > 0)
01487 {
01488 P = S[point];
01489 Q = S[point] + Tn * (Sp[point] / Tnl);
01490 return (P - Q).length();
01491 }
01492 }
01493 }
01494 }
01495 }
01496
01497
01498
01499
01500
01501
01502 if(shown_disjoint)
01503 {
01504 P = minP;
01505 Q = minQ;
01506 return sqrt(mindd);
01507 }
01508 else return 0;
01509 }
01510
01511
01512 FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
01513 const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
01514 Vec3f& P, Vec3f& Q)
01515 {
01516 Vec3f S[3];
01517 Vec3f T[3];
01518 S[0] = S1; S[1] = S2; S[2] = S3;
01519 T[0] = T1; T[1] = T2; T[2] = T3;
01520
01521 return triDistance(S, T, P, Q);
01522 }
01523
01524 FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
01525 const Matrix3f& R, const Vec3f& Tl,
01526 Vec3f& P, Vec3f& Q)
01527 {
01528 Vec3f T_transformed[3];
01529 T_transformed[0] = R * T[0] + Tl;
01530 T_transformed[1] = R * T[1] + Tl;
01531 T_transformed[2] = R * T[2] + Tl;
01532
01533 return triDistance(S, T_transformed, P, Q);
01534 }
01535
01536 FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
01537 const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
01538 const Matrix3f& R, const Vec3f& Tl,
01539 Vec3f& P, Vec3f& Q)
01540 {
01541 Vec3f T1_transformed = R * T1 + Tl;
01542 Vec3f T2_transformed = R * T2 + Tl;
01543 Vec3f T3_transformed = R * T3 + Tl;
01544 return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
01545 }
01546
01547 }