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