fcl » src » traversal
traversal_node_bvhs.cpp

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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) // only interested in collision or not
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 // need compute the contact information
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   // nearest point pair
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   // default T is 0
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   // nearest point pair
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     // n is in local frame of RSS c1, so we need to turn n into the global frame
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 }