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
00037 #ifndef FCL_TRAVERSAL_NODE_OCTREE_H
00038 #define FCL_TRAVERSAL_NODE_OCTREE_H
00039
00040 #include "fcl/collision_data.h"
00041 #include "fcl/traversal/traversal_node_base.h"
00042 #include "fcl/narrowphase/narrowphase.h"
00043 #include "fcl/shape/geometric_shapes_utility.h"
00044 #include "fcl/octree.h"
00045 #include "fcl/BVH/BVH_model.h"
00046
00047 namespace fcl
00048 {
00049
00051 template<typename NarrowPhaseSolver>
00052 class OcTreeSolver
00053 {
00054 private:
00055 const NarrowPhaseSolver* solver;
00056
00057 mutable const CollisionRequest* crequest;
00058 mutable const DistanceRequest* drequest;
00059
00060 mutable CollisionResult* cresult;
00061 mutable DistanceResult* dresult;
00062
00063 public:
00064 OcTreeSolver(const NarrowPhaseSolver* solver_) : solver(solver_),
00065 crequest(NULL),
00066 drequest(NULL),
00067 cresult(NULL),
00068 dresult(NULL)
00069 {
00070 }
00071
00073 void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
00074 const Transform3f& tf1, const Transform3f& tf2,
00075 const CollisionRequest& request_,
00076 CollisionResult& result_) const
00077 {
00078 crequest = &request_;
00079 cresult = &result_;
00080
00081 OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
00082 tree2, tree2->getRoot(), tree2->getRootBV(),
00083 tf1, tf2);
00084 }
00085
00087 void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
00088 const Transform3f& tf1, const Transform3f& tf2,
00089 const DistanceRequest& request_,
00090 DistanceResult& result_) const
00091 {
00092 drequest = &request_;
00093 dresult = &result_;
00094
00095 OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
00096 tree2, tree2->getRoot(), tree2->getRootBV(),
00097 tf1, tf2);
00098 }
00099
00101 template<typename BV>
00102 void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
00103 const Transform3f& tf1, const Transform3f& tf2,
00104 const CollisionRequest& request_,
00105 CollisionResult& result_) const
00106 {
00107 crequest = &request_;
00108 cresult = &result_;
00109
00110 OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
00111 tree2, 0,
00112 tf1, tf2);
00113 }
00114
00116 template<typename BV>
00117 void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
00118 const Transform3f& tf1, const Transform3f& tf2,
00119 const DistanceRequest& request_,
00120 DistanceResult& result_) const
00121 {
00122 drequest = &request_;
00123 dresult = &result_;
00124
00125 OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
00126 tree2, 0,
00127 tf1, tf2);
00128 }
00129
00131 template<typename BV>
00132 void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
00133 const Transform3f& tf1, const Transform3f& tf2,
00134 const CollisionRequest& request_,
00135 CollisionResult& result_) const
00136
00137 {
00138 crequest = &request_;
00139 cresult = &result_;
00140
00141 OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
00142 tree1, 0,
00143 tf2, tf1);
00144 }
00145
00147 template<typename BV>
00148 void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
00149 const Transform3f& tf1, const Transform3f& tf2,
00150 const DistanceRequest& request_,
00151 DistanceResult& result_) const
00152 {
00153 drequest = &request_;
00154 dresult = &result_;
00155
00156 OcTreeMeshDistanceRecurse(tree1, 0,
00157 tree2, tree2->getRoot(), tree2->getRootBV(),
00158 tf1, tf2);
00159 }
00160
00162 template<typename S>
00163 void OcTreeShapeIntersect(const OcTree* tree, const S& s,
00164 const Transform3f& tf1, const Transform3f& tf2,
00165 const CollisionRequest& request_,
00166 CollisionResult& result_) const
00167 {
00168 crequest = &request_;
00169 cresult = &result_;
00170
00171 AABB bv2;
00172 computeBV<AABB>(s, Transform3f(), bv2);
00173 OBB obb2;
00174 convertBV(bv2, tf2, obb2);
00175 OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
00176 s, obb2,
00177 tf1, tf2);
00178
00179 }
00180
00182 template<typename S>
00183 void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
00184 const Transform3f& tf1, const Transform3f& tf2,
00185 const CollisionRequest& request_,
00186 CollisionResult& result_) const
00187 {
00188 crequest = &request_;
00189 cresult = &result_;
00190
00191 AABB bv1;
00192 computeBV<AABB>(s, Transform3f(), bv1);
00193 OBB obb1;
00194 convertBV(bv1, tf1, obb1);
00195 OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
00196 s, obb1,
00197 tf2, tf1);
00198 }
00199
00201 template<typename S>
00202 void OcTreeShapeDistance(const OcTree* tree, const S& s,
00203 const Transform3f& tf1, const Transform3f& tf2,
00204 const DistanceRequest& request_,
00205 DistanceResult& result_) const
00206 {
00207 drequest = &request_;
00208 dresult = &result_;
00209
00210 AABB aabb2;
00211 computeBV<AABB>(s, tf2, aabb2);
00212 OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
00213 s, aabb2,
00214 tf1, tf2);
00215 }
00216
00218 template<typename S>
00219 void ShapeOcTreeDistance(const S& s, const OcTree* tree,
00220 const Transform3f& tf1, const Transform3f& tf2,
00221 const DistanceRequest& request_,
00222 DistanceResult& result_) const
00223 {
00224 drequest = &request_;
00225 dresult = &result_;
00226
00227 AABB aabb1;
00228 computeBV<AABB>(s, tf1, aabb1);
00229 OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
00230 s, aabb1,
00231 tf2, tf1);
00232 }
00233
00234
00235 private:
00236 template<typename S>
00237 bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
00238 const S& s, const AABB& aabb2,
00239 const Transform3f& tf1, const Transform3f& tf2) const
00240 {
00241 if(!root1->hasChildren())
00242 {
00243 if(tree1->isNodeOccupied(root1))
00244 {
00245 Box box;
00246 Transform3f box_tf;
00247 constructBox(bv1, tf1, box, box_tf);
00248
00249 FCL_REAL dist;
00250 solver->shapeDistance(box, box_tf, s, tf2, &dist);
00251
00252 dresult->update(dist, tree1, &s, root1 - tree1->getRoot(), DistanceResult::NONE);
00253
00254 return drequest->isSatisfied(*dresult);
00255 }
00256 else
00257 return false;
00258 }
00259
00260 if(!tree1->isNodeOccupied(root1)) return false;
00261
00262 for(unsigned int i = 0; i < 8; ++i)
00263 {
00264 if(root1->childExists(i))
00265 {
00266 const OcTree::OcTreeNode* child = root1->getChild(i);
00267 AABB child_bv;
00268 computeChildBV(bv1, i, child_bv);
00269
00270 AABB aabb1;
00271 convertBV(child_bv, tf1, aabb1);
00272 FCL_REAL d = aabb1.distance(aabb2);
00273 if(d < dresult->min_distance)
00274 {
00275 if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2))
00276 return true;
00277 }
00278 }
00279 }
00280
00281 return false;
00282 }
00283
00284 template<typename S>
00285 bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
00286 const S& s, const OBB& obb2,
00287 const Transform3f& tf1, const Transform3f& tf2) const
00288 {
00289 if(!root1->hasChildren())
00290 {
00291 if(tree1->isNodeOccupied(root1) && s.isOccupied())
00292 {
00293 OBB obb1;
00294 convertBV(bv1, tf1, obb1);
00295 if(obb1.overlap(obb2))
00296 {
00297 Box box;
00298 Transform3f box_tf;
00299 constructBox(bv1, tf1, box, box_tf);
00300
00301 bool is_intersect = false;
00302 if(!crequest->enable_contact)
00303 {
00304 if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
00305 {
00306 is_intersect = true;
00307 if(cresult->numContacts() < crequest->num_max_contacts)
00308 cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE));
00309 }
00310 }
00311 else
00312 {
00313 Vec3f contact;
00314 FCL_REAL depth;
00315 Vec3f normal;
00316
00317 if(solver->shapeIntersect(box, box_tf, s, tf2, &contact, &depth, &normal))
00318 {
00319 is_intersect = true;
00320 if(cresult->numContacts() < crequest->num_max_contacts)
00321 cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE, contact, normal, depth));
00322 }
00323 }
00324
00325 if(is_intersect && crequest->enable_cost)
00326 {
00327 AABB overlap_part;
00328 AABB aabb1, aabb2;
00329 computeBV<AABB, Box>(box, box_tf, aabb1);
00330 computeBV<AABB, S>(s, tf2, aabb2);
00331 aabb1.overlap(aabb2, overlap_part);
00332 cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * s.cost_density), crequest->num_max_cost_sources);
00333 }
00334
00335 return crequest->isSatisfied(*cresult);
00336 }
00337 else return false;
00338 }
00339 else if(!tree1->isNodeFree(root1) && !s.isFree() && crequest->enable_cost)
00340 {
00341 OBB obb1;
00342 convertBV(bv1, tf1, obb1);
00343 if(obb1.overlap(obb2))
00344 {
00345 Box box;
00346 Transform3f box_tf;
00347 constructBox(bv1, tf1, box, box_tf);
00348
00349 if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
00350 {
00351 AABB overlap_part;
00352 AABB aabb1, aabb2;
00353 computeBV<AABB, Box>(box, box_tf, aabb1);
00354 computeBV<AABB, S>(s, tf2, aabb2);
00355 aabb1.overlap(aabb2, overlap_part);
00356 cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * s.cost_density), crequest->num_max_cost_sources);
00357 }
00358 }
00359
00360 return false;
00361 }
00362 else
00363 return false;
00364 }
00365
00369 if(tree1->isNodeFree(root1) || s.isFree()) return false;
00370 else if((tree1->isNodeUncertain(root1) || s.isUncertain()) && !crequest->enable_cost) return false;
00371 else
00372 {
00373 OBB obb1;
00374 convertBV(bv1, tf1, obb1);
00375 if(!obb1.overlap(obb2)) return false;
00376 }
00377
00378 for(unsigned int i = 0; i < 8; ++i)
00379 {
00380 if(root1->childExists(i))
00381 {
00382 const OcTree::OcTreeNode* child = root1->getChild(i);
00383 AABB child_bv;
00384 computeChildBV(bv1, i, child_bv);
00385
00386 if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2))
00387 return true;
00388 }
00389 }
00390
00391 return false;
00392 }
00393
00394 template<typename BV>
00395 bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
00396 const BVHModel<BV>* tree2, int root2,
00397 const Transform3f& tf1, const Transform3f& tf2) const
00398 {
00399 if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
00400 {
00401 if(tree1->isNodeOccupied(root1))
00402 {
00403 Box box;
00404 Transform3f box_tf;
00405 constructBox(bv1, tf1, box, box_tf);
00406
00407 int primitive_id = tree2->getBV(root2).primitiveId();
00408 const Triangle& tri_id = tree2->tri_indices[primitive_id];
00409 const Vec3f& p1 = tree2->vertices[tri_id[0]];
00410 const Vec3f& p2 = tree2->vertices[tri_id[1]];
00411 const Vec3f& p3 = tree2->vertices[tri_id[2]];
00412
00413 FCL_REAL dist;
00414 solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist);
00415
00416 dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id);
00417
00418 return drequest->isSatisfied(*dresult);
00419 }
00420 else
00421 return false;
00422 }
00423
00424 if(!tree1->isNodeOccupied(root1)) return false;
00425
00426 if(tree2->getBV(root2).isLeaf() || (root1->hasChildren() && (bv1.size() > tree2->getBV(root2).bv.size())))
00427 {
00428 for(unsigned int i = 0; i < 8; ++i)
00429 {
00430 if(root1->childExists(i))
00431 {
00432 const OcTree::OcTreeNode* child = root1->getChild(i);
00433 AABB child_bv;
00434 computeChildBV(bv1, i, child_bv);
00435
00436 FCL_REAL d;
00437 AABB aabb1, aabb2;
00438 convertBV(child_bv, tf1, aabb1);
00439 convertBV(tree2->getBV(root2).bv, tf2, aabb2);
00440 d = aabb1.distance(aabb2);
00441
00442 if(d < dresult->min_distance)
00443 {
00444 if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
00445 return true;
00446 }
00447 }
00448 }
00449 }
00450 else
00451 {
00452 FCL_REAL d;
00453 AABB aabb1, aabb2;
00454 convertBV(bv1, tf1, aabb1);
00455 int child = tree2->getBV(root2).leftChild();
00456 convertBV(tree2->getBV(child).bv, tf2, aabb2);
00457 d = aabb1.distance(aabb2);
00458
00459 if(d < dresult->min_distance)
00460 {
00461 if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
00462 return true;
00463 }
00464
00465 child = tree2->getBV(root2).rightChild();
00466 convertBV(tree2->getBV(child).bv, tf2, aabb2);
00467 d = aabb1.distance(aabb2);
00468
00469 if(d < dresult->min_distance)
00470 {
00471 if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
00472 return true;
00473 }
00474 }
00475
00476 return false;
00477 }
00478
00479
00480 template<typename BV>
00481 bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
00482 const BVHModel<BV>* tree2, int root2,
00483 const Transform3f& tf1, const Transform3f& tf2) const
00484 {
00485 if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
00486 {
00487 if(tree1->isNodeOccupied(root1) && tree2->isOccupied())
00488 {
00489 OBB obb1, obb2;
00490 convertBV(bv1, tf1, obb1);
00491 convertBV(tree2->getBV(root2).bv, tf2, obb2);
00492 if(obb1.overlap(obb2))
00493 {
00494 Box box;
00495 Transform3f box_tf;
00496 constructBox(bv1, tf1, box, box_tf);
00497
00498 int primitive_id = tree2->getBV(root2).primitiveId();
00499 const Triangle& tri_id = tree2->tri_indices[primitive_id];
00500 const Vec3f& p1 = tree2->vertices[tri_id[0]];
00501 const Vec3f& p2 = tree2->vertices[tri_id[1]];
00502 const Vec3f& p3 = tree2->vertices[tri_id[2]];
00503
00504 bool is_intersect = false;
00505 if(!crequest->enable_contact)
00506 {
00507 if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL))
00508 {
00509 is_intersect = true;
00510 if(cresult->numContacts() < crequest->num_max_contacts)
00511 cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2));
00512 }
00513 }
00514 else
00515 {
00516 Vec3f contact;
00517 FCL_REAL depth;
00518 Vec3f normal;
00519
00520 if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, &contact, &depth, &normal))
00521 {
00522 is_intersect = true;
00523 if(cresult->numContacts() < crequest->num_max_contacts)
00524 cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2, contact, normal, depth));
00525 }
00526 }
00527
00528 if(is_intersect && crequest->enable_cost)
00529 {
00530 AABB overlap_part;
00531 AABB aabb1;
00532 computeBV<AABB, Box>(box, box_tf, aabb1);
00533 AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3));
00534 aabb1.overlap(aabb2, overlap_part);
00535 cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources);
00536 }
00537
00538 return crequest->isSatisfied(*cresult);
00539 }
00540 else
00541 return false;
00542 }
00543 else if(!tree1->isNodeFree(root1) && !tree2->isFree() && crequest->enable_cost)
00544 {
00545 OBB obb1, obb2;
00546 convertBV(bv1, tf1, obb1);
00547 convertBV(tree2->getBV(root2).bv, tf2, obb2);
00548 if(obb1.overlap(obb2))
00549 {
00550 Box box;
00551 Transform3f box_tf;
00552 constructBox(bv1, tf1, box, box_tf);
00553
00554 int primitive_id = tree2->getBV(root2).primitiveId();
00555 const Triangle& tri_id = tree2->tri_indices[primitive_id];
00556 const Vec3f& p1 = tree2->vertices[tri_id[0]];
00557 const Vec3f& p2 = tree2->vertices[tri_id[1]];
00558 const Vec3f& p3 = tree2->vertices[tri_id[2]];
00559
00560 if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL))
00561 {
00562 AABB overlap_part;
00563 AABB aabb1;
00564 computeBV<AABB, Box>(box, box_tf, aabb1);
00565 AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3));
00566 aabb1.overlap(aabb2, overlap_part);
00567 cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources);
00568 }
00569 }
00570
00571 return false;
00572 }
00573 else
00574 return false;
00575 }
00576
00580 if(tree1->isNodeFree(root1) || tree2->isFree()) return false;
00581 else if((tree1->isNodeUncertain(root1) || tree2->isUncertain()) && !crequest->enable_cost) return false;
00582 else
00583 {
00584 OBB obb1, obb2;
00585 convertBV(bv1, tf1, obb1);
00586 convertBV(tree2->getBV(root2).bv, tf2, obb2);
00587 if(!obb1.overlap(obb2)) return false;
00588 }
00589
00590 if(tree2->getBV(root2).isLeaf() || (root1->hasChildren() && (bv1.size() > tree2->getBV(root2).bv.size())))
00591 {
00592 for(unsigned int i = 0; i < 8; ++i)
00593 {
00594 if(root1->childExists(i))
00595 {
00596 const OcTree::OcTreeNode* child = root1->getChild(i);
00597 AABB child_bv;
00598 computeChildBV(bv1, i, child_bv);
00599
00600 if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
00601 return true;
00602 }
00603 }
00604 }
00605 else
00606 {
00607 if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2))
00608 return true;
00609
00610 if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2))
00611 return true;
00612
00613 }
00614
00615 return false;
00616 }
00617
00618 bool OcTreeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
00619 const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
00620 const Transform3f& tf1, const Transform3f& tf2) const
00621 {
00622 if(!root1->hasChildren() && !root2->hasChildren())
00623 {
00624 if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
00625 {
00626 Box box1, box2;
00627 Transform3f box1_tf, box2_tf;
00628 constructBox(bv1, tf1, box1, box1_tf);
00629 constructBox(bv2, tf2, box2, box2_tf);
00630
00631 FCL_REAL dist;
00632 solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist);
00633
00634 dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot());
00635
00636 return drequest->isSatisfied(*dresult);
00637 }
00638 else
00639 return false;
00640 }
00641
00642 if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false;
00643
00644 if(!root2->hasChildren() || (root1->hasChildren() && (bv1.size() > bv2.size())))
00645 {
00646 for(unsigned int i = 0; i < 8; ++i)
00647 {
00648 if(root1->childExists(i))
00649 {
00650 const OcTree::OcTreeNode* child = root1->getChild(i);
00651 AABB child_bv;
00652 computeChildBV(bv1, i, child_bv);
00653
00654 FCL_REAL d;
00655 AABB aabb1, aabb2;
00656 convertBV(bv1, tf1, aabb1);
00657 convertBV(bv2, tf2, aabb2);
00658 d = aabb1.distance(aabb2);
00659
00660 if(d < dresult->min_distance)
00661 {
00662
00663 if(OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2))
00664 return true;
00665 }
00666 }
00667 }
00668 }
00669 else
00670 {
00671 for(unsigned int i = 0; i < 8; ++i)
00672 {
00673 if(root2->childExists(i))
00674 {
00675 const OcTree::OcTreeNode* child = root2->getChild(i);
00676 AABB child_bv;
00677 computeChildBV(bv2, i, child_bv);
00678
00679 FCL_REAL d;
00680 AABB aabb1, aabb2;
00681 convertBV(bv1, tf1, aabb1);
00682 convertBV(bv2, tf2, aabb2);
00683 d = aabb1.distance(aabb2);
00684
00685 if(d < dresult->min_distance)
00686 {
00687 if(OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2))
00688 return true;
00689 }
00690 }
00691 }
00692 }
00693
00694 return false;
00695 }
00696
00697
00698 bool OcTreeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
00699 const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
00700 const Transform3f& tf1, const Transform3f& tf2) const
00701 {
00702 if(!root1->hasChildren() && !root2->hasChildren())
00703 {
00704 if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
00705 {
00706 bool is_intersect = false;
00707 if(!crequest->enable_contact)
00708 {
00709 OBB obb1, obb2;
00710 convertBV(bv1, tf1, obb1);
00711 convertBV(bv2, tf2, obb2);
00712
00713 if(obb1.overlap(obb2))
00714 {
00715 is_intersect = true;
00716 if(cresult->numContacts() < crequest->num_max_contacts)
00717 cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot()));
00718 }
00719 }
00720 else
00721 {
00722 Box box1, box2;
00723 Transform3f box1_tf, box2_tf;
00724 constructBox(bv1, tf1, box1, box1_tf);
00725 constructBox(bv2, tf2, box2, box2_tf);
00726
00727 Vec3f contact;
00728 FCL_REAL depth;
00729 Vec3f normal;
00730 if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contact, &depth, &normal))
00731 {
00732 is_intersect = true;
00733 if(cresult->numContacts() < crequest->num_max_contacts)
00734 cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contact, normal, depth));
00735 }
00736 }
00737
00738 if(is_intersect && crequest->enable_cost)
00739 {
00740 Box box1, box2;
00741 Transform3f box1_tf, box2_tf;
00742 constructBox(bv1, tf1, box1, box1_tf);
00743 constructBox(bv2, tf2, box2, box2_tf);
00744
00745 AABB overlap_part;
00746 AABB aabb1, aabb2;
00747 computeBV<AABB, Box>(box1, box1_tf, aabb1);
00748 computeBV<AABB, Box>(box2, box2_tf, aabb2);
00749 aabb1.overlap(aabb2, overlap_part);
00750 cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources);
00751 }
00752
00753 return crequest->isSatisfied(*cresult);
00754 }
00755 else if(!tree1->isNodeFree(root1) && !tree2->isNodeFree(root2) && crequest->enable_cost)
00756 {
00757 OBB obb1, obb2;
00758 convertBV(bv1, tf1, obb1);
00759 convertBV(bv2, tf2, obb2);
00760
00761 if(obb1.overlap(obb2))
00762 {
00763 Box box1, box2;
00764 Transform3f box1_tf, box2_tf;
00765 constructBox(bv1, tf1, box1, box1_tf);
00766 constructBox(bv2, tf2, box2, box2_tf);
00767
00768 AABB overlap_part;
00769 AABB aabb1, aabb2;
00770 computeBV<AABB, Box>(box1, box1_tf, aabb1);
00771 computeBV<AABB, Box>(box2, box2_tf, aabb2);
00772 aabb1.overlap(aabb2, overlap_part);
00773 cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources);
00774 }
00775
00776 return false;
00777 }
00778 else
00779 return false;
00780 }
00781
00785 if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false;
00786 else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)) && !crequest->enable_cost) return false;
00787 else
00788 {
00789 OBB obb1, obb2;
00790 convertBV(bv1, tf1, obb1);
00791 convertBV(bv2, tf2, obb2);
00792 if(!obb1.overlap(obb2)) return false;
00793 }
00794
00795 if(!root2->hasChildren() || (root1->hasChildren() && (bv1.size() > bv2.size())))
00796 {
00797 for(unsigned int i = 0; i < 8; ++i)
00798 {
00799 if(root1->childExists(i))
00800 {
00801 const OcTree::OcTreeNode* child = root1->getChild(i);
00802 AABB child_bv;
00803 computeChildBV(bv1, i, child_bv);
00804
00805 if(OcTreeIntersectRecurse(tree1, child, child_bv,
00806 tree2, root2, bv2,
00807 tf1, tf2))
00808 return true;
00809 }
00810 }
00811 }
00812 else
00813 {
00814 for(unsigned int i = 0; i < 8; ++i)
00815 {
00816 if(root2->childExists(i))
00817 {
00818 const OcTree::OcTreeNode* child = root2->getChild(i);
00819 AABB child_bv;
00820 computeChildBV(bv2, i, child_bv);
00821
00822 if(OcTreeIntersectRecurse(tree1, root1, bv1,
00823 tree2, child, child_bv,
00824 tf1, tf2))
00825 return true;
00826 }
00827 }
00828 }
00829
00830 return false;
00831 }
00832 };
00833
00834
00835
00836
00838 template<typename NarrowPhaseSolver>
00839 class OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
00840 {
00841 public:
00842 OcTreeCollisionTraversalNode()
00843 {
00844 model1 = NULL;
00845 model2 = NULL;
00846
00847 otsolver = NULL;
00848 }
00849
00850 bool BVTesting(int, int) const
00851 {
00852 return false;
00853 }
00854
00855 void leafTesting(int, int) const
00856 {
00857 otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
00858 }
00859
00860 const OcTree* model1;
00861 const OcTree* model2;
00862
00863 Transform3f tf1, tf2;
00864
00865 const OcTreeSolver<NarrowPhaseSolver>* otsolver;
00866 };
00867
00869 template<typename NarrowPhaseSolver>
00870 class OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
00871 {
00872 public:
00873 OcTreeDistanceTraversalNode()
00874 {
00875 model1 = NULL;
00876 model2 = NULL;
00877
00878 otsolver = NULL;
00879 }
00880
00881
00882 FCL_REAL BVTesting(int, int) const
00883 {
00884 return -1;
00885 }
00886
00887 void leafTesting(int, int) const
00888 {
00889 otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
00890 }
00891
00892 const OcTree* model1;
00893 const OcTree* model2;
00894
00895 const OcTreeSolver<NarrowPhaseSolver>* otsolver;
00896 };
00897
00899 template<typename S, typename NarrowPhaseSolver>
00900 class ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
00901 {
00902 public:
00903 ShapeOcTreeCollisionTraversalNode()
00904 {
00905 model1 = NULL;
00906 model2 = NULL;
00907
00908 otsolver = NULL;
00909 }
00910
00911 bool BVTesting(int, int) const
00912 {
00913 return false;
00914 }
00915
00916 void leafTesting(int, int) const
00917 {
00918 otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
00919 }
00920
00921 const S* model1;
00922 const OcTree* model2;
00923
00924 Transform3f tf1, tf2;
00925
00926 const OcTreeSolver<NarrowPhaseSolver>* otsolver;
00927 };
00928
00930 template<typename S, typename NarrowPhaseSolver>
00931 class OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase
00932 {
00933 public:
00934 OcTreeShapeCollisionTraversalNode()
00935 {
00936 model1 = NULL;
00937 model2 = NULL;
00938
00939 otsolver = NULL;
00940 }
00941
00942 bool BVTesting(int, int) const
00943 {
00944 return false;
00945 }
00946
00947 void leafTesting(int, int) const
00948 {
00949 otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
00950 }
00951
00952 const OcTree* model1;
00953 const S* model2;
00954
00955 Transform3f tf1, tf2;
00956
00957 const OcTreeSolver<NarrowPhaseSolver>* otsolver;
00958 };
00959
00961 template<typename S, typename NarrowPhaseSolver>
00962 class ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
00963 {
00964 public:
00965 ShapeOcTreeDistanceTraversalNode()
00966 {
00967 model1 = NULL;
00968 model2 = NULL;
00969
00970 otsolver = NULL;
00971 }
00972
00973 FCL_REAL BVTesting(int, int) const
00974 {
00975 return -1;
00976 }
00977
00978 void leafTesting(int, int) const
00979 {
00980 otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
00981 }
00982
00983 const S* model1;
00984 const OcTree* model2;
00985
00986 const OcTreeSolver<NarrowPhaseSolver>* otsolver;
00987 };
00988
00990 template<typename S, typename NarrowPhaseSolver>
00991 class OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase
00992 {
00993 public:
00994 OcTreeShapeDistanceTraversalNode()
00995 {
00996 model1 = NULL;
00997 model2 = NULL;
00998
00999 otsolver = NULL;
01000 }
01001
01002 FCL_REAL BVTesting(int, int) const
01003 {
01004 return -1;
01005 }
01006
01007 void leafTesting(int, int) const
01008 {
01009 otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
01010 }
01011
01012 const OcTree* model1;
01013 const S* model2;
01014
01015 const OcTreeSolver<NarrowPhaseSolver>* otsolver;
01016 };
01017
01019 template<typename BV, typename NarrowPhaseSolver>
01020 class MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
01021 {
01022 public:
01023 MeshOcTreeCollisionTraversalNode()
01024 {
01025 model1 = NULL;
01026 model2 = NULL;
01027
01028 otsolver = NULL;
01029 }
01030
01031 bool BVTesting(int, int) const
01032 {
01033 return false;
01034 }
01035
01036 void leafTesting(int, int) const
01037 {
01038 otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
01039 }
01040
01041 const BVHModel<BV>* model1;
01042 const OcTree* model2;
01043
01044 Transform3f tf1, tf2;
01045
01046 const OcTreeSolver<NarrowPhaseSolver>* otsolver;
01047 };
01048
01050 template<typename BV, typename NarrowPhaseSolver>
01051 class OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase
01052 {
01053 public:
01054 OcTreeMeshCollisionTraversalNode()
01055 {
01056 model1 = NULL;
01057 model2 = NULL;
01058
01059 otsolver = NULL;
01060 }
01061
01062 bool BVTesting(int, int) const
01063 {
01064 return false;
01065 }
01066
01067 void leafTesting(int, int) const
01068 {
01069 otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
01070 }
01071
01072 const OcTree* model1;
01073 const BVHModel<BV>* model2;
01074
01075 Transform3f tf1, tf2;
01076
01077 const OcTreeSolver<NarrowPhaseSolver>* otsolver;
01078 };
01079
01081 template<typename BV, typename NarrowPhaseSolver>
01082 class MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
01083 {
01084 public:
01085 MeshOcTreeDistanceTraversalNode()
01086 {
01087 model1 = NULL;
01088 model2 = NULL;
01089
01090 otsolver = NULL;
01091 }
01092
01093 FCL_REAL BVTesting(int, int) const
01094 {
01095 return -1;
01096 }
01097
01098 void leafTesting(int, int) const
01099 {
01100 otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
01101 }
01102
01103 const BVHModel<BV>* model1;
01104 const OcTree* model2;
01105
01106 const OcTreeSolver<NarrowPhaseSolver>* otsolver;
01107
01108 };
01109
01111 template<typename BV, typename NarrowPhaseSolver>
01112 class OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase
01113 {
01114 public:
01115 OcTreeMeshDistanceTraversalNode()
01116 {
01117 model1 = NULL;
01118 model2 = NULL;
01119
01120 otsolver = NULL;
01121 }
01122
01123 FCL_REAL BVTesting(int, int) const
01124 {
01125 return -1;
01126 }
01127
01128 void leafTesting(int, int) const
01129 {
01130 otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
01131 }
01132
01133 const OcTree* model1;
01134 const BVHModel<BV>* model2;
01135
01136 const OcTreeSolver<NarrowPhaseSolver>* otsolver;
01137
01138 };
01139
01140
01141
01142 }
01143
01144 #endif