00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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