All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends

traversal_node_setup.h

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