traversal_node_bvh_shape.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_MESH_SHAPE_H
00039 #define FCL_TRAVERSAL_NODE_MESH_SHAPE_H
00040 
00041 #include "fcl/collision_data.h"
00042 #include "fcl/shape/geometric_shapes.h"
00043 #include "fcl/shape/geometric_shapes_utility.h"
00044 #include "fcl/traversal/traversal_node_base.h"
00045 #include "fcl/BVH/BVH_model.h"
00046 
00047 
00048 namespace fcl
00049 {
00050 
00052 template<typename BV, typename S>
00053 class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase
00054 {
00055 public:
00056   BVHShapeCollisionTraversalNode() : CollisionTraversalNodeBase()
00057   {
00058     model1 = NULL;
00059     model2 = NULL;
00060 
00061     num_bv_tests = 0;
00062     num_leaf_tests = 0;
00063     query_time_seconds = 0.0;
00064   }
00065 
00067   bool isFirstNodeLeaf(int b) const
00068   {
00069     return model1->getBV(b).isLeaf();
00070   }
00071 
00073   int getFirstLeftChild(int b) const
00074   {
00075     return model1->getBV(b).leftChild();
00076   }
00077 
00079   int getFirstRightChild(int b) const
00080   {
00081     return model1->getBV(b).rightChild();
00082   }
00083 
00085   bool BVTesting(int b1, int b2) const
00086   {
00087     if(this->enable_statistics) num_bv_tests++;
00088     return !model1->getBV(b1).bv.overlap(model2_bv);
00089   }
00090 
00091   const BVHModel<BV>* model1;
00092   const S* model2;
00093   BV model2_bv;
00094 
00095   mutable int num_bv_tests;
00096   mutable int num_leaf_tests;
00097   mutable FCL_REAL query_time_seconds;
00098 };
00099 
00101 template<typename S, typename BV>
00102 class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase
00103 {
00104 public:
00105   ShapeBVHCollisionTraversalNode() : CollisionTraversalNodeBase()
00106   {
00107     model1 = NULL;
00108     model2 = NULL;
00109 
00110     num_bv_tests = 0;
00111     num_leaf_tests = 0;
00112     query_time_seconds = 0.0;
00113   }
00114 
00116   bool firstOverSecond(int, int) const
00117   {
00118     return false;
00119   }
00120 
00122   bool isSecondNodeLeaf(int b) const
00123   {
00124     return model2->getBV(b).isLeaf();
00125   }
00126 
00128   int getSecondLeftChild(int b) const
00129   {
00130     return model2->getBV(b).leftChild();
00131   }
00132 
00134   int getSecondRightChild(int b) const
00135   {
00136     return model2->getBV(b).rightChild();
00137   }
00138 
00140   bool BVTesting(int b1, int b2) const
00141   {
00142     if(this->enable_statistics) num_bv_tests++;
00143     return !model2->getBV(b2).bv.overlap(model1_bv);
00144   }
00145 
00146   const S* model1;
00147   const BVHModel<BV>* model2;
00148   BV model1_bv;
00149 
00150   mutable int num_bv_tests;
00151   mutable int num_leaf_tests;
00152   mutable FCL_REAL query_time_seconds;
00153 };
00154 
00155 
00157 template<typename BV, typename S, typename NarrowPhaseSolver>
00158 class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
00159 {
00160 public:
00161   MeshShapeCollisionTraversalNode() : BVHShapeCollisionTraversalNode<BV, S>()
00162   {
00163     vertices = NULL;
00164     tri_indices = NULL;
00165 
00166     nsolver = NULL;
00167   }
00168 
00170   void leafTesting(int b1, int b2) const
00171   {
00172     if(this->enable_statistics) this->num_leaf_tests++;
00173     const BVNode<BV>& node = this->model1->getBV(b1);
00174 
00175     int primitive_id = node.primitiveId();
00176 
00177     const Triangle& tri_id = tri_indices[primitive_id];
00178 
00179     const Vec3f& p1 = vertices[tri_id[0]];
00180     const Vec3f& p2 = vertices[tri_id[1]];
00181     const Vec3f& p3 = vertices[tri_id[2]];
00182 
00183     if(this->model1->isOccupied() && this->model2->isOccupied())
00184     {
00185       bool is_intersect = false;
00186 
00187       if(!this->request.enable_contact)
00188       {
00189         if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
00190         {
00191           is_intersect = true;
00192           if(this->request.num_max_contacts > this->result->numContacts())
00193             this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE));
00194         }
00195       }
00196       else
00197       {
00198         FCL_REAL penetration;
00199         Vec3f normal;
00200         Vec3f contactp;
00201 
00202         if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal))
00203         {
00204           is_intersect = true;
00205           if(this->request.num_max_contacts > this->result->numContacts())
00206             this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
00207         }
00208       }
00209 
00210       if(is_intersect && this->request.enable_cost)
00211       {
00212         AABB overlap_part;
00213         AABB shape_aabb;
00214         computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
00215         AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
00216         this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
00217       }
00218     }
00219     if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
00220     {
00221       if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
00222       {
00223         AABB overlap_part;
00224         AABB shape_aabb;
00225         computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
00226         AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
00227         this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);        
00228       }
00229     }
00230   }
00231 
00233   bool canStop() const
00234   {
00235     return this->request.isSatisfied(*(this->result));
00236   }
00237 
00238   Vec3f* vertices;
00239   Triangle* tri_indices;
00240   
00241   FCL_REAL cost_density;
00242 
00243   const NarrowPhaseSolver* nsolver;
00244 };
00245 
00247 namespace details
00248 {
00249 template<typename BV, typename S, typename NarrowPhaseSolver>
00250 static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
00251                                                              const BVHModel<BV>* model1, const S& model2,
00252                                                              Vec3f* vertices, Triangle* tri_indices,
00253                                                              const Transform3f& tf1,
00254                                                              const Transform3f& tf2, 
00255                                                              const NarrowPhaseSolver* nsolver,
00256                                                              bool enable_statistics, 
00257                                                              FCL_REAL cost_density,
00258                                                              int& num_leaf_tests,
00259                                                              const CollisionRequest& request,
00260                                                              CollisionResult& result)
00261 {
00262   if(enable_statistics) num_leaf_tests++;
00263   const BVNode<BV>& node = model1->getBV(b1);
00264 
00265   int primitive_id = node.primitiveId();
00266 
00267   const Triangle& tri_id = tri_indices[primitive_id];
00268 
00269   const Vec3f& p1 = vertices[tri_id[0]];
00270   const Vec3f& p2 = vertices[tri_id[1]];
00271   const Vec3f& p3 = vertices[tri_id[2]];
00272 
00273   if(model1->isOccupied() && model2.isOccupied())
00274   {
00275     bool is_intersect = false;
00276 
00277     if(!request.enable_contact) // only interested in collision or not
00278     {
00279       if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL))
00280       {
00281         is_intersect = true;
00282         if(request.num_max_contacts > result.numContacts())
00283           result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE));
00284       }
00285     }
00286     else
00287     {
00288       FCL_REAL penetration;
00289       Vec3f normal;
00290       Vec3f contactp;
00291 
00292       if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp, &penetration, &normal))
00293       {
00294         is_intersect = true;
00295         if(request.num_max_contacts > result.numContacts())
00296           result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
00297       }
00298     }
00299 
00300     if(is_intersect && request.enable_cost)
00301     {
00302       AABB overlap_part;
00303       AABB shape_aabb;
00304       computeBV<AABB, S>(model2, tf2, shape_aabb);
00305       bool res = AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(shape_aabb, overlap_part);
00306       result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
00307     }
00308   }
00309   else if((!model1->isFree() || model2.isFree()) && request.enable_cost)
00310   {
00311     if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL))
00312     {
00313       AABB overlap_part;
00314       AABB shape_aabb;
00315       computeBV<AABB, S>(model2, tf2, shape_aabb);
00316       bool res = AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(shape_aabb, overlap_part);
00317       result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);    
00318     }
00319   }
00320 }
00321 
00322 }
00323 
00325 
00326 
00328 template<typename S, typename NarrowPhaseSolver>
00329 class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
00330 {
00331 public:
00332   MeshShapeCollisionTraversalNodeOBB() : MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>()
00333   {
00334   }
00335 
00336   bool BVTesting(int b1, int b2) const
00337   {
00338     if(this->enable_statistics) this->num_bv_tests++;
00339     return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
00340   }
00341 
00342   void leafTesting(int b1, int b2) const
00343   {
00344     details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
00345                                                        this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
00346   }
00347 
00348 };
00349 
00350 template<typename S, typename NarrowPhaseSolver>
00351 class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>
00352 {
00353 public:
00354   MeshShapeCollisionTraversalNodeRSS() : MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>()
00355   {
00356   }
00357 
00358   bool BVTesting(int b1, int b2) const
00359   {
00360     if(this->enable_statistics) this->num_bv_tests++;
00361     return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
00362   }
00363 
00364   void leafTesting(int b1, int b2) const
00365   {
00366     details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
00367                                                        this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
00368   }
00369 
00370 };
00371 
00372 template<typename S, typename NarrowPhaseSolver>
00373 class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>
00374 {
00375 public:
00376   MeshShapeCollisionTraversalNodekIOS() : MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>()
00377   {
00378   }
00379 
00380   bool BVTesting(int b1, int b2) const
00381   {
00382     if(this->enable_statistics) this->num_bv_tests++;
00383     return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
00384   }
00385 
00386   void leafTesting(int b1, int b2) const
00387   {
00388     details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
00389                                                        this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
00390   }
00391 
00392 };
00393 
00394 template<typename S, typename NarrowPhaseSolver>
00395 class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>
00396 {
00397 public:
00398   MeshShapeCollisionTraversalNodeOBBRSS() : MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
00399   {
00400   }
00401 
00402   bool BVTesting(int b1, int b2) const
00403   {
00404     if(this->enable_statistics) this->num_bv_tests++;
00405     return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
00406   }
00407 
00408   void leafTesting(int b1, int b2) const
00409   {
00410     details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
00411                                                        this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
00412   }
00413 
00414 };
00415 
00416 
00418 template<typename S, typename BV, typename NarrowPhaseSolver>
00419 class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode<S, BV>
00420 {
00421 public:
00422   ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode<S, BV>()
00423   {
00424     vertices = NULL;
00425     tri_indices = NULL;
00426 
00427     nsolver = NULL;
00428   }
00429 
00431   void leafTesting(int b1, int b2) const
00432   {
00433     if(this->enable_statistics) this->num_leaf_tests++;
00434     const BVNode<BV>& node = this->model2->getBV(b2);
00435 
00436     int primitive_id = node.primitiveId();
00437 
00438     const Triangle& tri_id = tri_indices[primitive_id];
00439 
00440     const Vec3f& p1 = vertices[tri_id[0]];
00441     const Vec3f& p2 = vertices[tri_id[1]];
00442     const Vec3f& p3 = vertices[tri_id[2]];
00443 
00444     if(this->model1->isOccupied() && this->model2->isOccupied())
00445     {
00446       bool is_intersect = false;
00447 
00448       if(!this->request.enable_contact)
00449       {
00450         if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
00451         {
00452           is_intersect = true;
00453           if(this->request.num_max_contacts > this->result->numContacts())
00454             this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id));
00455         }
00456       }
00457       else
00458       {
00459         FCL_REAL penetration;
00460         Vec3f normal;
00461         Vec3f contactp;
00462 
00463         if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
00464         {
00465           is_intersect = true;
00466           if(this->request.num_max_contacts > this->result->numContacts())
00467             this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration));
00468         }
00469       }
00470 
00471       if(is_intersect && this->request.enable_cost)
00472       {
00473         AABB overlap_part;
00474         AABB shape_aabb;
00475         computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
00476         AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
00477         this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
00478       }
00479     }
00480     else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
00481     {
00482       if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
00483       {
00484         AABB overlap_part;
00485         AABB shape_aabb;
00486         computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
00487         AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
00488         this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
00489       }   
00490     }
00491   }
00492 
00494   bool canStop() const
00495   {
00496     return this->request.isSatisfied(*(this->result));
00497   }
00498 
00499   Vec3f* vertices;
00500   Triangle* tri_indices;
00501 
00502   FCL_REAL cost_density;
00503 
00504   const NarrowPhaseSolver* nsolver;
00505 };
00506 
00508 template<typename S, typename NarrowPhaseSolver>
00509 class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode<S, OBB, NarrowPhaseSolver>
00510 {
00511 public:
00512   ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode<S, OBB, NarrowPhaseSolver>()
00513   {
00514   }
00515 
00516   bool BVTesting(int b1, int b2) const
00517   {
00518     if(this->enable_statistics) this->num_bv_tests++;
00519     return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
00520   }
00521 
00522   void leafTesting(int b1, int b2) const
00523   {
00524     details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 
00525                                                        this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
00526 
00527     // may need to change the order in pairs
00528   }
00529 };
00530 
00531 
00532 template<typename S, typename NarrowPhaseSolver>
00533 class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNode<S, RSS, NarrowPhaseSolver>
00534 {
00535 public:
00536   ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode<S, RSS, NarrowPhaseSolver>()
00537   {
00538   }
00539 
00540   bool BVTesting(int b1, int b2) const
00541   {
00542     if(this->enable_statistics) this->num_bv_tests++;
00543     return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
00544   }
00545 
00546   void leafTesting(int b1, int b2) const
00547   {
00548     details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 
00549                                                        this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
00550 
00551     // may need to change the order in pairs
00552   }
00553 
00554 };
00555 
00556 
00557 template<typename S, typename NarrowPhaseSolver>
00558 class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNode<S, kIOS, NarrowPhaseSolver>
00559 {
00560 public:
00561   ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode<S, kIOS, NarrowPhaseSolver>()
00562   {
00563   }
00564 
00565   bool BVTesting(int b1, int b2) const
00566   {
00567     if(this->enable_statistics) this->num_bv_tests++;
00568     return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
00569   }
00570 
00571   void leafTesting(int b1, int b2) const
00572   {
00573     details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 
00574                                                        this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
00575 
00576     // may need to change the order in pairs
00577   }
00578 
00579 };
00580 
00581 
00582 template<typename S, typename NarrowPhaseSolver>
00583 class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversalNode<S, OBBRSS, NarrowPhaseSolver>
00584 {
00585 public:
00586   ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
00587   {
00588   }
00589 
00590   bool BVTesting(int b1, int b2) const
00591   {
00592     if(this->enable_statistics) this->num_bv_tests++;
00593     return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
00594   }
00595 
00596   void leafTesting(int b1, int b2) const
00597   {
00598     details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 
00599                                                        this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
00600 
00601     // may need to change the order in pairs
00602   }
00603 
00604 };
00605 
00607 template<typename BV, typename S>
00608 class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase
00609 {
00610 public:
00611   BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase()
00612   {
00613     model1 = NULL;
00614     model2 = NULL;
00615     
00616     num_bv_tests = 0;
00617     num_leaf_tests = 0;
00618     query_time_seconds = 0.0;
00619   }
00620 
00622   bool isFirstNodeLeaf(int b) const 
00623   {
00624     return model1->getBV(b).isLeaf();
00625   }
00626 
00628   int getFirstLeftChild(int b) const
00629   {
00630     return model1->getBV(b).leftChild();
00631   }
00632 
00634   int getFirstRightChild(int b) const
00635   {
00636     return model1->getBV(b).rightChild();
00637   }
00638 
00640   FCL_REAL BVTesting(int b1, int b2) const
00641   {
00642     return model1->getBV(b1).bv.distance(model2_bv);
00643   }
00644 
00645   const BVHModel<BV>* model1;
00646   const S* model2;
00647   BV model2_bv;
00648 
00649   mutable int num_bv_tests;
00650   mutable int num_leaf_tests;
00651   mutable FCL_REAL query_time_seconds;
00652 };
00653 
00655 template<typename S, typename BV>
00656 class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase
00657 {
00658 public:
00659   ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase()
00660   {
00661     model1 = NULL;
00662     model2 = NULL;
00663     
00664     num_bv_tests = 0;
00665     num_leaf_tests = 0;
00666     query_time_seconds = 0.0;
00667   }
00668 
00670   bool isSecondNodeLeaf(int b) const
00671   {
00672     return model2->getBV(b).isLeaf();
00673   }
00674 
00676   int getSecondLeftChild(int b) const
00677   {
00678     return model2->getBV(b).leftChild();
00679   }
00680 
00682   int getSecondRightChild(int b) const
00683   {
00684     return model2->getBV(b).rightChild();
00685   }
00686 
00688   FCL_REAL BVTesting(int b1, int b2) const
00689   {
00690     return model1_bv.distance(model2->getBV(b2).bv);
00691   }
00692 
00693   const S* model1;
00694   const BVHModel<BV>* model2;
00695   BV model1_bv;
00696   
00697   mutable int num_bv_tests;
00698   mutable int num_leaf_tests;
00699   mutable FCL_REAL query_time_seconds;
00700 };
00701                                   
00702 
00704 template<typename BV, typename S, typename NarrowPhaseSolver>
00705 class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode<BV, S>
00706 { 
00707 public:
00708   MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>()
00709   {
00710     vertices = NULL;
00711     tri_indices = NULL;
00712 
00713     rel_err = 0;
00714     abs_err = 0;
00715 
00716     nsolver = NULL;
00717   }
00718 
00720   void leafTesting(int b1, int b2) const
00721   {
00722     if(this->enable_statistics) this->num_leaf_tests++;
00723     
00724     const BVNode<BV>& node = this->model1->getBV(b1);
00725     
00726     int primitive_id = node.primitiveId();
00727     
00728     const Triangle& tri_id = tri_indices[primitive_id];
00729 
00730     const Vec3f& p1 = vertices[tri_id[0]];
00731     const Vec3f& p2 = vertices[tri_id[1]];
00732     const Vec3f& p3 = vertices[tri_id[2]];
00733     
00734     FCL_REAL d;
00735     nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d);
00736 
00737     this->result->update(d, this->model1, this->model2, primitive_id, DistanceResult::NONE);
00738   }
00739 
00741   bool canStop(FCL_REAL c) const
00742   {
00743     if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
00744       return true;
00745     return false;
00746   }
00747 
00748   Vec3f* vertices;
00749   Triangle* tri_indices;
00750 
00751   FCL_REAL rel_err;
00752   FCL_REAL abs_err;
00753     
00754   const NarrowPhaseSolver* nsolver;
00755 };
00756 
00758 namespace details
00759 {
00760 
00761 template<typename BV, typename S, typename NarrowPhaseSolver>
00762 void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
00763                                               const BVHModel<BV>* model1, const S& model2,
00764                                               Vec3f* vertices, Triangle* tri_indices,
00765                                               const Transform3f& tf1,
00766                                               const Transform3f& tf2,
00767                                               const NarrowPhaseSolver* nsolver,
00768                                               bool enable_statistics,
00769                                               int & num_leaf_tests,
00770                                               const DistanceRequest& request,
00771                                               DistanceResult& result)
00772 {
00773   if(enable_statistics) num_leaf_tests++;
00774     
00775   const BVNode<BV>& node = model1->getBV(b1);
00776   int primitive_id = node.primitiveId();
00777 
00778   const Triangle& tri_id = tri_indices[primitive_id];
00779   const Vec3f& p1 = vertices[tri_id[0]];
00780   const Vec3f& p2 = vertices[tri_id[1]];
00781   const Vec3f& p3 = vertices[tri_id[2]];
00782     
00783   FCL_REAL distance;
00784   nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance);
00785 
00786   result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE);
00787 }
00788 
00789 
00790 template<typename BV, typename S, typename NarrowPhaseSolver>
00791 static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
00792                                                   Vec3f* vertices, Triangle* tri_indices, int init_tri_id,
00793                                                   const S& model2, const Transform3f& tf1, const Transform3f& tf2,
00794                                                   const NarrowPhaseSolver* nsolver,
00795                                                   const DistanceRequest& request,
00796                                                   DistanceResult& result)
00797 {
00798   const Triangle& init_tri = tri_indices[init_tri_id];
00799   
00800   const Vec3f& p1 = vertices[init_tri[0]];
00801   const Vec3f& p2 = vertices[init_tri[1]];
00802   const Vec3f& p3 = vertices[init_tri[2]];
00803   
00804   FCL_REAL distance;
00805   nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance);
00806 
00807   result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE);
00808 }
00809 
00810 
00811 }
00812 
00814 
00815 
00816 
00818 template<typename S, typename NarrowPhaseSolver>
00819 class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode<RSS, S, NarrowPhaseSolver>
00820 {
00821 public:
00822   MeshShapeDistanceTraversalNodeRSS() : MeshShapeDistanceTraversalNode<RSS, S, NarrowPhaseSolver>()
00823   {
00824   }
00825 
00826   void preprocess()
00827   {
00828     details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, 
00829                                             *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
00830   }
00831 
00832   void postprocess()
00833   {
00834   }
00835 
00836   FCL_REAL BVTesting(int b1, int b2) const
00837   {
00838     if(this->enable_statistics) this->num_bv_tests++;
00839     return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
00840   }
00841 
00842   void leafTesting(int b1, int b2) const
00843   {
00844     details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
00845                                                       this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
00846   }
00847 };
00848 
00849 
00850 template<typename S, typename NarrowPhaseSolver>
00851 class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode<kIOS, S, NarrowPhaseSolver>
00852 {
00853 public:
00854   MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode<kIOS, S, NarrowPhaseSolver>()
00855   {
00856   }
00857 
00858   void preprocess()
00859   {
00860     details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, 
00861                                             *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
00862   }
00863 
00864   void postprocess()
00865   {    
00866   }
00867 
00868   FCL_REAL BVTesting(int b1, int b2) const
00869   {
00870     if(this->enable_statistics) this->num_bv_tests++;
00871     return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
00872   }
00873 
00874   void leafTesting(int b1, int b2) const
00875   {
00876     details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
00877                                                       this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
00878   }
00879 
00880 };
00881 
00882 template<typename S, typename NarrowPhaseSolver>
00883 class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>
00884 {
00885 public:
00886   MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
00887   {
00888   }
00889 
00890   void preprocess()
00891   {
00892     details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, 
00893                                             *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
00894   }
00895 
00896   void postprocess()
00897   {
00898     
00899   }
00900 
00901   FCL_REAL BVTesting(int b1, int b2) const
00902   {
00903     if(this->enable_statistics) this->num_bv_tests++;
00904     return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
00905   }
00906 
00907   void leafTesting(int b1, int b2) const
00908   {
00909     details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
00910                                                       this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
00911   }
00912   
00913 };
00914 
00916 template<typename S, typename BV, typename NarrowPhaseSolver>
00917 class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode<S, BV>
00918 { 
00919 public:
00920   ShapeMeshDistanceTraversalNode() : ShapeBVHDistanceTraversalNode<S, BV>()
00921   {
00922     vertices = NULL;
00923     tri_indices = NULL;
00924 
00925     rel_err = 0;
00926     abs_err = 0;
00927 
00928     nsolver = NULL;
00929   }
00930 
00932   void leafTesting(int b1, int b2) const
00933   {
00934     if(this->enable_statistics) this->num_leaf_tests++;
00935     
00936     const BVNode<BV>& node = this->model2->getBV(b2);
00937     
00938     int primitive_id = node.primitiveId();
00939     
00940     const Triangle& tri_id = tri_indices[primitive_id];
00941 
00942     const Vec3f& p1 = vertices[tri_id[0]];
00943     const Vec3f& p2 = vertices[tri_id[1]];
00944     const Vec3f& p3 = vertices[tri_id[2]];
00945     
00946     FCL_REAL distance;
00947     nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &distance);
00948 
00949     this->result->update(distance, this->model1, this->model2, DistanceResult::NONE, primitive_id);
00950   }
00951 
00953   bool canStop(FCL_REAL c) const
00954   {
00955     if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
00956       return true;
00957     return false;
00958   }
00959 
00960   Vec3f* vertices;
00961   Triangle* tri_indices;
00962 
00963   FCL_REAL rel_err;
00964   FCL_REAL abs_err;
00965     
00966   const NarrowPhaseSolver* nsolver;
00967 };
00968 
00969 template<typename S, typename NarrowPhaseSolver>
00970 class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>
00971 {
00972 public:
00973   ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>()
00974   {
00975   }
00976 
00977   void preprocess()
00978   {
00979     details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
00980                                             *(this->model1), this->tf2, this->tf1, this->nsolver, this->request, *(this->result));
00981   }
00982 
00983   void postprocess()
00984   {
00985   }
00986 
00987   FCL_REAL BVTesting(int b1, int b2) const
00988   {
00989     if(this->enable_statistics) this->num_bv_tests++;
00990     return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
00991   }
00992 
00993   void leafTesting(int b1, int b2) const
00994   {
00995     details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
00996                                                       this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
00997   }
00998 
00999 };
01000 
01001 template<typename S, typename NarrowPhaseSolver>
01002 class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>
01003 {
01004 public:
01005   ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>()
01006   {
01007   }
01008 
01009   void preprocess()
01010   {
01011     details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
01012                                             *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result));
01013   }
01014 
01015   void postprocess()
01016   {
01017   }
01018 
01019   FCL_REAL BVTesting(int b1, int b2) const
01020   {
01021     if(this->enable_statistics) this->num_bv_tests++;
01022     return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
01023   }
01024 
01025   void leafTesting(int b1, int b2) const
01026   {
01027     details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
01028                                                       this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
01029   }
01030   
01031 };
01032 
01033 template<typename S, typename NarrowPhaseSolver>
01034 class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>
01035 {
01036 public:
01037   ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
01038   {
01039   }
01040 
01041   void preprocess()
01042   {
01043     details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
01044                                             *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result));
01045   }
01046 
01047   void postprocess()
01048   {    
01049   }
01050 
01051   FCL_REAL BVTesting(int b1, int b2) const
01052   {
01053     if(this->enable_statistics) this->num_bv_tests++;
01054     return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
01055   }
01056 
01057   void leafTesting(int b1, int b2) const
01058   {
01059     details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
01060                                                       this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
01061   }
01062   
01063 };
01064 }
01065 
01066 #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