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
00038 #include "fcl/traversal/traversal_node_bvhs.h"
00039
00040 namespace fcl
00041 {
00042
00043 namespace details
00044 {
00045 template<typename BV>
00046 static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
00047 const BVHModel<BV>* model1, const BVHModel<BV>* model2,
00048 Vec3f* vertices1, Vec3f* vertices2,
00049 Triangle* tri_indices1, Triangle* tri_indices2,
00050 const Matrix3f& R, const Vec3f& T,
00051 const Transform3f& tf1, const Transform3f& tf2,
00052 bool enable_statistics,
00053 FCL_REAL cost_density,
00054 int& num_leaf_tests,
00055 const CollisionRequest& request,
00056 CollisionResult& result)
00057 {
00058 if(enable_statistics) num_leaf_tests++;
00059
00060 const BVNode<BV>& node1 = model1->getBV(b1);
00061 const BVNode<BV>& node2 = model2->getBV(b2);
00062
00063 int primitive_id1 = node1.primitiveId();
00064 int primitive_id2 = node2.primitiveId();
00065
00066 const Triangle& tri_id1 = tri_indices1[primitive_id1];
00067 const Triangle& tri_id2 = tri_indices2[primitive_id2];
00068
00069 const Vec3f& p1 = vertices1[tri_id1[0]];
00070 const Vec3f& p2 = vertices1[tri_id1[1]];
00071 const Vec3f& p3 = vertices1[tri_id1[2]];
00072 const Vec3f& q1 = vertices2[tri_id2[0]];
00073 const Vec3f& q2 = vertices2[tri_id2[1]];
00074 const Vec3f& q3 = vertices2[tri_id2[2]];
00075
00076 if(model1->isOccupied() && model2->isOccupied())
00077 {
00078 bool is_intersect = false;
00079
00080 if(!request.enable_contact)
00081 {
00082 if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
00083 {
00084 is_intersect = true;
00085 if(result.numContacts() < request.num_max_contacts)
00086 result.addContact(Contact(model1, model2, primitive_id1, primitive_id2));
00087 }
00088 }
00089 else
00090 {
00091 FCL_REAL penetration;
00092 Vec3f normal;
00093 unsigned int n_contacts;
00094 Vec3f contacts[2];
00095
00096 if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
00097 R, T,
00098 contacts,
00099 &n_contacts,
00100 &penetration,
00101 &normal))
00102 {
00103 is_intersect = true;
00104
00105 if(request.num_max_contacts < result.numContacts() + n_contacts)
00106 n_contacts = (request.num_max_contacts > result.numContacts()) ? (request.num_max_contacts - result.numContacts()) : 0;
00107
00108 for(unsigned int i = 0; i < n_contacts; ++i)
00109 {
00110 result.addContact(Contact(model1, model2, primitive_id1, primitive_id2, tf1.transform(contacts[i]), tf1.getQuatRotation().transform(normal), penetration));
00111 }
00112 }
00113 }
00114
00115 if(is_intersect && request.enable_cost)
00116 {
00117 AABB overlap_part;
00118 AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(AABB(tf2.transform(q1), tf2.transform(q2), tf2.transform(q3)), overlap_part);
00119 result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
00120 }
00121 }
00122 else if((!model1->isFree() && !model2->isFree()) && request.enable_cost)
00123 {
00124 if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
00125 {
00126 AABB overlap_part;
00127 AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(AABB(tf2.transform(q1), tf2.transform(q2), tf2.transform(q3)), overlap_part);
00128 result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
00129 }
00130 }
00131 }
00132
00133
00134
00135
00136 template<typename BV>
00137 static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
00138 const BVHModel<BV>* model1, const BVHModel<BV>* model2,
00139 Vec3f* vertices1, Vec3f* vertices2,
00140 Triangle* tri_indices1, Triangle* tri_indices2,
00141 const Matrix3f& R, const Vec3f& T,
00142 bool enable_statistics,
00143 int& num_leaf_tests,
00144 const DistanceRequest& request,
00145 DistanceResult& result)
00146 {
00147 if(enable_statistics) num_leaf_tests++;
00148
00149 const BVNode<BV>& node1 = model1->getBV(b1);
00150 const BVNode<BV>& node2 = model2->getBV(b2);
00151
00152 int primitive_id1 = node1.primitiveId();
00153 int primitive_id2 = node2.primitiveId();
00154
00155 const Triangle& tri_id1 = tri_indices1[primitive_id1];
00156 const Triangle& tri_id2 = tri_indices2[primitive_id2];
00157
00158 const Vec3f& t11 = vertices1[tri_id1[0]];
00159 const Vec3f& t12 = vertices1[tri_id1[1]];
00160 const Vec3f& t13 = vertices1[tri_id1[2]];
00161
00162 const Vec3f& t21 = vertices2[tri_id2[0]];
00163 const Vec3f& t22 = vertices2[tri_id2[1]];
00164 const Vec3f& t23 = vertices2[tri_id2[2]];
00165
00166
00167 Vec3f P1, P2;
00168
00169 FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
00170 R, T,
00171 P1, P2);
00172
00173 if(request.enable_nearest_points)
00174 result.update(d, model1, model2, primitive_id1, primitive_id2, P1, P2);
00175 else
00176 result.update(d, model1, model2, primitive_id1, primitive_id2);
00177 }
00178
00179 }
00180
00181 MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB() : MeshCollisionTraversalNode<OBB>()
00182 {
00183 R.setIdentity();
00184 }
00185
00186 bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
00187 {
00188 if(enable_statistics) num_bv_tests++;
00189 return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
00190 }
00191
00192 void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
00193 {
00194 details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
00195 tri_indices1, tri_indices2,
00196 R, T,
00197 tf1, tf2,
00198 enable_statistics, cost_density,
00199 num_leaf_tests,
00200 request, *result);
00201 }
00202
00203
00204 bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const
00205 {
00206 if(enable_statistics) num_bv_tests++;
00207 return obbDisjoint(Rc, Tc, model1->getBV(b1).bv.extent, model2->getBV(b2).bv.extent);
00208 }
00209
00210 void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const
00211 {
00212 details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
00213 tri_indices1, tri_indices2,
00214 R, T,
00215 tf1, tf2,
00216 enable_statistics, cost_density,
00217 num_leaf_tests,
00218 request, *result);
00219 }
00220
00221
00222
00223 MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS() : MeshCollisionTraversalNode<RSS>()
00224 {
00225 R.setIdentity();
00226 }
00227
00228 bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
00229 {
00230 if(enable_statistics) num_bv_tests++;
00231 return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
00232 }
00233
00234 void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
00235 {
00236 details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
00237 tri_indices1, tri_indices2,
00238 R, T,
00239 tf1, tf2,
00240 enable_statistics, cost_density,
00241 num_leaf_tests,
00242 request, *result);
00243 }
00244
00245
00246
00247
00248 MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS() : MeshCollisionTraversalNode<kIOS>()
00249 {
00250 R.setIdentity();
00251 }
00252
00253 bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const
00254 {
00255 if(enable_statistics) num_bv_tests++;
00256 return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
00257 }
00258
00259 void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const
00260 {
00261 details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
00262 tri_indices1, tri_indices2,
00263 R, T,
00264 tf1, tf2,
00265 enable_statistics, cost_density,
00266 num_leaf_tests,
00267 request, *result);
00268 }
00269
00270
00271
00272 MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS() : MeshCollisionTraversalNode<OBBRSS>()
00273 {
00274 R.setIdentity();
00275 }
00276
00277 bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
00278 {
00279 if(enable_statistics) num_bv_tests++;
00280 return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
00281 }
00282
00283 void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
00284 {
00285 details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
00286 tri_indices1, tri_indices2,
00287 R, T,
00288 tf1, tf2,
00289 enable_statistics, cost_density,
00290 num_leaf_tests,
00291 request,*result);
00292 }
00293
00294
00295 namespace details
00296 {
00297
00298 template<typename BV>
00299 static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1, const BVHModel<BV>* model2,
00300 const Vec3f* vertices1, Vec3f* vertices2,
00301 Triangle* tri_indices1, Triangle* tri_indices2,
00302 int init_tri_id1, int init_tri_id2,
00303 const Matrix3f& R, const Vec3f& T,
00304 const DistanceRequest& request,
00305 DistanceResult& result)
00306 {
00307 const Triangle& init_tri1 = tri_indices1[init_tri_id1];
00308 const Triangle& init_tri2 = tri_indices2[init_tri_id2];
00309
00310 Vec3f init_tri1_points[3];
00311 Vec3f init_tri2_points[3];
00312
00313 init_tri1_points[0] = vertices1[init_tri1[0]];
00314 init_tri1_points[1] = vertices1[init_tri1[1]];
00315 init_tri1_points[2] = vertices1[init_tri1[2]];
00316
00317 init_tri2_points[0] = vertices2[init_tri2[0]];
00318 init_tri2_points[1] = vertices2[init_tri2[1]];
00319 init_tri2_points[2] = vertices2[init_tri2[2]];
00320
00321 Vec3f p1, p2;
00322 FCL_REAL distance = TriangleDistance::triDistance(init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
00323 init_tri2_points[0], init_tri2_points[1], init_tri2_points[2],
00324 R, T, p1, p2);
00325
00326 if(request.enable_nearest_points)
00327 result.update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2);
00328 else
00329 result.update(distance, model1, model2, init_tri_id1, init_tri_id2);
00330 }
00331
00332 template<typename BV>
00333 static inline void distancePostprocessOrientedNode(const BVHModel<BV>* model1, const BVHModel<BV>* model2,
00334 const Transform3f& tf1, const DistanceRequest& request, DistanceResult& result)
00335 {
00337 if(request.enable_nearest_points && (result.o1 == model1) && (result.o2 == model2))
00338 {
00339 result.nearest_points[0] = tf1.transform(result.nearest_points[0]);
00340 result.nearest_points[1] = tf1.transform(result.nearest_points[1]);
00341 }
00342 }
00343
00344 }
00345
00346 MeshDistanceTraversalNodeRSS::MeshDistanceTraversalNodeRSS() : MeshDistanceTraversalNode<RSS>()
00347 {
00348 R.setIdentity();
00349 }
00350
00351 void MeshDistanceTraversalNodeRSS::preprocess()
00352 {
00353 details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
00354 }
00355
00356 void MeshDistanceTraversalNodeRSS::postprocess()
00357 {
00358 details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
00359 }
00360
00361 FCL_REAL MeshDistanceTraversalNodeRSS::BVTesting(int b1, int b2) const
00362 {
00363 if(enable_statistics) num_bv_tests++;
00364 return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
00365 }
00366
00367 void MeshDistanceTraversalNodeRSS::leafTesting(int b1, int b2) const
00368 {
00369 details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
00370 R, T, enable_statistics, num_leaf_tests,
00371 request, *result);
00372 }
00373
00374 MeshDistanceTraversalNodekIOS::MeshDistanceTraversalNodekIOS() : MeshDistanceTraversalNode<kIOS>()
00375 {
00376 R.setIdentity();
00377 }
00378
00379 void MeshDistanceTraversalNodekIOS::preprocess()
00380 {
00381 details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
00382 }
00383
00384 void MeshDistanceTraversalNodekIOS::postprocess()
00385 {
00386 details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
00387 }
00388
00389 FCL_REAL MeshDistanceTraversalNodekIOS::BVTesting(int b1, int b2) const
00390 {
00391 if(enable_statistics) num_bv_tests++;
00392 return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
00393 }
00394
00395 void MeshDistanceTraversalNodekIOS::leafTesting(int b1, int b2) const
00396 {
00397 details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
00398 R, T, enable_statistics, num_leaf_tests,
00399 request, *result);
00400 }
00401
00402 MeshDistanceTraversalNodeOBBRSS::MeshDistanceTraversalNodeOBBRSS() : MeshDistanceTraversalNode<OBBRSS>()
00403 {
00404 R.setIdentity();
00405 }
00406
00407 void MeshDistanceTraversalNodeOBBRSS::preprocess()
00408 {
00409 details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
00410 }
00411
00412 void MeshDistanceTraversalNodeOBBRSS::postprocess()
00413 {
00414 details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
00415 }
00416
00417 FCL_REAL MeshDistanceTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
00418 {
00419 if(enable_statistics) num_bv_tests++;
00420 return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
00421 }
00422
00423 void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
00424 {
00425 details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
00426 R, T, enable_statistics, num_leaf_tests,
00427 request, *result);
00428 }
00429
00430
00432 template<>
00433 bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
00434 {
00435 if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
00436 {
00437 const ConservativeAdvancementStackData& data = stack.back();
00438 FCL_REAL d = data.d;
00439 Vec3f n;
00440 int c1, c2;
00441
00442 if(d > c)
00443 {
00444 const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
00445 d = data2.d;
00446 n = data2.P2 - data2.P1;
00447 c1 = data2.c1;
00448 c2 = data2.c2;
00449 stack[stack.size() - 2] = stack[stack.size() - 1];
00450 }
00451 else
00452 {
00453 n = data.P2 - data.P1;
00454 c1 = data.c1;
00455 c2 = data.c2;
00456 }
00457
00458 assert(c == d);
00459
00460 Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[1] + model1->getBV(c1).bv.axis[2] * n[2];
00461
00462 FCL_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
00463 FCL_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
00464
00465 FCL_REAL bound = bound1 + bound2;
00466
00467 FCL_REAL cur_delta_t;
00468 if(bound <= c) cur_delta_t = 1;
00469 else cur_delta_t = c / bound;
00470
00471 if(cur_delta_t < delta_t)
00472 delta_t = cur_delta_t;
00473
00474 stack.pop_back();
00475
00476 return true;
00477 }
00478 else
00479 {
00480 const ConservativeAdvancementStackData& data = stack.back();
00481 FCL_REAL d = data.d;
00482
00483 if(d > c)
00484 stack[stack.size() - 2] = stack[stack.size() - 1];
00485
00486 stack.pop_back();
00487
00488 return false;
00489 }
00490 }
00491
00492 template<>
00493 bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const
00494 {
00495 if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
00496 {
00497 const ConservativeAdvancementStackData& data = stack.back();
00498 FCL_REAL d = data.d;
00499 Vec3f n;
00500 int c1, c2;
00501
00502 if(d > c)
00503 {
00504 const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
00505 d = data2.d;
00506 n = data2.P2 - data2.P1;
00507 c1 = data2.c1;
00508 c2 = data2.c2;
00509 stack[stack.size() - 2] = stack[stack.size() - 1];
00510 }
00511 else
00512 {
00513 n = data.P2 - data.P1;
00514 c1 = data.c1;
00515 c2 = data.c2;
00516 }
00517
00518 assert(c == d);
00519
00520 Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[1] + model1->getBV(c1).bv.axis[2] * n[2];
00521
00522 FCL_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
00523 FCL_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
00524
00525 FCL_REAL bound = bound1 + bound2;
00526
00527 FCL_REAL cur_delta_t;
00528 if(bound <= c) cur_delta_t = 1;
00529 else cur_delta_t = c / bound;
00530
00531 if(cur_delta_t < delta_t)
00532 delta_t = cur_delta_t;
00533
00534 stack.pop_back();
00535
00536 return true;
00537 }
00538 else
00539 {
00540 const ConservativeAdvancementStackData& data = stack.back();
00541 FCL_REAL d = data.d;
00542
00543 if(d > c)
00544 stack[stack.size() - 2] = stack[stack.size() - 1];
00545
00546 stack.pop_back();
00547
00548 return false;
00549 }
00550 }
00551
00552
00553 MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_) : MeshConservativeAdvancementTraversalNode<RSS>(w_)
00554 {
00555 R.setIdentity();
00556
00557 }
00558
00559 FCL_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2) const
00560 {
00561 if(enable_statistics) num_bv_tests++;
00562 Vec3f P1, P2;
00563 FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
00564
00565 stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
00566
00567 return d;
00568 }
00569
00570
00571 void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) const
00572 {
00573 if(enable_statistics) num_leaf_tests++;
00574
00575 const BVNode<RSS>& node1 = model1->getBV(b1);
00576 const BVNode<RSS>& node2 = model2->getBV(b2);
00577
00578 int primitive_id1 = node1.primitiveId();
00579 int primitive_id2 = node2.primitiveId();
00580
00581 const Triangle& tri_id1 = tri_indices1[primitive_id1];
00582 const Triangle& tri_id2 = tri_indices2[primitive_id2];
00583
00584 const Vec3f& t11 = vertices1[tri_id1[0]];
00585 const Vec3f& t12 = vertices1[tri_id1[1]];
00586 const Vec3f& t13 = vertices1[tri_id1[2]];
00587
00588 const Vec3f& t21 = vertices2[tri_id2[0]];
00589 const Vec3f& t22 = vertices2[tri_id2[1]];
00590 const Vec3f& t23 = vertices2[tri_id2[2]];
00591
00592
00593 Vec3f P1, P2;
00594
00595 FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
00596 R, T,
00597 P1, P2);
00598
00599 if(d < min_distance)
00600 {
00601 min_distance = d;
00602
00603 p1 = P1;
00604 p2 = P2;
00605
00606 last_tri_id1 = primitive_id1;
00607 last_tri_id2 = primitive_id2;
00608 }
00609
00610
00612 Vec3f n = P2 - P1;
00614 Matrix3f R0;
00615 motion1->getCurrentRotation(R0);
00616 Vec3f n_transformed = R0 * n;
00617 n_transformed.normalize();
00618 FCL_REAL bound1 = motion1->computeMotionBound(t11, t12, t13, n_transformed);
00619 FCL_REAL bound2 = motion2->computeMotionBound(t21, t22, t23, -n_transformed);
00620
00621 FCL_REAL bound = bound1 + bound2;
00622
00623 FCL_REAL cur_delta_t;
00624 if(bound <= d) cur_delta_t = 1;
00625 else cur_delta_t = d / bound;
00626
00627 if(cur_delta_t < delta_t)
00628 delta_t = cur_delta_t;
00629 }
00630
00631 bool MeshConservativeAdvancementTraversalNodeRSS::canStop(FCL_REAL c) const
00632 {
00633 if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
00634 {
00635 const ConservativeAdvancementStackData& data = stack.back();
00636 FCL_REAL d = data.d;
00637 Vec3f n;
00638 int c1, c2;
00639
00640 if(d > c)
00641 {
00642 const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
00643 d = data2.d;
00644 n = data2.P2 - data2.P1;
00645 c1 = data2.c1;
00646 c2 = data2.c2;
00647 stack[stack.size() - 2] = stack[stack.size() - 1];
00648 }
00649 else
00650 {
00651 n = data.P2 - data.P1;
00652 c1 = data.c1;
00653 c2 = data.c2;
00654 }
00655
00656 assert(c == d);
00657
00658
00659 Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[2] + model1->getBV(c1).bv.axis[2] * n[2];
00660 Matrix3f R0;
00661 motion1->getCurrentRotation(R0);
00662 n_transformed = R0 * n_transformed;
00663 n_transformed.normalize();
00664
00665 FCL_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
00666 FCL_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, -n_transformed);
00667
00668 FCL_REAL bound = bound1 + bound2;
00669
00670 FCL_REAL cur_delta_t;
00671 if(bound <= c) cur_delta_t = 1;
00672 else cur_delta_t = c / bound;
00673
00674 if(cur_delta_t < delta_t)
00675 delta_t = cur_delta_t;
00676
00677 stack.pop_back();
00678
00679 return true;
00680 }
00681 else
00682 {
00683 const ConservativeAdvancementStackData& data = stack.back();
00684 FCL_REAL d = data.d;
00685
00686 if(d > c)
00687 stack[stack.size() - 2] = stack[stack.size() - 1];
00688
00689 stack.pop_back();
00690
00691 return false;
00692 }
00693 }
00694
00695
00696
00697
00698
00699 }