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/narrowphase/narrowphase.h"
00038 #include <boost/math/constants/constants.hpp>
00039 #include <vector>
00040
00041 namespace fcl
00042 {
00043
00044 namespace details
00045 {
00046
00047 bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
00048 const Sphere& s2, const Transform3f& tf2,
00049 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
00050 {
00051 Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
00052 FCL_REAL len = diff.length();
00053 if(len > s1.radius + s2.radius)
00054 return false;
00055
00056 if(penetration_depth)
00057 *penetration_depth = s1.radius + s2.radius - len;
00058 if(normal)
00059 {
00060 if(len > 0)
00061 *normal = diff / len;
00062 else
00063 *normal = diff;
00064 }
00065
00066 if(contact_points)
00067 *contact_points = tf1.transform(Vec3f()) + diff * 0.5;
00068
00069 return true;
00070 }
00071
00072
00073 bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
00074 const Sphere& s2, const Transform3f& tf2,
00075 FCL_REAL* dist)
00076 {
00077 Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
00078 FCL_REAL len = diff.length();
00079 if(len > s1.radius + s2.radius)
00080 {
00081 *dist = len - (s1.radius + s2.radius);
00082 return true;
00083 }
00084
00085 *dist = -1;
00086 return false;
00087 }
00088
00090 FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to,const Vec3f& p, Vec3f& nearest)
00091 {
00092 Vec3f diff = p - from;
00093 Vec3f v = to - from;
00094 FCL_REAL t = v.dot(diff);
00095
00096 if(t > 0)
00097 {
00098 FCL_REAL dotVV = v.dot(v);
00099 if(t < dotVV)
00100 {
00101 t /= dotVV;
00102 diff -= v * t;
00103 }
00104 else
00105 {
00106 t = 1;
00107 diff -= v;
00108 }
00109 }
00110 else
00111 t = 0;
00112
00113 nearest = from + v * t;
00114 return diff.dot(diff);
00115 }
00116
00118 bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& normal, const Vec3f& p)
00119 {
00120 Vec3f edge1(p2 - p1);
00121 Vec3f edge2(p3 - p2);
00122 Vec3f edge3(p1 - p3);
00123
00124 Vec3f p1_to_p(p - p1);
00125 Vec3f p2_to_p(p - p2);
00126 Vec3f p3_to_p(p - p3);
00127
00128 Vec3f edge1_normal(edge1.cross(normal));
00129 Vec3f edge2_normal(edge2.cross(normal));
00130 Vec3f edge3_normal(edge3.cross(normal));
00131
00132 FCL_REAL r1, r2, r3;
00133 r1 = edge1_normal.dot(p1_to_p);
00134 r2 = edge2_normal.dot(p2_to_p);
00135 r3 = edge3_normal.dot(p3_to_p);
00136 if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
00137 ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
00138 return true;
00139 return false;
00140 }
00141
00142
00143 bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
00144 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
00145 {
00146 Vec3f normal = (P2 - P1).cross(P3 - P1);
00147 normal.normalize();
00148 const Vec3f& center = tf.getTranslation();
00149 const FCL_REAL& radius = s.radius;
00150 FCL_REAL radius_with_threshold = radius + std::numeric_limits<FCL_REAL>::epsilon();
00151 Vec3f p1_to_center = center - P1;
00152 FCL_REAL distance_from_plane = p1_to_center.dot(normal);
00153
00154 if(distance_from_plane < 0)
00155 {
00156 distance_from_plane *= -1;
00157 normal *= -1;
00158 }
00159
00160 bool is_inside_contact_plane = (distance_from_plane < radius_with_threshold);
00161
00162 bool has_contact = false;
00163 Vec3f contact_point;
00164 if(is_inside_contact_plane)
00165 {
00166 if(projectInTriangle(P1, P2, P3, center, normal))
00167 {
00168 has_contact = true;
00169 contact_point = center - normal * distance_from_plane;
00170 }
00171 else
00172 {
00173 FCL_REAL contact_capsule_radius_sqr = radius_with_threshold * radius_with_threshold;
00174 Vec3f nearest_on_edge;
00175 FCL_REAL distance_sqr;
00176 distance_sqr = segmentSqrDistance(P1, P2, center, nearest_on_edge);
00177 if(distance_sqr < contact_capsule_radius_sqr)
00178 {
00179 has_contact = true;
00180 contact_point = nearest_on_edge;
00181 }
00182
00183 distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
00184 if(distance_sqr < contact_capsule_radius_sqr)
00185 {
00186 has_contact = true;
00187 contact_point = nearest_on_edge;
00188 }
00189
00190 distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge);
00191 if(distance_sqr < contact_capsule_radius_sqr)
00192 {
00193 has_contact = true;
00194 contact_point = nearest_on_edge;
00195 }
00196 }
00197 }
00198
00199 if(has_contact)
00200 {
00201 Vec3f contact_to_center = center - contact_point;
00202 FCL_REAL distance_sqr = contact_to_center.sqrLength();
00203
00204 if(distance_sqr < radius_with_threshold * radius_with_threshold)
00205 {
00206 if(distance_sqr > 0)
00207 {
00208 FCL_REAL distance = std::sqrt(distance_sqr);
00209 if(normal_) *normal_ = normalize(contact_to_center);
00210 if(contact_points) *contact_points = contact_point;
00211 if(penetration_depth) *penetration_depth = -(radius - distance);
00212 }
00213 else
00214 {
00215 FCL_REAL distance = 0;
00216 if(normal_) *normal_ = normal;
00217 if(contact_points) *contact_points = contact_point;
00218 if(penetration_depth) *penetration_depth = -radius;
00219 }
00220
00221 return true;
00222 }
00223 }
00224
00225 return false;
00226 }
00227
00228 bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
00229 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00230 FCL_REAL* dist)
00231 {
00232
00233
00234 const Vec3f& center = tf.getTranslation();
00235 FCL_REAL radius = sp.radius;
00236 Vec3f diff = P1 - center;
00237 Vec3f edge0 = P2 - P1;
00238 Vec3f edge1 = P3 - P1;
00239 FCL_REAL a00 = edge0.sqrLength();
00240 FCL_REAL a01 = edge0.dot(edge1);
00241 FCL_REAL a11 = edge1.sqrLength();
00242 FCL_REAL b0 = diff.dot(edge0);
00243 FCL_REAL b1 = diff.dot(edge1);
00244 FCL_REAL c = diff.sqrLength();
00245 FCL_REAL det = fabs(a00*a11 - a01*a01);
00246 FCL_REAL s = a01*b1 - a11*b0;
00247 FCL_REAL t = a01*b0 - a00*b1;
00248
00249 FCL_REAL sqr_dist;
00250
00251 if(s + t <= det)
00252 {
00253 if(s < 0)
00254 {
00255 if(t < 0)
00256 {
00257 if(b0 < 0)
00258 {
00259 t = 0;
00260 if(-b0 >= a00)
00261 {
00262 s = 1;
00263 sqr_dist = a00 + 2*b0 + c;
00264 }
00265 else
00266 {
00267 s = -b0/a00;
00268 sqr_dist = b0*s + c;
00269 }
00270 }
00271 else
00272 {
00273 s = 0;
00274 if(b1 >= 0)
00275 {
00276 t = 0;
00277 sqr_dist = c;
00278 }
00279 else if(-b1 >= a11)
00280 {
00281 t = 1;
00282 sqr_dist = a11 + 2*b1 + c;
00283 }
00284 else
00285 {
00286 t = -b1/a11;
00287 sqr_dist = b1*t + c;
00288 }
00289 }
00290 }
00291 else
00292 {
00293 s = 0;
00294 if(b1 >= 0)
00295 {
00296 t = 0;
00297 sqr_dist = c;
00298 }
00299 else if(-b1 >= a11)
00300 {
00301 t = 1;
00302 sqr_dist = a11 + 2*b1 + c;
00303 }
00304 else
00305 {
00306 t = -b1/a11;
00307 sqr_dist = b1*t + c;
00308 }
00309 }
00310 }
00311 else if(t < 0)
00312 {
00313 t = 0;
00314 if(b0 >= 0)
00315 {
00316 s = 0;
00317 sqr_dist = c;
00318 }
00319 else if(-b0 >= a00)
00320 {
00321 s = 1;
00322 sqr_dist = a00 + 2*b0 + c;
00323 }
00324 else
00325 {
00326 s = -b0/a00;
00327 sqr_dist = b0*s + c;
00328 }
00329 }
00330 else
00331 {
00332
00333 FCL_REAL inv_det = (1)/det;
00334 s *= inv_det;
00335 t *= inv_det;
00336 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
00337 }
00338 }
00339 else
00340 {
00341 FCL_REAL tmp0, tmp1, numer, denom;
00342
00343 if(s < 0)
00344 {
00345 tmp0 = a01 + b0;
00346 tmp1 = a11 + b1;
00347 if(tmp1 > tmp0)
00348 {
00349 numer = tmp1 - tmp0;
00350 denom = a00 - 2*a01 + a11;
00351 if(numer >= denom)
00352 {
00353 s = 1;
00354 t = 0;
00355 sqr_dist = a00 + 2*b0 + c;
00356 }
00357 else
00358 {
00359 s = numer/denom;
00360 t = 1 - s;
00361 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
00362 }
00363 }
00364 else
00365 {
00366 s = 0;
00367 if(tmp1 <= 0)
00368 {
00369 t = 1;
00370 sqr_dist = a11 + 2*b1 + c;
00371 }
00372 else if(b1 >= 0)
00373 {
00374 t = 0;
00375 sqr_dist = c;
00376 }
00377 else
00378 {
00379 t = -b1/a11;
00380 sqr_dist = b1*t + c;
00381 }
00382 }
00383 }
00384 else if(t < 0)
00385 {
00386 tmp0 = a01 + b1;
00387 tmp1 = a00 + b0;
00388 if(tmp1 > tmp0)
00389 {
00390 numer = tmp1 - tmp0;
00391 denom = a00 - 2*a01 + a11;
00392 if(numer >= denom)
00393 {
00394 t = 1;
00395 s = 0;
00396 sqr_dist = a11 + 2*b1 + c;
00397 }
00398 else
00399 {
00400 t = numer/denom;
00401 s = 1 - t;
00402 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
00403 }
00404 }
00405 else
00406 {
00407 t = 0;
00408 if(tmp1 <= 0)
00409 {
00410 s = 1;
00411 sqr_dist = a00 + 2*b0 + c;
00412 }
00413 else if(b0 >= 0)
00414 {
00415 s = 0;
00416 sqr_dist = c;
00417 }
00418 else
00419 {
00420 s = -b0/a00;
00421 sqr_dist = b0*s + c;
00422 }
00423 }
00424 }
00425 else
00426 {
00427 numer = a11 + b1 - a01 - b0;
00428 if(numer <= 0)
00429 {
00430 s = 0;
00431 t = 1;
00432 sqr_dist = a11 + 2*b1 + c;
00433 }
00434 else
00435 {
00436 denom = a00 - 2*a01 + a11;
00437 if(numer >= denom)
00438 {
00439 s = 1;
00440 t = 0;
00441 sqr_dist = a00 + 2*b0 + c;
00442 }
00443 else
00444 {
00445 s = numer/denom;
00446 t = 1 - s;
00447 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
00448 }
00449 }
00450 }
00451 }
00452
00453
00454 if(sqr_dist < 0)
00455 sqr_dist = 0;
00456
00457 if(sqr_dist > radius * radius)
00458 {
00459 *dist = std::sqrt(sqr_dist) - radius;
00460 return true;
00461 }
00462 else
00463 {
00464 *dist = -1;
00465 return false;
00466 }
00467 }
00468
00469
00470
00471 struct ContactPoint
00472 {
00473 Vec3f normal;
00474 Vec3f point;
00475 FCL_REAL depth;
00476 ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d) : normal(n), point(p), depth(d) {}
00477 };
00478
00479
00480 static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua,
00481 const Vec3f& pb, const Vec3f& ub,
00482 FCL_REAL* alpha, FCL_REAL* beta)
00483 {
00484 Vec3f p = pb - pa;
00485 FCL_REAL uaub = ua.dot(ub);
00486 FCL_REAL q1 = ua.dot(p);
00487 FCL_REAL q2 = -ub.dot(p);
00488 FCL_REAL d = 1 - uaub * uaub;
00489 if(d <= (FCL_REAL)(0.0001f))
00490 {
00491 *alpha = 0;
00492 *beta = 0;
00493 }
00494 else
00495 {
00496 d = 1 / d;
00497 *alpha = (q1 + uaub * q2) * d;
00498 *beta = (uaub * q1 + q2) * d;
00499 }
00500 }
00501
00502
00503
00504
00505
00506
00507
00508
00509 static int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16])
00510 {
00511
00512
00513 int nq = 4, nr = 0;
00514 FCL_REAL buffer[16];
00515 FCL_REAL* q = p;
00516 FCL_REAL* r = ret;
00517 for(int dir = 0; dir <= 1; ++dir)
00518 {
00519
00520 for(int sign = -1; sign <= 1; sign += 2)
00521 {
00522
00523 FCL_REAL* pq = q;
00524 FCL_REAL* pr = r;
00525 nr = 0;
00526 for(int i = nq; i > 0; --i)
00527 {
00528
00529 if(sign * pq[dir] < h[dir])
00530 {
00531
00532 pr[0] = pq[0];
00533 pr[1] = pq[1];
00534 pr += 2;
00535 nr++;
00536 if(nr & 8)
00537 {
00538 q = r;
00539 goto done;
00540 }
00541 }
00542 FCL_REAL* nextq = (i > 1) ? pq+2 : q;
00543 if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir]))
00544 {
00545
00546 pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
00547 (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
00548 pr[dir] = sign*h[dir];
00549 pr += 2;
00550 nr++;
00551 if(nr & 8)
00552 {
00553 q = r;
00554 goto done;
00555 }
00556 }
00557 pq += 2;
00558 }
00559 q = r;
00560 r = (q == ret) ? buffer : ret;
00561 nq = nr;
00562 }
00563 }
00564
00565 done:
00566 if(q != ret) memcpy(ret, q, nr*2*sizeof(FCL_REAL));
00567 return nr;
00568 }
00569
00570
00571
00572
00573
00574
00575
00576
00577 static inline void cullPoints2(int n, FCL_REAL p[], int m, int i0, int iret[])
00578 {
00579
00580 FCL_REAL a, cx, cy, q;
00581 switch(n)
00582 {
00583 case 1:
00584 cx = p[0];
00585 cy = p[1];
00586 break;
00587 case 2:
00588 cx = 0.5 * (p[0] + p[2]);
00589 cy = 0.5 * (p[1] + p[3]);
00590 break;
00591 default:
00592 a = 0;
00593 cx = 0;
00594 cy = 0;
00595 for(int i = 0; i < n-1; ++i)
00596 {
00597 q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
00598 a += q;
00599 cx += q*(p[i*2]+p[i*2+2]);
00600 cy += q*(p[i*2+1]+p[i*2+3]);
00601 }
00602 q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
00603 if(std::abs(a+q) > std::numeric_limits<FCL_REAL>::epsilon())
00604 a = 1/(3*(a+q));
00605 else
00606 a= 1e18f;
00607
00608 cx = a*(cx + q*(p[n*2-2]+p[0]));
00609 cy = a*(cy + q*(p[n*2-1]+p[1]));
00610 }
00611
00612
00613
00614 FCL_REAL A[8];
00615 for(int i = 0; i < n; ++i)
00616 A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx);
00617
00618
00619 int avail[8];
00620 for(int i = 0; i < n; ++i) avail[i] = 1;
00621 avail[i0] = 0;
00622 iret[0] = i0;
00623 iret++;
00624 const double pi = boost::math::constants::pi<FCL_REAL>();
00625 for(int j = 1; j < m; ++j)
00626 {
00627 a = j*(2*pi/m) + A[i0];
00628 if (a > pi) a -= 2*pi;
00629 FCL_REAL maxdiff= 1e9, diff;
00630
00631 *iret = i0;
00632 for(int i = 0; i < n; ++i)
00633 {
00634 if(avail[i])
00635 {
00636 diff = std::abs(A[i]-a);
00637 if(diff > pi) diff = 2*pi - diff;
00638 if(diff < maxdiff)
00639 {
00640 maxdiff = diff;
00641 *iret = i;
00642 }
00643 }
00644 }
00645 avail[*iret] = 0;
00646 iret++;
00647 }
00648 }
00649
00650
00651
00652 int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
00653 const Vec3f& side2, const Matrix3f& R2, const Vec3f& T2,
00654 Vec3f& normal, FCL_REAL* depth, int* return_code,
00655 int maxc, std::vector<ContactPoint>& contacts)
00656 {
00657 const FCL_REAL fudge_factor = FCL_REAL(1.05);
00658 Vec3f normalC;
00659 FCL_REAL s, s2, l;
00660 int invert_normal, code;
00661
00662 Vec3f p = T2 - T1;
00663 Vec3f pp = R1.transposeTimes(p);
00664
00665
00666 Vec3f A = side1 * 0.5;
00667 Vec3f B = side2 * 0.5;
00668
00669
00670 Matrix3f R = R1.transposeTimes(R2);
00671 Matrix3f Q = abs(R);
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684 int best_col_id = -1;
00685 FCL_REAL tmp = 0;
00686
00687 s = - std::numeric_limits<FCL_REAL>::max();
00688 invert_normal = 0;
00689 code = 0;
00690
00691
00692 tmp = pp[0];
00693 s2 = std::abs(tmp) - (Q.dotX(B) + A[0]);
00694 if(s2 > 0) { *return_code = 0; return 0; }
00695 if(s2 > s)
00696 {
00697 s = s2;
00698 best_col_id = 0;
00699 invert_normal = (tmp < 0);
00700 code = 1;
00701 }
00702
00703 tmp = pp[1];
00704 s2 = std::abs(tmp) - (Q.dotY(B) + A[1]);
00705 if(s2 > 0) { *return_code = 0; return 0; }
00706 if(s2 > s)
00707 {
00708 s = s2;
00709 best_col_id = 1;
00710 invert_normal = (tmp < 0);
00711 code = 2;
00712 }
00713
00714 tmp = pp[2];
00715 s2 = std::abs(tmp) - (Q.dotZ(B) + A[2]);
00716 if(s2 > 0) { *return_code = 0; return 0; }
00717 if(s2 > s)
00718 {
00719 s = s2;
00720 best_col_id = 2;
00721 invert_normal = (tmp < 0);
00722 code = 3;
00723 }
00724
00725
00726 tmp = R2.transposeDotX(p);
00727 s2 = std::abs(tmp) - (Q.transposeDotX(A) + B[0]);
00728 if(s2 > 0) { *return_code = 0; return 0; }
00729 if(s2 > s)
00730 {
00731 s = s2;
00732 best_col_id = 0;
00733 code = 4;
00734 }
00735
00736 tmp = R2.transposeDotY(p);
00737 s2 = std::abs(tmp) - (Q.transposeDotY(A) + B[1]);
00738 if(s2 > 0) { *return_code = 0; return 0; }
00739 if(s2 > s)
00740 {
00741 s = s2;
00742 best_col_id = 1;
00743 code = 5;
00744 }
00745
00746 tmp = R2.transposeDotZ(p);
00747 s2 = std::abs(tmp) - (Q.transposeDotZ(A) + B[2]);
00748 if(s2 > 0) { *return_code = 0; return 0; }
00749 if(s2 > s)
00750 {
00751 s = s2;
00752 best_col_id = 2;
00753 code = 6;
00754 }
00755
00756
00757 FCL_REAL fudge2(1.0e-6);
00758 Q += fudge2;
00759
00760 Vec3f n;
00761 FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon();
00762
00763
00764 tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0);
00765 s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1));
00766 if(s2 > eps) { *return_code = 0; return 0; }
00767 n = Vec3f(0, -R(2, 0), R(1, 0));
00768 l = n.length();
00769 if(l > eps)
00770 {
00771 s2 /= l;
00772 if(s2 * fudge_factor > s)
00773 {
00774 s = s2;
00775 best_col_id = 0;
00776 normalC = n / l;
00777 invert_normal = (tmp < 0);
00778 code = 7;
00779 }
00780 }
00781
00782 tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1);
00783 s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0));
00784 if(s2 > eps) { *return_code = 0; return 0; }
00785 n = Vec3f(0, -R(2, 1), R(1, 1));
00786 l = n.length();
00787 if(l > eps)
00788 {
00789 s2 /= l;
00790 if(s2 * fudge_factor > s)
00791 {
00792 s = s2;
00793 best_col_id = 0;
00794 normalC = n / l;
00795 invert_normal = (tmp < 0);
00796 code = 8;
00797 }
00798 }
00799
00800 tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2);
00801 s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0));
00802 if(s2 > eps) { *return_code = 0; return 0; }
00803 n = Vec3f(0, -R(2, 2), R(1, 2));
00804 l = n.length();
00805 if(l > eps)
00806 {
00807 s2 /= l;
00808 if(s2 * fudge_factor > s)
00809 {
00810 s = s2;
00811 best_col_id = 0;
00812 normalC = n / l;
00813 invert_normal = (tmp < 0);
00814 code = 9;
00815 }
00816 }
00817
00818
00819 tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0);
00820 s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1));
00821 if(s2 > eps) { *return_code = 0; return 0; }
00822 n = Vec3f(R(2, 0), 0, -R(0, 0));
00823 l = n.length();
00824 if(l > eps)
00825 {
00826 s2 /= l;
00827 if(s2 * fudge_factor > s)
00828 {
00829 s = s2;
00830 best_col_id = 0;
00831 normalC = n / l;
00832 invert_normal = (tmp < 0);
00833 code = 10;
00834 }
00835 }
00836
00837 tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1);
00838 s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0));
00839 if(s2 > eps) { *return_code = 0; return 0; }
00840 n = Vec3f(R(2, 1), 0, -R(0, 1));
00841 l = n.length();
00842 if(l > eps)
00843 {
00844 s2 /= l;
00845 if(s2 * fudge_factor > s)
00846 {
00847 s = s2;
00848 best_col_id = 0;
00849 normalC = n / l;
00850 invert_normal = (tmp < 0);
00851 code = 11;
00852 }
00853 }
00854
00855 tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2);
00856 s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0));
00857 if(s2 > eps) { *return_code = 0; return 0; }
00858 n = Vec3f(R(2, 2), 0, -R(0, 2));
00859 l = n.length();
00860 if(l > eps)
00861 {
00862 s2 /= l;
00863 if(s2 * fudge_factor > s)
00864 {
00865 s = s2;
00866 best_col_id = 0;
00867 normalC = n / l;
00868 invert_normal = (tmp < 0);
00869 code = 12;
00870 }
00871 }
00872
00873
00874 tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0);
00875 s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1));
00876 if(s2 > eps) { *return_code = 0; return 0; }
00877 n = Vec3f(-R(1, 0), R(0, 0), 0);
00878 l = n.length();
00879 if(l > eps)
00880 {
00881 s2 /= l;
00882 if(s2 * fudge_factor > s)
00883 {
00884 s = s2;
00885 best_col_id = 0;
00886 normalC = n / l;
00887 invert_normal = (tmp < 0);
00888 code = 13;
00889 }
00890 }
00891
00892 tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1);
00893 s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0));
00894 if(s2 > eps) { *return_code = 0; return 0; }
00895 n = Vec3f(-R(1, 1), R(0, 1), 0);
00896 l = n.length();
00897 if(l > eps)
00898 {
00899 s2 /= l;
00900 if(s2 * fudge_factor > s)
00901 {
00902 s = s2;
00903 best_col_id = 0;
00904 normalC = n / l;
00905 invert_normal = (tmp < 0);
00906 code = 14;
00907 }
00908 }
00909
00910 tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2);
00911 s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0));
00912 if(s2 > eps) { *return_code = 0; return 0; }
00913 n = Vec3f(-R(1, 2), R(0, 2), 0);
00914 l = n.length();
00915 if(l > eps)
00916 {
00917 s2 /= l;
00918 if(s2 * fudge_factor > s)
00919 {
00920 s = s2;
00921 best_col_id = 0;
00922 normalC = n / l;
00923 invert_normal = (tmp < 0);
00924 code = 15;
00925 }
00926 }
00927
00928
00929
00930 if (!code) { *return_code = code; return 0; }
00931
00932
00933
00934 if(best_col_id != -1)
00935 normal = R.getColumn(best_col_id);
00936 else
00937 normal = R1 * normalC;
00938
00939 if(invert_normal)
00940 normal.negate();
00941
00942 *depth = -s;
00943
00944
00945
00946 if(code > 6)
00947 {
00948
00949
00950 Vec3f pa(T1);
00951 FCL_REAL sign;
00952
00953 for(int j = 0; j < 3; ++j)
00954 {
00955 sign = (R1.transposeDot(j, normal) > 0) ? 1 : -1;
00956 pa += R1.getColumn(j) * (A[j] * sign);
00957 }
00958
00959
00960 Vec3f pb;
00961 pb = T2;
00962 for(int j = 0; j < 3; ++j)
00963 {
00964 sign = (R2.transposeDot(j, normal) > 0) ? 1 : -1;
00965 pb += R2.getColumn(j) * (B[j] * sign);
00966 }
00967
00968 FCL_REAL alpha, beta;
00969 Vec3f ua(R1.getColumn((code-7)/3));
00970 Vec3f ub(R2.getColumn((code-7)%3));
00971
00972 lineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
00973 pa += ua * alpha;
00974 pb += ub * beta;
00975
00976
00977 Vec3f pointInWorld((pa + pb) * 0.5);
00978 contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth));
00979 *return_code = code;
00980
00981 return 1;
00982 }
00983
00984
00985
00986
00987
00988
00989 const Matrix3f *Ra, *Rb;
00990 const Vec3f *pa, *pb, *Sa, *Sb;
00991
00992 if(code <= 3)
00993 {
00994 Ra = &R1;
00995 Rb = &R2;
00996 pa = &T1;
00997 pb = &T2;
00998 Sa = &A;
00999 Sb = &B;
01000 }
01001 else
01002 {
01003 Ra = &R2;
01004 Rb = &R1;
01005 pa = &T2;
01006 pb = &T1;
01007 Sa = &B;
01008 Sb = &A;
01009 }
01010
01011
01012
01013 Vec3f normal2, nr, anr;
01014 if(code <= 3)
01015 normal2 = normal;
01016 else
01017 normal2 = -normal;
01018
01019 nr = Rb->transposeTimes(normal2);
01020 anr = abs(anr);
01021
01022
01023
01024
01025 int lanr, a1, a2;
01026 if(anr[1] > anr[0])
01027 {
01028 if(anr[1] > anr[2])
01029 {
01030 a1 = 0;
01031 lanr = 1;
01032 a2 = 2;
01033 }
01034 else
01035 {
01036 a1 = 0;
01037 a2 = 1;
01038 lanr = 2;
01039 }
01040 }
01041 else
01042 {
01043 if(anr[0] > anr[2])
01044 {
01045 lanr = 0;
01046 a1 = 1;
01047 a2 = 2;
01048 }
01049 else
01050 {
01051 a1 = 0;
01052 a2 = 1;
01053 lanr = 2;
01054 }
01055 }
01056
01057
01058 Vec3f center;
01059 if(nr[lanr] < 0)
01060 center = (*pb) - (*pa) + Rb->getColumn(lanr) * ((*Sb)[lanr]);
01061 else
01062 center = (*pb) - (*pa) - Rb->getColumn(lanr) * ((*Sb)[lanr]);
01063
01064
01065 int codeN, code1, code2;
01066 if(code <= 3)
01067 codeN = code-1;
01068 else codeN = code-4;
01069
01070 if(codeN == 0)
01071 {
01072 code1 = 1;
01073 code2 = 2;
01074 }
01075 else if(codeN == 1)
01076 {
01077 code1 = 0;
01078 code2 = 2;
01079 }
01080 else
01081 {
01082 code1 = 0;
01083 code2 = 1;
01084 }
01085
01086
01087 FCL_REAL quad[8];
01088 FCL_REAL c1, c2, m11, m12, m21, m22;
01089 c1 = Ra->transposeDot(code1, center);
01090 c2 = Ra->transposeDot(code2, center);
01091
01092
01093
01094 Vec3f tempRac = Ra->getColumn(code1);
01095 m11 = Rb->transposeDot(a1, tempRac);
01096 m12 = Rb->transposeDot(a2, tempRac);
01097 tempRac = Ra->getColumn(code2);
01098 m21 = Rb->transposeDot(a1, tempRac);
01099 m22 = Rb->transposeDot(a2, tempRac);
01100
01101 FCL_REAL k1 = m11 * (*Sb)[a1];
01102 FCL_REAL k2 = m21 * (*Sb)[a1];
01103 FCL_REAL k3 = m12 * (*Sb)[a2];
01104 FCL_REAL k4 = m22 * (*Sb)[a2];
01105 quad[0] = c1 - k1 - k3;
01106 quad[1] = c2 - k2 - k4;
01107 quad[2] = c1 - k1 + k3;
01108 quad[3] = c2 - k2 + k4;
01109 quad[4] = c1 + k1 + k3;
01110 quad[5] = c2 + k2 + k4;
01111 quad[6] = c1 + k1 - k3;
01112 quad[7] = c2 + k2 - k4;
01113
01114
01115 FCL_REAL rect[2];
01116 rect[0] = (*Sa)[code1];
01117 rect[1] = (*Sa)[code2];
01118
01119
01120 FCL_REAL ret[16];
01121 int n_intersect = intersectRectQuad2(rect, quad, ret);
01122 if(n_intersect < 1) { *return_code = code; return 0; }
01123
01124
01125
01126
01127
01128 Vec3f points[8];
01129 FCL_REAL dep[8];
01130 FCL_REAL det1 = 1.f/(m11*m22 - m12*m21);
01131 m11 *= det1;
01132 m12 *= det1;
01133 m21 *= det1;
01134 m22 *= det1;
01135 int cnum = 0;
01136 for(int j = 0; j < n_intersect; ++j)
01137 {
01138 FCL_REAL k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
01139 FCL_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
01140 points[cnum] = center + Rb->getColumn(a1) * k1 + Rb->getColumn(a2) * k2;
01141 dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]);
01142 if(dep[cnum] >= 0)
01143 {
01144 ret[cnum*2] = ret[j*2];
01145 ret[cnum*2+1] = ret[j*2+1];
01146 cnum++;
01147 }
01148 }
01149 if(cnum < 1) { *return_code = code; return 0; }
01150
01151
01152 if(maxc > cnum) maxc = cnum;
01153 if(maxc < 1) maxc = 1;
01154
01155 if(cnum <= maxc)
01156 {
01157 if(code<4)
01158 {
01159
01160 for(int j = 0; j < cnum; ++j)
01161 {
01162 Vec3f pointInWorld = points[j] + (*pa);
01163 contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
01164 }
01165 }
01166 else
01167 {
01168
01169 for(int j = 0; j < cnum; ++j)
01170 {
01171 Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j];
01172 contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
01173 }
01174 }
01175 }
01176 else
01177 {
01178
01179
01180 int i1 = 0;
01181 FCL_REAL maxdepth = dep[0];
01182 for(int i = 1; i < cnum; ++i)
01183 {
01184 if(dep[i] > maxdepth)
01185 {
01186 maxdepth = dep[i];
01187 i1 = i;
01188 }
01189 }
01190
01191 int iret[8];
01192 cullPoints2(cnum, ret, maxc, i1, iret);
01193
01194 for(int j = 0; j < maxc; ++j)
01195 {
01196 Vec3f posInWorld = points[iret[j] * 3] + (*pa);
01197 if(code < 4)
01198 contacts.push_back(ContactPoint(-normal, posInWorld, -dep[iret[j]]));
01199 else
01200 contacts.push_back(ContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]));
01201 }
01202 cnum = maxc;
01203 }
01204
01205 *return_code = code;
01206 return cnum;
01207 }
01208
01209
01210
01211 bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
01212 const Box& s2, const Transform3f& tf2,
01213 Vec3f* contact_points, FCL_REAL* penetration_depth_, Vec3f* normal_)
01214 {
01215 std::vector<ContactPoint> contacts;
01216 int return_code;
01217 Vec3f normal;
01218 FCL_REAL depth;
01219 int cnum = boxBox2(s1.side, tf1.getRotation(), tf1.getTranslation(),
01220 s2.side, tf2.getRotation(), tf2.getTranslation(),
01221 normal, &depth, &return_code,
01222 4, contacts);
01223
01224 if(normal_) *normal_ = normal;
01225 if(penetration_depth_) *penetration_depth_ = depth;
01226
01227 if(contact_points)
01228 {
01229 Vec3f contact_point;
01230 for(size_t i = 0; i < contacts.size(); ++i)
01231 {
01232 contact_point += contacts[i].point;
01233 }
01234
01235 contact_point = contact_point / (FCL_REAL)contacts.size();
01236
01237 *contact_points = contact_point;
01238 }
01239
01240 return return_code != 0;
01241 }
01242
01243
01244
01245
01246 }
01247
01248
01249 template<>
01250 bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
01251 const Sphere& s2, const Transform3f& tf2,
01252 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01253 {
01254 return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
01255 }
01256
01257 template<>
01258 bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
01259 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01260 {
01261 return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
01262 }
01263
01264 template<>
01265 bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
01266 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01267 {
01268 return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal);
01269 }
01270
01271 template<>
01272 bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
01273 const Sphere& s2, const Transform3f& tf2,
01274 FCL_REAL* dist) const
01275 {
01276 return details::sphereSphereDistance(s1, tf1, s2, tf2, dist);
01277 }
01278
01279 template<>
01280 bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
01281 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
01282 FCL_REAL* dist) const
01283 {
01284 return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist);
01285 }
01286
01287 template<>
01288 bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
01289 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
01290 FCL_REAL* dist) const
01291 {
01292 return details::sphereTriangleDistance(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist);
01293 }
01294
01295
01296
01297
01298
01299 template<>
01300 bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
01301 const Sphere& s2, const Transform3f& tf2,
01302 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01303 {
01304 return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
01305 }
01306
01307 template<>
01308 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
01309 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01310 {
01311 return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
01312 }
01313
01314 template<>
01315 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
01316 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01317 {
01318 return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal);
01319 }
01320
01321
01322 template<>
01323 bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
01324 const Sphere& s2, const Transform3f& tf2,
01325 FCL_REAL* dist) const
01326 {
01327 return details::sphereSphereDistance(s1, tf1, s2, tf2, dist);
01328 }
01329
01330
01331 template<>
01332 bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
01333 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
01334 FCL_REAL* dist) const
01335 {
01336 return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist);
01337 }
01338
01339 template<>
01340 bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
01341 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
01342 FCL_REAL* dist) const
01343 {
01344 return details::sphereTriangleDistance(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist);
01345 }
01346
01347
01348 template<>
01349 bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
01350 const Box& s2, const Transform3f& tf2,
01351 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01352 {
01353 return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
01354 }
01355
01356 template<>
01357 bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
01358 const Box& s2, const Transform3f& tf2,
01359 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
01360 {
01361 return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
01362 }
01363
01364
01365
01366 }