traversal_node_bvhs.h
Go to the documentation of this file.
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 #ifndef FCL_TRAVERSAL_NODE_MESHES_H
00039 #define FCL_TRAVERSAL_NODE_MESHES_H
00040 
00041 #include "fcl/collision_data.h"
00042 #include "fcl/traversal/traversal_node_base.h"
00043 #include "fcl/BV/BV_node.h"
00044 #include "fcl/BV/BV.h"
00045 #include "fcl/BVH/BVH_model.h"
00046 #include "fcl/intersect.h"
00047 #include "fcl/ccd/motion.h"
00048 
00049 #include <boost/shared_array.hpp>
00050 #include <boost/shared_ptr.hpp>
00051 #include <limits>
00052 #include <vector>
00053 #include <cassert>
00054 
00055 
00056 namespace fcl
00057 {
00058 
00060 template<typename BV>
00061 class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
00062 {
00063 public:
00064   BVHCollisionTraversalNode() : CollisionTraversalNodeBase()
00065   {
00066     model1 = NULL;
00067     model2 = NULL;
00068 
00069     num_bv_tests = 0;
00070     num_leaf_tests = 0;
00071     query_time_seconds = 0.0;
00072   }
00073 
00075   bool isFirstNodeLeaf(int b) const
00076   {
00077     return model1->getBV(b).isLeaf();
00078   }
00079 
00081   bool isSecondNodeLeaf(int b) const
00082   {
00083     return model2->getBV(b).isLeaf();
00084   }
00085 
00087   bool firstOverSecond(int b1, int b2) const
00088   {
00089     FCL_REAL sz1 = model1->getBV(b1).bv.size();
00090     FCL_REAL sz2 = model2->getBV(b2).bv.size();
00091 
00092     bool l1 = model1->getBV(b1).isLeaf();
00093     bool l2 = model2->getBV(b2).isLeaf();
00094 
00095     if(l2 || (!l1 && (sz1 > sz2)))
00096       return true;
00097     return false;
00098   }
00099 
00101   int getFirstLeftChild(int b) const
00102   {
00103     return model1->getBV(b).leftChild();
00104   }
00105 
00107   int getFirstRightChild(int b) const
00108   {
00109     return model1->getBV(b).rightChild();
00110   }
00111 
00113   int getSecondLeftChild(int b) const
00114   {
00115     return model2->getBV(b).leftChild();
00116   }
00117 
00119   int getSecondRightChild(int b) const
00120   {
00121     return model2->getBV(b).rightChild();
00122   }
00123 
00125   bool BVTesting(int b1, int b2) const
00126   {
00127     if(enable_statistics) num_bv_tests++;
00128     return !model1->getBV(b1).overlap(model2->getBV(b2));
00129   }
00130   
00132   const BVHModel<BV>* model1;
00134   const BVHModel<BV>* model2;
00135 
00137   mutable int num_bv_tests;
00138   mutable int num_leaf_tests;
00139   mutable FCL_REAL query_time_seconds;
00140 
00141 };
00142 
00143 
00145 template<typename BV>
00146 class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
00147 {
00148 public:
00149   MeshCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
00150   {
00151     vertices1 = NULL;
00152     vertices2 = NULL;
00153     tri_indices1 = NULL;
00154     tri_indices2 = NULL;
00155   }
00156 
00158   void leafTesting(int b1, int b2) const
00159   {
00160     if(this->enable_statistics) this->num_leaf_tests++;
00161 
00162     const BVNode<BV>& node1 = this->model1->getBV(b1);
00163     const BVNode<BV>& node2 = this->model2->getBV(b2);
00164 
00165     int primitive_id1 = node1.primitiveId();
00166     int primitive_id2 = node2.primitiveId();
00167 
00168     const Triangle& tri_id1 = tri_indices1[primitive_id1];
00169     const Triangle& tri_id2 = tri_indices2[primitive_id2];
00170 
00171     const Vec3f& p1 = vertices1[tri_id1[0]];
00172     const Vec3f& p2 = vertices1[tri_id1[1]];
00173     const Vec3f& p3 = vertices1[tri_id1[2]];
00174     const Vec3f& q1 = vertices2[tri_id2[0]];
00175     const Vec3f& q2 = vertices2[tri_id2[1]];
00176     const Vec3f& q3 = vertices2[tri_id2[2]];
00177     
00178     if(this->model1->isOccupied() && this->model2->isOccupied())
00179     {
00180       bool is_intersect = false;
00181 
00182       if(!this->request.enable_contact) // only interested in collision or not
00183       {
00184         if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
00185         {
00186           is_intersect = true;
00187           if(this->result->numContacts() < this->request.num_max_contacts)
00188             this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2));
00189         }
00190       }
00191       else // need compute the contact information
00192       {
00193         FCL_REAL penetration;
00194         Vec3f normal;
00195         unsigned int n_contacts;
00196         Vec3f contacts[2];
00197 
00198         if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
00199                                          contacts,
00200                                          &n_contacts,
00201                                          &penetration,
00202                                          &normal))
00203         {
00204           is_intersect = true;
00205           
00206           if(this->request.num_max_contacts < n_contacts + this->result->numContacts())
00207             n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0;
00208     
00209           for(unsigned int i = 0; i < n_contacts; ++i)
00210           {
00211             this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, contacts[i], normal, penetration));
00212           }
00213         }
00214       }
00215 
00216       if(is_intersect && this->request.enable_cost)
00217       {
00218         AABB overlap_part;
00219         AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
00220         this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);      
00221       }
00222     }   
00223     else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
00224     {
00225       if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
00226       {
00227         AABB overlap_part;
00228         AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
00229         this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);      
00230       }
00231     }
00232   }
00233 
00235   bool canStop() const
00236   {
00237     return this->request.isSatisfied(*(this->result));
00238   }
00239 
00240   Vec3f* vertices1;
00241   Vec3f* vertices2;
00242 
00243   Triangle* tri_indices1;
00244   Triangle* tri_indices2;
00245 
00246   FCL_REAL cost_density;
00247 };
00248 
00249 
00251 class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB>
00252 {
00253 public:
00254   MeshCollisionTraversalNodeOBB();
00255 
00256   bool BVTesting(int b1, int b2) const;
00257 
00258   void leafTesting(int b1, int b2) const;
00259 
00260   bool BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
00261 
00262   void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
00263 
00264   Matrix3f R;
00265   Vec3f T;
00266 };
00267 
00268 class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS>
00269 {
00270 public:
00271   MeshCollisionTraversalNodeRSS();
00272 
00273   bool BVTesting(int b1, int b2) const;
00274 
00275   void leafTesting(int b1, int b2) const;
00276 
00277   bool BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
00278 
00279   void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
00280 
00281   Matrix3f R;
00282   Vec3f T;
00283 };
00284 
00285 class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
00286 {
00287 public:
00288   MeshCollisionTraversalNodekIOS();
00289  
00290   bool BVTesting(int b1, int b2) const;
00291 
00292   void leafTesting(int b1, int b2) const;
00293 
00294   Matrix3f R;
00295   Vec3f T;
00296 };
00297 
00298 class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode<OBBRSS>
00299 {
00300 public:
00301   MeshCollisionTraversalNodeOBBRSS();
00302  
00303   bool BVTesting(int b1, int b2) const;
00304 
00305   void leafTesting(int b1, int b2) const;
00306 
00307   Matrix3f R;
00308   Vec3f T;
00309 };
00310 
00312 struct BVHContinuousCollisionPair
00313 {
00314   BVHContinuousCollisionPair() {}
00315 
00316   BVHContinuousCollisionPair(int id1_, int id2_, FCL_REAL time) : id1(id1_), id2(id2_), collision_time(time) {}
00317 
00319   int id1;
00320 
00322   int id2;
00323 
00325   FCL_REAL collision_time;
00326 };
00327 
00329 template<typename BV>
00330 class MeshContinuousCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
00331 {
00332 public:
00333   MeshContinuousCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
00334   {
00335     vertices1 = NULL;
00336     vertices2 = NULL;
00337     tri_indices1 = NULL;
00338     tri_indices2 = NULL;
00339     prev_vertices1 = NULL;
00340     prev_vertices2 = NULL;
00341 
00342     num_vf_tests = 0;
00343     num_ee_tests = 0;
00344   }
00345 
00347   void leafTesting(int b1, int b2) const
00348   {
00349     if(this->enable_statistics) this->num_leaf_tests++;
00350 
00351     const BVNode<BV>& node1 = this->model1->getBV(b1);
00352     const BVNode<BV>& node2 = this->model2->getBV(b2);
00353 
00354     FCL_REAL collision_time = 2;
00355     Vec3f collision_pos;
00356 
00357     int primitive_id1 = node1.primitiveId();
00358     int primitive_id2 = node2.primitiveId();
00359 
00360     const Triangle& tri_id1 = tri_indices1[primitive_id1];
00361     const Triangle& tri_id2 = tri_indices2[primitive_id2];
00362 
00363     Vec3f* S0[3];
00364     Vec3f* S1[3];
00365     Vec3f* T0[3];
00366     Vec3f* T1[3];
00367 
00368 
00369     for(int i = 0; i < 3; ++i)
00370     {
00371       S0[i] = prev_vertices1 + tri_id1[i];
00372       S1[i] = vertices1 + tri_id1[i];
00373       T0[i] = prev_vertices2 + tri_id2[i];
00374       T1[i] = vertices2 + tri_id2[i];
00375     }
00376 
00377     FCL_REAL tmp;
00378     Vec3f tmpv;
00379 
00380     // 6 VF checks
00381     for(int i = 0; i < 3; ++i)
00382     {
00383       if(this->enable_statistics) num_vf_tests++;
00384       if(Intersect::intersect_VF(*(S0[0]), *(S0[1]), *(S0[2]), *(T0[i]), *(S1[0]), *(S1[1]), *(S1[2]), *(T1[i]), &tmp, &tmpv))
00385       {
00386         if(collision_time > tmp)
00387         {
00388           collision_time = tmp; collision_pos = tmpv;
00389         }
00390       }
00391 
00392       if(this->enable_statistics) num_vf_tests++;
00393       if(Intersect::intersect_VF(*(T0[0]), *(T0[1]), *(T0[2]), *(S0[i]), *(T1[0]), *(T1[1]), *(T1[2]), *(S1[i]), &tmp, &tmpv))
00394       {
00395         if(collision_time > tmp)
00396         {
00397           collision_time = tmp; collision_pos = tmpv;
00398         }
00399       }
00400     }
00401 
00402     // 9 EE checks
00403     for(int i = 0; i < 3; ++i)
00404     {
00405       int S_id1 = i;
00406       int S_id2 = i + 1;
00407       if(S_id2 == 3) S_id2 = 0;
00408       for(int j = 0; j < 3; ++j)
00409       {
00410         int T_id1 = j;
00411         int T_id2 = j + 1;
00412         if(T_id2 == 3) T_id2 = 0;
00413 
00414         num_ee_tests++;
00415         if(Intersect::intersect_EE(*(S0[S_id1]), *(S0[S_id2]), *(T0[T_id1]), *(T0[T_id2]), *(S1[S_id1]), *(S1[S_id2]), *(T1[T_id1]), *(T1[T_id2]), &tmp, &tmpv))
00416         {
00417           if(collision_time > tmp)
00418           {
00419             collision_time = tmp; collision_pos = tmpv;
00420           }
00421         }
00422       }
00423     }
00424 
00425     if(!(collision_time > 1)) // collision happens
00426       pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time));
00427   }
00428 
00430   bool canStop() const
00431   {
00432     return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size());
00433   }
00434 
00435   Vec3f* vertices1;
00436   Vec3f* vertices2;
00437 
00438   Triangle* tri_indices1;
00439   Triangle* tri_indices2;
00440 
00441   Vec3f* prev_vertices1;
00442   Vec3f* prev_vertices2;
00443 
00444   mutable int num_vf_tests;
00445   mutable int num_ee_tests;
00446 
00447   mutable std::vector<BVHContinuousCollisionPair> pairs;
00448 };
00449 
00450 
00451 
00453 template<typename BV>
00454 class BVHDistanceTraversalNode : public DistanceTraversalNodeBase
00455 {
00456 public:
00457   BVHDistanceTraversalNode() : DistanceTraversalNodeBase()
00458   {
00459     model1 = NULL;
00460     model2 = NULL;
00461 
00462     num_bv_tests = 0;
00463     num_leaf_tests = 0;
00464     query_time_seconds = 0.0;
00465   }
00466 
00468   bool isFirstNodeLeaf(int b) const
00469   {
00470     return model1->getBV(b).isLeaf();
00471   }
00472 
00474   bool isSecondNodeLeaf(int b) const
00475   {
00476     return model2->getBV(b).isLeaf();
00477   }
00478 
00480   bool firstOverSecond(int b1, int b2) const
00481   {
00482     FCL_REAL sz1 = model1->getBV(b1).bv.size();
00483     FCL_REAL sz2 = model2->getBV(b2).bv.size();
00484 
00485     bool l1 = model1->getBV(b1).isLeaf();
00486     bool l2 = model2->getBV(b2).isLeaf();
00487 
00488     if(l2 || (!l1 && (sz1 > sz2)))
00489       return true;
00490     return false;
00491   }
00492 
00494   int getFirstLeftChild(int b) const
00495   {
00496     return model1->getBV(b).leftChild();
00497   }
00498 
00500   int getFirstRightChild(int b) const
00501   {
00502     return model1->getBV(b).rightChild();
00503   }
00504 
00506   int getSecondLeftChild(int b) const
00507   {
00508     return model2->getBV(b).leftChild();
00509   }
00510 
00512   int getSecondRightChild(int b) const
00513   {
00514     return model2->getBV(b).rightChild();
00515   }
00516 
00518   FCL_REAL BVTesting(int b1, int b2) const
00519   {
00520     if(enable_statistics) num_bv_tests++;
00521     return model1->getBV(b1).distance(model2->getBV(b2));
00522   }
00523 
00525   const BVHModel<BV>* model1;
00527   const BVHModel<BV>* model2;
00528 
00530   mutable int num_bv_tests;
00531   mutable int num_leaf_tests;
00532   mutable FCL_REAL query_time_seconds;
00533 };
00534 
00535 
00537 template<typename BV>
00538 class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV>
00539 {
00540 public:
00541   MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>()
00542   {
00543     vertices1 = NULL;
00544     vertices2 = NULL;
00545     tri_indices1 = NULL;
00546     tri_indices2 = NULL;
00547 
00548     rel_err = 0;
00549     abs_err = 0;
00550   }
00551 
00553   void leafTesting(int b1, int b2) const
00554   {
00555     if(this->enable_statistics) this->num_leaf_tests++;
00556 
00557     const BVNode<BV>& node1 = this->model1->getBV(b1);
00558     const BVNode<BV>& node2 = this->model2->getBV(b2);
00559 
00560     int primitive_id1 = node1.primitiveId();
00561     int primitive_id2 = node2.primitiveId();
00562 
00563     const Triangle& tri_id1 = tri_indices1[primitive_id1];
00564     const Triangle& tri_id2 = tri_indices2[primitive_id2];
00565 
00566     const Vec3f& t11 = vertices1[tri_id1[0]];
00567     const Vec3f& t12 = vertices1[tri_id1[1]];
00568     const Vec3f& t13 = vertices1[tri_id1[2]];
00569 
00570     const Vec3f& t21 = vertices2[tri_id2[0]];
00571     const Vec3f& t22 = vertices2[tri_id2[1]];
00572     const Vec3f& t23 = vertices2[tri_id2[2]];
00573 
00574     // nearest point pair
00575     Vec3f P1, P2;
00576 
00577     FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
00578                                                P1, P2);
00579 
00580     if(this->request.enable_nearest_points)
00581     {
00582       this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2);
00583     }
00584     else
00585     {
00586       this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2);
00587     }
00588   }
00589 
00591   bool canStop(FCL_REAL c) const
00592   {
00593     if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
00594       return true;
00595     return false;
00596   }
00597 
00598   Vec3f* vertices1;
00599   Vec3f* vertices2;
00600 
00601   Triangle* tri_indices1;
00602   Triangle* tri_indices2;
00603 
00605   FCL_REAL rel_err;
00606   FCL_REAL abs_err;
00607 };
00608 
00610 class MeshDistanceTraversalNodeRSS : public MeshDistanceTraversalNode<RSS>
00611 {
00612 public:
00613   MeshDistanceTraversalNodeRSS();
00614 
00615   void preprocess();
00616 
00617   void postprocess();
00618 
00619   FCL_REAL BVTesting(int b1, int b2) const;
00620 
00621   void leafTesting(int b1, int b2) const;
00622 
00623   Matrix3f R;
00624   Vec3f T;
00625 };
00626 
00627 
00628 class MeshDistanceTraversalNodekIOS : public MeshDistanceTraversalNode<kIOS>
00629 {
00630 public:
00631   MeshDistanceTraversalNodekIOS();
00632 
00633   void preprocess();
00634   
00635   void postprocess();
00636 
00637   FCL_REAL BVTesting(int b1, int b2) const;
00638 
00639   void leafTesting(int b1, int b2) const;
00640 
00641   Matrix3f R;
00642   Vec3f T;
00643 };
00644 
00645 class MeshDistanceTraversalNodeOBBRSS : public MeshDistanceTraversalNode<OBBRSS>
00646 {
00647 public:
00648   MeshDistanceTraversalNodeOBBRSS();
00649 
00650   void preprocess();
00651 
00652   void postprocess();
00653 
00654   FCL_REAL BVTesting(int b1, int b2) const;
00655 
00656   void leafTesting(int b1, int b2) const;
00657 
00658   Matrix3f R;
00659   Vec3f T;
00660 };
00661 
00662 
00663 struct ConservativeAdvancementStackData
00664 {
00665   ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
00666 
00667   Vec3f P1;
00668   Vec3f P2;
00669   int c1;
00670   int c2;
00671   FCL_REAL d;
00672 };
00673 
00675 template<typename BV>
00676 class MeshConservativeAdvancementTraversalNode : public MeshDistanceTraversalNode<BV>
00677 {
00678 public:
00679   MeshConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : MeshDistanceTraversalNode<BV>()
00680   {
00681     delta_t = 1;
00682     toc = 0;
00683     t_err = (FCL_REAL)0.00001;
00684 
00685     w = w_;
00686 
00687     motion1 = NULL;
00688     motion2 = NULL;
00689   }
00690 
00692   FCL_REAL BVTesting(int b1, int b2) const
00693   {
00694     if(this->enable_statistics) this->num_bv_tests++;
00695     Vec3f P1, P2;
00696     FCL_REAL d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2);
00697 
00698     stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
00699 
00700     return d;
00701   }
00702 
00704   void leafTesting(int b1, int b2) const
00705   {
00706     if(this->enable_statistics) this->num_leaf_tests++;
00707 
00708     const BVNode<BV>& node1 = this->model1->getBV(b1);
00709     const BVNode<BV>& node2 = this->model2->getBV(b2);
00710 
00711     int primitive_id1 = node1.primitiveId();
00712     int primitive_id2 = node2.primitiveId();
00713 
00714     const Triangle& tri_id1 = this->tri_indices1[primitive_id1];
00715     const Triangle& tri_id2 = this->tri_indices2[primitive_id2];
00716 
00717     const Vec3f& p1 = this->vertices1[tri_id1[0]];
00718     const Vec3f& p2 = this->vertices1[tri_id1[1]];
00719     const Vec3f& p3 = this->vertices1[tri_id1[2]];
00720 
00721     const Vec3f& q1 = this->vertices2[tri_id2[0]];
00722     const Vec3f& q2 = this->vertices2[tri_id2[1]];
00723     const Vec3f& q3 = this->vertices2[tri_id2[2]];
00724 
00725     // nearest point pair
00726     Vec3f P1, P2;
00727 
00728     FCL_REAL d = TriangleDistance::triDistance(p1, p2, p3, q1, q2, q3,
00729                                                P1, P2);
00730 
00731     if(d < this->min_distance)
00732     {
00733       this->min_distance = d;
00734 
00735       this->p1 = P1;
00736       this->p2 = P2;
00737 
00738       this->last_tri_id1 = primitive_id1;
00739       this->last_tri_id2 = primitive_id2;
00740     }
00741 
00742 
00743     // n is the local frame of object 1
00744     Vec3f n = P2 - P1;
00745     // here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH
00746     FCL_REAL bound1 = motion1->computeMotionBound(p1, p2, p3, n);
00747     FCL_REAL bound2 = motion2->computeMotionBound(q1, q2, q3, n);
00748 
00749     FCL_REAL bound = bound1 + bound2;
00750 
00751     FCL_REAL cur_delta_t;
00752     if(bound <= d) cur_delta_t = 1;
00753     else cur_delta_t = d / bound;
00754 
00755     if(cur_delta_t < delta_t)
00756       delta_t = cur_delta_t;
00757   }
00758 
00760   bool canStop(FCL_REAL c) const
00761   {
00762     if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
00763     {
00764       const ConservativeAdvancementStackData& data = stack.back();
00765       FCL_REAL d = data.d;
00766       Vec3f n;
00767       int c1, c2;
00768 
00769       if(d > c)
00770       {
00771         const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
00772         d = data2.d;
00773         n = data2.P2 - data2.P1;
00774         c1 = data2.c1;
00775         c2 = data2.c2;
00776         stack[stack.size() - 2] = stack[stack.size() - 1];
00777       }
00778       else
00779       {
00780         n = data.P2 - data.P1;
00781         c1 = data.c1;
00782         c2 = data.c2;
00783       }
00784 
00785       assert(c == d);
00786 
00787       FCL_REAL bound1 = motion1->computeMotionBound((this->tree1 + c1)->bv, n);
00788       FCL_REAL bound2 = motion2->computeMotionBound((this->tree2 + c2)->bv, n);
00789 
00790       FCL_REAL bound = bound1 + bound2;
00791 
00792       FCL_REAL cur_delta_t;
00793       if(bound <= c) cur_delta_t = 1;
00794       else cur_delta_t = c / bound;
00795 
00796       if(cur_delta_t < delta_t)
00797         delta_t = cur_delta_t;
00798 
00799       stack.pop_back();
00800 
00801       return true;
00802     }
00803     else
00804     {
00805       const ConservativeAdvancementStackData& data = stack.back();
00806       FCL_REAL d = data.d;
00807 
00808       if(d > c)
00809         stack[stack.size() - 2] = stack[stack.size() - 1];
00810 
00811       stack.pop_back();
00812 
00813       return false;
00814     }
00815   }
00816 
00817   mutable FCL_REAL min_distance;
00818 
00819   mutable Vec3f p1, p2;
00820 
00821   mutable int last_tri_id1, last_tri_id2;
00822 
00823 
00825   FCL_REAL w;
00826 
00828   FCL_REAL toc;
00829   FCL_REAL t_err;
00830 
00832   mutable FCL_REAL delta_t;
00833 
00835   MotionBase<BV>* motion1;
00836   MotionBase<BV>* motion2;
00837 
00838   mutable std::vector<ConservativeAdvancementStackData> stack;
00839 };
00840 
00841 
00843 template<>
00844 bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const;
00845 
00846 template<>
00847 bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const;
00848 
00849 
00850 class MeshConservativeAdvancementTraversalNodeRSS : public MeshConservativeAdvancementTraversalNode<RSS>
00851 {
00852 public:
00853   MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1);
00854 
00855   FCL_REAL BVTesting(int b1, int b2) const;
00856 
00857   void leafTesting(int b1, int b2) const;
00858 
00859   bool canStop(FCL_REAL c) const;
00860 
00861   Matrix3f R;
00862   Vec3f T;
00863 };
00864 
00865 }
00866 
00867 
00868 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:31