fcl: traversal_node_setup.h Source File

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_SETUP_H
00039 #define FCL_TRAVERSAL_NODE_SETUP_H
00040 
00041 #include "fcl/config.h"
00042 #include "fcl/traversal/traversal_node_bvhs.h"
00043 #include "fcl/traversal/traversal_node_shapes.h"
00044 #include "fcl/traversal/traversal_node_bvh_shape.h"
00045 
00046 #if FCL_HAVE_OCTOMAP
00047 #include "fcl/traversal/traversal_node_octree.h"
00048 #endif
00049 
00050 #include "fcl/BVH/BVH_utility.h"
00051 
00052 namespace fcl
00053 {
00054 
00055 #if FCL_HAVE_OCTOMAP
00056 
00057 template<typename NarrowPhaseSolver>
00058 bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
00059                 const OcTree& model1, const Transform3f& tf1,
00060                 const OcTree& model2, const Transform3f& tf2,
00061                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
00062                 const CollisionRequest& request,
00063                 CollisionResult& result)
00064 {
00065   node.request = request;
00066   node.result = &result;
00067 
00068   node.model1 = &model1;
00069   node.model2 = &model2;
00070   
00071   node.otsolver = otsolver;
00072 
00073   node.tf1 = tf1;
00074   node.tf2 = tf2;
00075 
00076   return true;
00077 }
00078 
00080 template<typename NarrowPhaseSolver>
00081 bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
00082                 const OcTree& model1, const Transform3f& tf1,
00083                 const OcTree& model2, const Transform3f& tf2,
00084                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
00085                 const DistanceRequest& request,
00086                 DistanceResult& result)
00087 {
00088   node.request = request;
00089   node.result = &result;
00090  
00091   node.model1 = &model1;
00092   node.model2 = &model2;
00093   
00094   node.otsolver = otsolver;
00095   
00096   node.tf1 = tf1;
00097   node.tf2 = tf2;
00098 
00099   return true;
00100 }
00101 
00103 template<typename S, typename NarrowPhaseSolver>
00104 bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
00105                 const S& model1, const Transform3f& tf1,
00106                 const OcTree& model2, const Transform3f& tf2,
00107                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
00108                 const CollisionRequest& request,
00109                 CollisionResult& result)
00110 {
00111   node.request = request;
00112   node.result = &result;
00113 
00114   node.model1 = &model1;
00115   node.model2 = &model2;
00116 
00117   node.otsolver = otsolver;
00118 
00119   node.tf1 = tf1;
00120   node.tf2 = tf2;
00121 
00122   return true;
00123 }
00124 
00126 template<typename S, typename NarrowPhaseSolver>
00127 bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
00128                 const OcTree& model1, const Transform3f& tf1,
00129                 const S& model2, const Transform3f& tf2,
00130                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
00131                 const CollisionRequest& request,
00132                 CollisionResult& result)
00133 {
00134   node.request = request;
00135   node.result = &result;
00136 
00137   node.model1 = &model1;
00138   node.model2 = &model2;
00139 
00140   node.otsolver = otsolver;
00141 
00142   node.tf1 = tf1;
00143   node.tf2 = tf2;
00144 
00145   return true;
00146 }
00147 
00149 template<typename S, typename NarrowPhaseSolver>
00150 bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
00151                 const S& model1, const Transform3f& tf1,
00152                 const OcTree& model2, const Transform3f& tf2,
00153                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
00154                 const DistanceRequest& request,
00155                 DistanceResult& result)
00156 {
00157   node.request = request;
00158   node.result = &result;
00159 
00160   node.model1 = &model1;
00161   node.model2 = &model2;
00162 
00163   node.otsolver = otsolver;
00164 
00165   node.tf1 = tf1;
00166   node.tf2 = tf2;
00167 
00168   return true;
00169 }
00170 
00172 template<typename S, typename NarrowPhaseSolver>
00173 bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
00174                 const OcTree& model1, const Transform3f& tf1,
00175                 const S& model2, const Transform3f& tf2,
00176                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
00177                 const DistanceRequest& request,
00178                 DistanceResult& result)
00179 {
00180   node.request = request;
00181   node.result = &result;
00182 
00183   node.model1 = &model1;
00184   node.model2 = &model2;
00185 
00186   node.otsolver = otsolver;
00187 
00188   node.tf1 = tf1;
00189   node.tf2 = tf2;
00190 
00191   return true;
00192 }
00193 
00195 template<typename BV, typename NarrowPhaseSolver>
00196 bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
00197                 const BVHModel<BV>& model1, const Transform3f& tf1,
00198                 const OcTree& model2, const Transform3f& tf2,
00199                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
00200                 const CollisionRequest& request,
00201                 CollisionResult& result)
00202 {
00203   node.request = request;
00204   node.result = &result;
00205 
00206   node.model1 = &model1;
00207   node.model2 = &model2;
00208 
00209   node.otsolver = otsolver;
00210 
00211   node.tf1 = tf1;
00212   node.tf2 = tf2;
00213 
00214   return true;
00215 }
00216 
00218 template<typename BV, typename NarrowPhaseSolver>
00219 bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
00220                 const OcTree& model1, const Transform3f& tf1,
00221                 const BVHModel<BV>& model2, const Transform3f& tf2,
00222                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
00223                 const CollisionRequest& request,
00224                 CollisionResult& result)
00225 {
00226   node.request = request;
00227   node.result = &result;
00228 
00229   node.model1 = &model1;
00230   node.model2 = &model2;
00231 
00232   node.otsolver = otsolver;
00233 
00234   node.tf1 = tf1;
00235   node.tf2 = tf2;
00236 
00237   return true;
00238 }
00239 
00241 template<typename BV, typename NarrowPhaseSolver>
00242 bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
00243                 const BVHModel<BV>& model1, const Transform3f& tf1,
00244                 const OcTree& model2, const Transform3f& tf2,
00245                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
00246                 const DistanceRequest& request,
00247                 DistanceResult& result)
00248 {
00249   node.request = request;
00250   node.result = &result;
00251 
00252   node.model1 = &model1;
00253   node.model2 = &model2;
00254   
00255   node.otsolver = otsolver;
00256 
00257   node.tf1 = tf1;
00258   node.tf2 = tf2;
00259 
00260   return true;
00261 }
00262 
00264 template<typename BV, typename NarrowPhaseSolver>
00265 bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
00266                 const OcTree& model1, const Transform3f& tf1,
00267                 const BVHModel<BV>& model2, const Transform3f& tf2,
00268                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
00269                 const DistanceRequest& request,
00270                 DistanceResult& result)
00271 {
00272   node.request = request;
00273   node.result = &result;
00274 
00275   node.model1 = &model1;
00276   node.model2 = &model2;
00277   
00278   node.otsolver = otsolver;
00279 
00280   node.tf1 = tf1;
00281   node.tf2 = tf2;
00282 
00283   return true;
00284 }
00285 
00286 #endif
00287 
00288 
00290 template<typename S1, typename S2, typename NarrowPhaseSolver>
00291 bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
00292                 const S1& shape1, const Transform3f& tf1,
00293                 const S2& shape2, const Transform3f& tf2,
00294                 const NarrowPhaseSolver* nsolver,
00295                 const CollisionRequest& request,
00296                 CollisionResult& result)
00297 {
00298   node.model1 = &shape1;
00299   node.tf1 = tf1;
00300   node.model2 = &shape2;
00301   node.tf2 = tf2;
00302   node.nsolver = nsolver;
00303 
00304   node.request = request;
00305   node.result = &result;
00306   
00307   node.cost_density = shape1.cost_density * shape2.cost_density;
00308 
00309   return true;
00310 }
00311 
00313 template<typename BV, typename S, typename NarrowPhaseSolver>
00314 bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
00315                 BVHModel<BV>& model1, Transform3f& tf1,
00316                 const S& model2, const Transform3f& tf2,
00317                 const NarrowPhaseSolver* nsolver,
00318                 const CollisionRequest& request,
00319                 CollisionResult& result,
00320                 bool use_refit = false, bool refit_bottomup = false)
00321 {
00322   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
00323     return false;
00324 
00325   if(!tf1.isIdentity())
00326   {
00327     std::vector<Vec3f> vertices_transformed(model1.num_vertices);
00328     for(int i = 0; i < model1.num_vertices; ++i)
00329     {
00330       Vec3f& p = model1.vertices[i];
00331       Vec3f new_v = tf1.transform(p);
00332       vertices_transformed[i] = new_v;
00333     }
00334 
00335     model1.beginReplaceModel();
00336     model1.replaceSubModel(vertices_transformed);
00337     model1.endReplaceModel(use_refit, refit_bottomup);
00338 
00339     tf1.setIdentity();
00340   }
00341 
00342   node.model1 = &model1;
00343   node.tf1 = tf1;
00344   node.model2 = &model2;
00345   node.tf2 = tf2;
00346   node.nsolver = nsolver;
00347 
00348   computeBV(model2, tf2, node.model2_bv);
00349 
00350   node.vertices = model1.vertices;
00351   node.tri_indices = model1.tri_indices;
00352 
00353   node.request = request;
00354   node.result = &result;
00355 
00356   node.cost_density = model1.cost_density * model2.cost_density;
00357 
00358   return true;
00359 }
00360 
00361 
00363 template<typename S, typename BV, typename NarrowPhaseSolver>
00364 bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
00365                 const S& model1, const Transform3f& tf1,
00366                 BVHModel<BV>& model2, Transform3f& tf2,
00367                 const NarrowPhaseSolver* nsolver,
00368                 const CollisionRequest& request,
00369                 CollisionResult& result,
00370                 bool use_refit = false, bool refit_bottomup = false)
00371 {
00372   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
00373     return false;
00374 
00375   if(!tf2.isIdentity())
00376   {
00377     std::vector<Vec3f> vertices_transformed(model2.num_vertices);
00378     for(int i = 0; i < model2.num_vertices; ++i)
00379     {
00380       Vec3f& p = model2.vertices[i];
00381       Vec3f new_v = tf2.transform(p);
00382       vertices_transformed[i] = new_v;
00383     }
00384 
00385     model2.beginReplaceModel();
00386     model2.replaceSubModel(vertices_transformed);
00387     model2.endReplaceModel(use_refit, refit_bottomup);
00388 
00389     tf2.setIdentity();
00390   }
00391 
00392   node.model1 = &model1;
00393   node.tf1 = tf1;
00394   node.model2 = &model2;
00395   node.tf2 = tf2;
00396   node.nsolver = nsolver;
00397 
00398   computeBV(model1, tf1, node.model1_bv);
00399 
00400   node.vertices = model2.vertices;
00401   node.tri_indices = model2.tri_indices;
00402 
00403   node.request = request;
00404   node.result = &result;
00405 
00406   node.cost_density = model1.cost_density * model2.cost_density;
00407 
00408   return true;
00409 }
00410 
00412 namespace details
00413 {
00414 
00415 template<typename BV, typename S, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
00416 static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node, 
00417                                                        const BVHModel<BV>& model1, const Transform3f& tf1,
00418                                                        const S& model2, const Transform3f& tf2,
00419                                                        const NarrowPhaseSolver* nsolver,
00420                                                        const CollisionRequest& request,
00421                                                        CollisionResult& result)
00422 {
00423   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
00424     return false;
00425 
00426   node.model1 = &model1;
00427   node.tf1 = tf1;
00428   node.model2 = &model2;
00429   node.tf2 = tf2;
00430   node.nsolver = nsolver;
00431 
00432   computeBV(model2, tf2, node.model2_bv);
00433 
00434   node.vertices = model1.vertices;
00435   node.tri_indices = model1.tri_indices;
00436 
00437   node.request = request;
00438   node.result = &result;
00439 
00440   node.cost_density = model1.cost_density * model2.cost_density;
00441 
00442   return true;
00443 }
00444 
00445 }
00447 
00448 
00449 
00451 template<typename S, typename NarrowPhaseSolver>
00452 bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
00453                 const BVHModel<OBB>& model1, const Transform3f& tf1,
00454                 const S& model2, const Transform3f& tf2,
00455                 const NarrowPhaseSolver* nsolver,
00456                 const CollisionRequest& request,
00457                 CollisionResult& result)
00458 {
00459   return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
00460 }
00461 
00463 template<typename S, typename NarrowPhaseSolver>
00464 bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
00465                 const BVHModel<RSS>& model1, const Transform3f& tf1,
00466                 const S& model2, const Transform3f& tf2,
00467                 const NarrowPhaseSolver* nsolver,
00468                 const CollisionRequest& request, 
00469                 CollisionResult& result)
00470 {
00471   return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
00472 }
00473 
00475 template<typename S, typename NarrowPhaseSolver>
00476 bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
00477                 const BVHModel<kIOS>& model1, const Transform3f& tf1,
00478                 const S& model2, const Transform3f& tf2,
00479                 const NarrowPhaseSolver* nsolver,
00480                 const CollisionRequest& request,
00481                 CollisionResult& result)
00482 {
00483   return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
00484 }
00485 
00487 template<typename S, typename NarrowPhaseSolver>
00488 bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
00489                 const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
00490                 const S& model2, const Transform3f& tf2,
00491                 const NarrowPhaseSolver* nsolver,
00492                 const CollisionRequest& request,
00493                 CollisionResult& result)
00494 {
00495   return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
00496 }
00497 
00498 
00500 namespace details
00501 {
00502 template<typename S, typename BV, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
00503 static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node, 
00504                                                        const S& model1, const Transform3f& tf1,
00505                                                        const BVHModel<BV>& model2, const Transform3f& tf2,
00506                                                        const NarrowPhaseSolver* nsolver,
00507                                                        const CollisionRequest& request,
00508                                                        CollisionResult& result)
00509 {
00510   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
00511     return false;
00512 
00513   node.model1 = &model1;
00514   node.tf1 = tf1;
00515   node.model2 = &model2;
00516   node.tf2 = tf2;
00517   node.nsolver = nsolver;
00518 
00519   computeBV(model1, tf1, node.model1_bv);
00520 
00521   node.vertices = model2.vertices;
00522   node.tri_indices = model2.tri_indices;
00523 
00524   node.request = request;
00525   node.result = &result;
00526 
00527   node.cost_density = model1.cost_density * model2.cost_density;
00528 
00529   return true;
00530 }
00531 }
00533 
00535 template<typename S, typename NarrowPhaseSolver>
00536 bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
00537                 const S& model1, const Transform3f& tf1,
00538                 const BVHModel<OBB>& model2, const Transform3f& tf2,
00539                 const NarrowPhaseSolver* nsolver,
00540                 const CollisionRequest& request,
00541                 CollisionResult& result)
00542 {
00543   return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
00544 }
00545 
00547 template<typename S, typename NarrowPhaseSolver>
00548 bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
00549                 const S& model1, const Transform3f& tf1,
00550                 const BVHModel<RSS>& model2, const Transform3f& tf2,
00551                 const NarrowPhaseSolver* nsolver,
00552                 const CollisionRequest& request,
00553                 CollisionResult& result)
00554 {
00555   return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
00556 }
00557 
00559 template<typename S, typename NarrowPhaseSolver>
00560 bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
00561                 const S& model1, const Transform3f& tf1,
00562                 const BVHModel<kIOS>& model2, const Transform3f& tf2,
00563                 const NarrowPhaseSolver* nsolver,
00564                 const CollisionRequest& request,
00565                 CollisionResult& result)
00566 {
00567   return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
00568 }
00569 
00571 template<typename S, typename NarrowPhaseSolver>
00572 bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
00573                 const S& model1, const Transform3f& tf1,
00574                 const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
00575                 const NarrowPhaseSolver* nsolver,
00576                 const CollisionRequest& request,
00577                 CollisionResult& result)
00578 {
00579   return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
00580 }
00581 
00582 
00583 
00584 
00586 template<typename BV>
00587 bool initialize(MeshCollisionTraversalNode<BV>& node,
00588                 BVHModel<BV>& model1, Transform3f& tf1,
00589                 BVHModel<BV>& model2, Transform3f& tf2,
00590                 const CollisionRequest& request,
00591                 CollisionResult& result,
00592                 bool use_refit = false, bool refit_bottomup = false)
00593 {
00594   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
00595     return false;
00596 
00597   if(!tf1.isIdentity())
00598   {
00599     std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
00600     for(int i = 0; i < model1.num_vertices; ++i)
00601     {
00602       Vec3f& p = model1.vertices[i];
00603       Vec3f new_v = tf1.transform(p);
00604       vertices_transformed1[i] = new_v;
00605     }
00606 
00607     model1.beginReplaceModel();
00608     model1.replaceSubModel(vertices_transformed1);
00609     model1.endReplaceModel(use_refit, refit_bottomup);
00610 
00611     tf1.setIdentity();
00612   }
00613 
00614   if(!tf2.isIdentity())
00615   {
00616     std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
00617     for(int i = 0; i < model2.num_vertices; ++i)
00618     {
00619       Vec3f& p = model2.vertices[i];
00620       Vec3f new_v = tf2.transform(p);
00621       vertices_transformed2[i] = new_v;
00622     }
00623 
00624     model2.beginReplaceModel();
00625     model2.replaceSubModel(vertices_transformed2);
00626     model2.endReplaceModel(use_refit, refit_bottomup);
00627 
00628     tf2.setIdentity();
00629   }
00630 
00631   node.model1 = &model1;
00632   node.tf1 = tf1;
00633   node.model2 = &model2;
00634   node.tf2 = tf2;
00635 
00636   node.vertices1 = model1.vertices;
00637   node.vertices2 = model2.vertices;
00638 
00639   node.tri_indices1 = model1.tri_indices;
00640   node.tri_indices2 = model2.tri_indices;
00641 
00642   node.request = request;
00643   node.result = &result;
00644 
00645   node.cost_density = model1.cost_density * model2.cost_density;
00646 
00647   return true;
00648 }
00649 
00650 
00652 bool initialize(MeshCollisionTraversalNodeOBB& node,
00653                 const BVHModel<OBB>& model1, const Transform3f& tf1,
00654                 const BVHModel<OBB>& model2, const Transform3f& tf2,
00655                 const CollisionRequest& request, CollisionResult& result);
00656 
00658 bool initialize(MeshCollisionTraversalNodeRSS& node,
00659                 const BVHModel<RSS>& model1, const Transform3f& tf1,
00660                 const BVHModel<RSS>& model2, const Transform3f& tf2,
00661                 const CollisionRequest& request, CollisionResult& result);
00662 
00664 bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
00665                 const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
00666                 const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
00667                 const CollisionRequest& request, CollisionResult& result);
00668 
00670 bool initialize(MeshCollisionTraversalNodekIOS& node,
00671                 const BVHModel<kIOS>& model1, const Transform3f& tf1,
00672                 const BVHModel<kIOS>& model2, const Transform3f& tf2,
00673                 const CollisionRequest& request, CollisionResult& result);
00674 
00675 
00677 template<typename S1, typename S2, typename NarrowPhaseSolver>
00678 bool initialize(ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>& node,
00679                 const S1& shape1, const Transform3f& tf1,
00680                 const S2& shape2, const Transform3f& tf2,
00681                 const NarrowPhaseSolver* nsolver,
00682                 const DistanceRequest& request,
00683                 DistanceResult& result)
00684 {
00685   node.request = request;
00686   node.result = &result;
00687 
00688   node.model1 = &shape1;
00689   node.tf1 = tf1;
00690   node.model2 = &shape2;
00691   node.tf2 = tf2;
00692   node.nsolver = nsolver;
00693 
00694   return true;
00695 }
00696 
00698 template<typename BV>
00699 bool initialize(MeshDistanceTraversalNode<BV>& node,
00700                 BVHModel<BV>& model1, Transform3f& tf1,
00701                 BVHModel<BV>& model2, Transform3f& tf2,
00702                 const DistanceRequest& request,
00703                 DistanceResult& result,
00704                 bool use_refit = false, bool refit_bottomup = false)
00705 {
00706   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
00707     return false;
00708 
00709   if(!tf1.isIdentity())
00710   {
00711     std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
00712     for(int i = 0; i < model1.num_vertices; ++i)
00713     {
00714       Vec3f& p = model1.vertices[i];
00715       Vec3f new_v = tf1.transform(p);
00716       vertices_transformed1[i] = new_v;
00717     }
00718 
00719     model1.beginReplaceModel();
00720     model1.replaceSubModel(vertices_transformed1);
00721     model1.endReplaceModel(use_refit, refit_bottomup);
00722 
00723     tf1.setIdentity();
00724   }
00725 
00726   if(!tf2.isIdentity())
00727   {
00728     std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
00729     for(int i = 0; i < model2.num_vertices; ++i)
00730     {
00731       Vec3f& p = model2.vertices[i];
00732       Vec3f new_v = tf2.transform(p);
00733       vertices_transformed2[i] = new_v;
00734     }
00735 
00736     model2.beginReplaceModel();
00737     model2.replaceSubModel(vertices_transformed2);
00738     model2.endReplaceModel(use_refit, refit_bottomup);
00739 
00740     tf2.setIdentity();
00741   }
00742 
00743   node.request = request;
00744   node.result = &result;
00745 
00746   node.model1 = &model1;
00747   node.tf1 = tf1;
00748   node.model2 = &model2;
00749   node.tf2 = tf2;
00750 
00751   node.vertices1 = model1.vertices;
00752   node.vertices2 = model2.vertices;
00753 
00754   node.tri_indices1 = model1.tri_indices;
00755   node.tri_indices2 = model2.tri_indices;
00756 
00757   return true;
00758 }
00759 
00760 
00762 bool initialize(MeshDistanceTraversalNodeRSS& node,
00763                 const BVHModel<RSS>& model1, const Transform3f& tf1,
00764                 const BVHModel<RSS>& model2, const Transform3f& tf2,
00765                 const DistanceRequest& request,
00766                 DistanceResult& result);
00767 
00769 bool initialize(MeshDistanceTraversalNodekIOS& node,
00770                 const BVHModel<kIOS>& model1, const Transform3f& tf1,
00771                 const BVHModel<kIOS>& model2, const Transform3f& tf2,
00772                 const DistanceRequest& request,
00773                 DistanceResult& result);
00774 
00776 bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
00777                 const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
00778                 const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
00779                 const DistanceRequest& request,
00780                 DistanceResult& result);
00781 
00783 template<typename BV, typename S, typename NarrowPhaseSolver>
00784 bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
00785                 BVHModel<BV>& model1, Transform3f& tf1,
00786                 const S& model2, const Transform3f& tf2,
00787                 const NarrowPhaseSolver* nsolver,
00788                 const DistanceRequest& request,
00789                 DistanceResult& result,
00790                 bool use_refit = false, bool refit_bottomup = false)
00791 {
00792   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
00793     return false;
00794 
00795   if(!tf1.isIdentity())
00796   {
00797     std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
00798     for(int i = 0; i < model1.num_vertices; ++i)
00799     {
00800       Vec3f& p = model1.vertices[i];
00801       Vec3f new_v = tf1.transform(p);
00802       vertices_transformed1[i] = new_v;
00803     }
00804 
00805     model1.beginReplaceModel();
00806     model1.replaceSubModel(vertices_transformed1);
00807     model1.endReplaceModel(use_refit, refit_bottomup);
00808 
00809     tf1.setIdentity();
00810   }
00811 
00812   node.request = request;
00813   node.result = &result;
00814 
00815   node.model1 = &model1;
00816   node.tf1 = tf1;
00817   node.model2 = &model2;
00818   node.tf2 = tf2;
00819   node.nsolver = nsolver;
00820   
00821   node.vertices = model1.vertices;
00822   node.tri_indices = model1.tri_indices;
00823 
00824   computeBV(model2, tf2, node.model2_bv);
00825 
00826   return true;
00827 }
00828 
00830 template<typename S, typename BV, typename NarrowPhaseSolver>
00831 bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
00832                 const S& model1, const Transform3f& tf1,
00833                 BVHModel<BV>& model2, Transform3f& tf2,
00834                 const NarrowPhaseSolver* nsolver,
00835                 const DistanceRequest& request,
00836                 DistanceResult& result,
00837                 bool use_refit = false, bool refit_bottomup = false)
00838 {
00839   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
00840     return false;
00841   
00842   if(!tf2.isIdentity())
00843   {
00844     std::vector<Vec3f> vertices_transformed(model2.num_vertices);
00845     for(int i = 0; i < model2.num_vertices; ++i)
00846     {
00847       Vec3f& p = model2.vertices[i];
00848       Vec3f new_v = tf2.transform(p);
00849       vertices_transformed[i] = new_v;
00850     }
00851 
00852     model2.beginReplaceModel();
00853     model2.replaceSubModel(vertices_transformed);
00854     model2.endReplaceModel(use_refit, refit_bottomup);
00855 
00856     tf2.setIdentity();
00857   }
00858 
00859   node.request = request;
00860   node.result = &result;
00861 
00862   node.model1 = &model1;
00863   node.tf1 = tf1;
00864   node.model2 = &model2;
00865   node.tf2 = tf2;
00866   node.nsolver = nsolver;
00867 
00868   node.vertices = model2.vertices;
00869   node.tri_indices = model2.tri_indices;
00870 
00871   computeBV(model1, tf1, node.model1_bv);
00872   
00873   return true;
00874 }
00875 
00877 namespace details
00878 {
00879 
00880 template<typename BV, typename S, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
00881 static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node, 
00882                                                       const BVHModel<BV>& model1, const Transform3f& tf1,
00883                                                       const S& model2, const Transform3f& tf2,
00884                                                       const NarrowPhaseSolver* nsolver,
00885                                                       const DistanceRequest& request,
00886                                                       DistanceResult& result)
00887 {
00888   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
00889     return false;
00890 
00891   node.request = request;
00892   node.result = &result;
00893 
00894   node.model1 = &model1;
00895   node.tf1 = tf1;
00896   node.model2 = &model2;
00897   node.tf2 = tf2;
00898   node.nsolver = nsolver;
00899 
00900   computeBV(model2, tf2, node.model2_bv);
00901 
00902   node.vertices = model1.vertices;
00903   node.tri_indices = model1.tri_indices;
00904 
00905   return true;
00906 }
00907 }
00909 
00911 template<typename S, typename NarrowPhaseSolver>
00912 bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
00913                 const BVHModel<RSS>& model1, const Transform3f& tf1,
00914                 const S& model2, const Transform3f& tf2,
00915                 const NarrowPhaseSolver* nsolver,
00916                 const DistanceRequest& request, 
00917                 DistanceResult& result)
00918 {
00919   return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
00920 }
00921 
00923 template<typename S, typename NarrowPhaseSolver>
00924 bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
00925                 const BVHModel<kIOS>& model1, const Transform3f& tf1,
00926                 const S& model2, const Transform3f& tf2,
00927                 const NarrowPhaseSolver* nsolver,
00928                 const DistanceRequest& request,
00929                 DistanceResult& result)
00930 {
00931   return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);  
00932 }
00933 
00935 template<typename S, typename NarrowPhaseSolver>
00936 bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
00937                 const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
00938                 const S& model2, const Transform3f& tf2,
00939                 const NarrowPhaseSolver* nsolver,
00940                 const DistanceRequest& request,
00941                 DistanceResult& result)
00942 {
00943   return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
00944 }
00945 
00946 
00947 namespace details
00948 {
00949 template<typename S, typename BV, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
00950 static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
00951                                                       const S& model1, const Transform3f& tf1,
00952                                                       const BVHModel<BV>& model2, const Transform3f& tf2,
00953                                                       const NarrowPhaseSolver* nsolver,
00954                                                       const DistanceRequest& request,
00955                                                       DistanceResult& result)
00956 {
00957   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
00958     return false;
00959 
00960   node.request = request;
00961   node.result = &result;
00962 
00963   node.model1 = &model1;
00964   node.tf1 = tf1;
00965   node.model2 = &model2;
00966   node.tf2 = tf2;
00967   node.nsolver = nsolver;
00968 
00969   computeBV(model1, tf1, node.model1_bv);
00970 
00971   node.vertices = model2.vertices;
00972   node.tri_indices = model2.tri_indices;
00973   node.R = tf2.getRotation();
00974   node.T = tf2.getTranslation();
00975 
00976   return true;  
00977 }
00978 }
00979 
00980 
00982 template<typename S, typename NarrowPhaseSolver>
00983 bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
00984                 const S& model1, const Transform3f& tf1,
00985                 const BVHModel<RSS>& model2, const Transform3f& tf2,
00986                 const NarrowPhaseSolver* nsolver,
00987                 const DistanceRequest& request,
00988                 DistanceResult& result)
00989 {
00990   return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
00991 }
00992 
00994 template<typename S, typename NarrowPhaseSolver>
00995 bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
00996                 const S& model1, const Transform3f& tf1,
00997                 const BVHModel<kIOS>& model2, const Transform3f& tf2,
00998                 const NarrowPhaseSolver* nsolver,
00999                 const DistanceRequest& request,
01000                 DistanceResult& result)
01001 {
01002   return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
01003 }
01004 
01006 template<typename S, typename NarrowPhaseSolver>
01007 bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
01008                 const S& model1, const Transform3f& tf1,
01009                 const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
01010                 const NarrowPhaseSolver* nsolver,
01011                 const DistanceRequest& request,
01012                 DistanceResult& result)
01013 {
01014   return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
01015 }
01016 
01017 
01018 
01020 template<typename BV>
01021 bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
01022                 const BVHModel<BV>& model1, const Transform3f& tf1,
01023                 const BVHModel<BV>& model2, const Transform3f& tf2,
01024                 const CollisionRequest& request)
01025 {
01026   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
01027     return false;
01028 
01029   node.model1 = &model1;
01030   node.tf1 = tf1;
01031   node.model2 = &model2;
01032   node.tf2 = tf2;
01033 
01034   node.vertices1 = model1.vertices;
01035   node.vertices2 = model2.vertices;
01036 
01037   node.tri_indices1 = model1.tri_indices;
01038   node.tri_indices2 = model2.tri_indices;
01039 
01040   node.prev_vertices1 = model1.prev_vertices;
01041   node.prev_vertices2 = model2.prev_vertices;
01042 
01043   node.request = request;
01044 
01045   return true;
01046 }
01047 
01049 template<typename BV>
01050 bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
01051                 BVHModel<BV>& model1,
01052                 BVHModel<BV>& model2,
01053                 const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, FCL_REAL w = 1,
01054                 bool use_refit = false, bool refit_bottomup = false)
01055 {
01056   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
01057     return false;
01058 
01059   std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
01060   for(int i = 0; i < model1.num_vertices; ++i)
01061   {
01062     Vec3f& p = model1.vertices[i];
01063     Vec3f new_v = R1 * p + T1;
01064     vertices_transformed1[i] = new_v;
01065   }
01066 
01067 
01068   std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
01069   for(int i = 0; i < model2.num_vertices; ++i)
01070   {
01071     Vec3f& p = model2.vertices[i];
01072     Vec3f new_v = R2 * p + T2;
01073     vertices_transformed2[i] = new_v;
01074   }
01075 
01076   model1.beginReplaceModel();
01077   model1.replaceSubModel(vertices_transformed1);
01078   model1.endReplaceModel(use_refit, refit_bottomup);
01079 
01080   model2.beginReplaceModel();
01081   model2.replaceSubModel(vertices_transformed2);
01082   model2.endReplaceModel(use_refit, refit_bottomup);
01083 
01084   node.model1 = &model1;
01085   node.model2 = &model2;
01086 
01087   node.vertices1 = model1.vertices;
01088   node.vertices2 = model2.vertices;
01089 
01090   node.tri_indices1 = model1.tri_indices;
01091   node.tri_indices2 = model2.tri_indices;
01092 
01093   node.w = w;
01094 
01095   return true;
01096 }
01097 
01098 
01100 inline bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
01101                        const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, FCL_REAL w = 1)
01102 {
01103   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
01104     return false;
01105 
01106   node.model1 = &model1;
01107   node.model2 = &model2;
01108 
01109   node.vertices1 = model1.vertices;
01110   node.vertices2 = model2.vertices;
01111 
01112   node.tri_indices1 = model1.tri_indices;
01113   node.tri_indices2 = model2.tri_indices;
01114 
01115   node.w = w;
01116 
01117   relativeTransform(R1, T1, R2, T2, node.R, node.T);
01118 
01119   return true;
01120 }
01121 
01122 }
01123 
01124 #endif