38 #ifndef FCL_TRAVERSAL_OCTREE_OCTREESOLVER_INL_H
39 #define FCL_TRAVERSAL_OCTREE_OCTREESOLVER_INL_H
52 template <
typename NarrowPhaseSolver>
54 const NarrowPhaseSolver* solver_)
65 template <
typename NarrowPhaseSolver>
67 const OcTree<S>* tree1,
68 const OcTree<S>* tree2,
77 OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
78 tree2, tree2->getRoot(), tree2->getRootBV(),
83 template <
typename NarrowPhaseSolver>
85 const OcTree<S>* tree1,
86 const OcTree<S>* tree2,
95 OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
96 tree2, tree2->getRoot(), tree2->getRootBV(),
101 template <
typename NarrowPhaseSolver>
102 template <
typename BV>
104 const OcTree<S>* tree1,
111 crequest = &request_;
114 OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
120 template <
typename NarrowPhaseSolver>
121 template <
typename BV>
123 const OcTree<S>* tree1,
130 drequest = &request_;
133 OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
139 template <
typename NarrowPhaseSolver>
140 template <
typename BV>
143 const OcTree<S>* tree2,
150 crequest = &request_;
153 OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
160 template <
typename NarrowPhaseSolver>
161 template <
typename BV>
164 const OcTree<S>* tree2,
170 drequest = &request_;
173 OcTreeMeshDistanceRecurse(tree1, 0,
174 tree2, tree2->getRoot(), tree2->getRootBV(),
180 template <
typename NarrowPhaseSolver>
181 template <
typename Shape>
183 const OcTree<S>* tree,
190 crequest = &request_;
197 OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
205 template <
typename NarrowPhaseSolver>
206 template <
typename Shape>
209 const OcTree<S>* tree,
215 crequest = &request_;
222 OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
229 template <
typename NarrowPhaseSolver>
230 template <
typename Shape>
232 const OcTree<S>* tree,
239 drequest = &request_;
244 OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
251 template <
typename NarrowPhaseSolver>
252 template <
typename Shape>
255 const OcTree<S>* tree,
261 drequest = &request_;
266 OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
272 template <
typename NarrowPhaseSolver>
273 template <
typename Shape>
275 const Shape& s,
const AABB<S>& aabb2,
278 if(!tree1->nodeHasChildren(root1))
280 if(tree1->isNodeOccupied(root1))
295 solver->shapeDistance(box, box_tf, s, tf2, &dist, &closest_p1, &closest_p2);
299 return drequest->isSatisfied(*dresult);
305 if(!tree1->isNodeOccupied(root1))
return false;
307 for(
unsigned int i = 0; i < 8; ++i)
309 if(tree1->nodeChildExists(root1, i))
311 const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
313 computeChildBV(bv1, i, child_bv);
318 if(d < dresult->min_distance)
320 if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2))
330 template <
typename NarrowPhaseSolver>
331 template <
typename Shape>
333 const Shape& s,
const OBB<S>& obb2,
346 if(solver->shapeIntersect(box, box_tf, s, tf2,
nullptr))
352 aabb1.
overlap(aabb2, overlap_part);
353 cresult->addCostSource(
CostSource<S>(overlap_part, tree1->getOccupancyThres() * s.cost_density), crequest->num_max_cost_sources);
359 else if(!tree1->nodeHasChildren(root1))
361 if(tree1->isNodeOccupied(root1) && s.isOccupied())
371 bool is_intersect =
false;
372 if(!crequest->enable_contact)
374 if(solver->shapeIntersect(box, box_tf, s, tf2,
nullptr))
377 if(cresult->numContacts() < crequest->num_max_contacts)
383 std::vector<ContactPoint<S>> contacts;
384 if(solver->shapeIntersect(box, box_tf, s, tf2, &contacts))
387 if(crequest->num_max_contacts > cresult->numContacts())
389 const size_t free_space = crequest->num_max_contacts - cresult->numContacts();
390 size_t num_adding_contacts;
393 if (free_space < contacts.size())
395 std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth<S>, std::placeholders::_2, std::placeholders::_1));
396 num_adding_contacts = free_space;
400 num_adding_contacts = contacts.size();
403 for(
size_t i = 0; i < num_adding_contacts; ++i)
404 cresult->addContact(
Contact<S>(tree1, &s, root1 - tree1->getRoot(),
Contact<S>::NONE, contacts[i].
pos, contacts[i].normal, contacts[i].penetration_depth));
409 if(is_intersect && crequest->enable_cost)
415 aabb1.
overlap(aabb2, overlap_part);
418 return crequest->isSatisfied(*cresult);
422 else if(!tree1->isNodeFree(root1) && !s.isFree() && crequest->enable_cost)
432 if(solver->shapeIntersect(box, box_tf, s, tf2,
nullptr))
438 aabb1.
overlap(aabb2, overlap_part);
451 if(tree1->isNodeFree(root1) || s.isFree())
return false;
452 else if((tree1->isNodeUncertain(root1) || s.isUncertain()) && !crequest->enable_cost)
return false;
457 if(!obb1.
overlap(obb2))
return false;
460 for(
unsigned int i = 0; i < 8; ++i)
462 if(tree1->nodeChildExists(root1, i))
464 const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
466 computeChildBV(bv1, i, child_bv);
468 if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2))
471 else if(!s.isFree() && crequest->enable_cost)
474 computeChildBV(bv1, i, child_bv);
476 if(OcTreeShapeIntersectRecurse(tree1,
nullptr, child_bv, s, obb2, tf1, tf2))
485 template <
typename NarrowPhaseSolver>
486 template <
typename BV>
491 if(!tree1->nodeHasChildren(root1) && tree2->
getBV(root2).isLeaf())
493 if(tree1->isNodeOccupied(root1))
499 int primitive_id = tree2->
getBV(root2).primitiveId();
507 solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist, &closest_p1, &closest_p2);
509 dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id, closest_p1, closest_p2);
511 return drequest->isSatisfied(*dresult);
517 if(!tree1->isNodeOccupied(root1))
return false;
519 if(tree2->
getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.
size() > tree2->
getBV(root2).bv.size())))
521 for(
unsigned int i = 0; i < 8; ++i)
523 if(tree1->nodeChildExists(root1, i))
525 const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
527 computeChildBV(bv1, i, child_bv);
535 if(d < dresult->min_distance)
537 if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
548 int child = tree2->
getBV(root2).leftChild();
552 if(d < dresult->min_distance)
554 if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
558 child = tree2->
getBV(root2).rightChild();
562 if(d < dresult->min_distance)
564 if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
573 template <
typename NarrowPhaseSolver>
574 template <
typename BV>
581 if(tree2->
getBV(root2).isLeaf())
592 int primitive_id = tree2->
getBV(root2).primitiveId();
598 if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2,
nullptr,
nullptr,
nullptr))
603 AABB<S> aabb2(tf2 * p1, tf2 * p2, tf2 * p3);
604 aabb1.
overlap(aabb2, overlap_part);
605 cresult->addCostSource(
CostSource<S>(overlap_part, tree1->getOccupancyThres() * tree2->
cost_density), crequest->num_max_cost_sources);
613 if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->
getBV(root2).leftChild(), tf1, tf2))
616 if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->
getBV(root2).rightChild(), tf1, tf2))
622 else if(!tree1->nodeHasChildren(root1) && tree2->
getBV(root2).isLeaf())
624 if(tree1->isNodeOccupied(root1) && tree2->
isOccupied())
635 int primitive_id = tree2->
getBV(root2).primitiveId();
641 bool is_intersect =
false;
642 if(!crequest->enable_contact)
644 if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2,
nullptr,
nullptr,
nullptr))
647 if(cresult->numContacts() < crequest->num_max_contacts)
648 cresult->addContact(
Contact<S>(tree1, tree2, root1 - tree1->getRoot(), primitive_id));
657 if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, &contact, &depth, &normal))
660 if(cresult->numContacts() < crequest->num_max_contacts)
661 cresult->addContact(
Contact<S>(tree1, tree2, root1 - tree1->getRoot(), primitive_id, contact, normal, depth));
665 if(is_intersect && crequest->enable_cost)
670 AABB<S> aabb2(tf2 * p1, tf2 * p2, tf2 * p3);
671 aabb1.
overlap(aabb2, overlap_part);
672 cresult->addCostSource(
CostSource<S>(overlap_part, root1->getOccupancy() * tree2->
cost_density), crequest->num_max_cost_sources);
675 return crequest->isSatisfied(*cresult);
680 else if(!tree1->isNodeFree(root1) && !tree2->
isFree() && crequest->enable_cost)
691 int primitive_id = tree2->
getBV(root2).primitiveId();
697 if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2,
nullptr,
nullptr,
nullptr))
702 AABB<S> aabb2(tf2 * p1, tf2 * p2, tf2 * p3);
703 aabb1.
overlap(aabb2, overlap_part);
704 cresult->addCostSource(
CostSource<S>(overlap_part, root1->getOccupancy() * tree2->
cost_density), crequest->num_max_cost_sources);
717 if(tree1->isNodeFree(root1) || tree2->
isFree())
return false;
718 else if((tree1->isNodeUncertain(root1) || tree2->
isUncertain()) && !crequest->enable_cost)
return false;
724 if(!obb1.
overlap(obb2))
return false;
727 if(tree2->
getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.
size() > tree2->
getBV(root2).bv.size())))
729 for(
unsigned int i = 0; i < 8; ++i)
731 if(tree1->nodeChildExists(root1, i))
733 const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
735 computeChildBV(bv1, i, child_bv);
737 if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
740 else if(!tree2->
isFree() && crequest->enable_cost)
743 computeChildBV(bv1, i, child_bv);
745 if(OcTreeMeshIntersectRecurse(tree1,
nullptr, child_bv, tree2, root2, tf1, tf2))
752 if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->
getBV(root2).leftChild(), tf1, tf2))
755 if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->
getBV(root2).rightChild(), tf1, tf2))
764 template <
typename NarrowPhaseSolver>
767 if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2))
769 if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
785 solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist, &closest_p1, &closest_p2);
787 dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), closest_p1, closest_p2);
789 return drequest->isSatisfied(*dresult);
795 if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2))
return false;
797 if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.
size() > bv2.
size())))
799 for(
unsigned int i = 0; i < 8; ++i)
801 if(tree1->nodeChildExists(root1, i))
803 const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
805 computeChildBV(bv1, i, child_bv);
813 if(d < dresult->min_distance)
816 if(OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2))
824 for(
unsigned int i = 0; i < 8; ++i)
826 if(tree2->nodeChildExists(root2, i))
828 const typename OcTree<S>::OcTreeNode* child = tree2->getNodeChild(root2, i);
830 computeChildBV(bv2, i, child_bv);
838 if(d < dresult->min_distance)
840 if(OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2))
851 template <
typename NarrowPhaseSolver>
871 aabb1.
overlap(aabb2, overlap_part);
872 cresult->addCostSource(
CostSource<S>(overlap_part, tree1->getOccupancyThres() * tree2->getOccupancyThres()), crequest->num_max_cost_sources);
877 else if(!root1 && root2)
879 if(tree2->nodeHasChildren(root2))
881 for(
unsigned int i = 0; i < 8; ++i)
883 if(tree2->nodeChildExists(root2, i))
885 const typename OcTree<S>::OcTreeNode* child = tree2->getNodeChild(root2, i);
887 computeChildBV(bv2, i, child_bv);
888 if(OcTreeIntersectRecurse(tree1,
nullptr, bv1, tree2, child, child_bv, tf1, tf2))
894 computeChildBV(bv2, i, child_bv);
895 if(OcTreeIntersectRecurse(tree1,
nullptr, bv1, tree2,
nullptr, child_bv, tf1, tf2))
902 if(OcTreeIntersectRecurse(tree1,
nullptr, bv1, tree2,
nullptr, bv2, tf1, tf2))
908 else if(root1 && !root2)
910 if(tree1->nodeHasChildren(root1))
912 for(
unsigned int i = 0; i < 8; ++i)
914 if(tree1->nodeChildExists(root1, i))
916 const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
918 computeChildBV(bv1, i, child_bv);
919 if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2,
nullptr, bv2, tf1, tf2))
925 computeChildBV(bv1, i, child_bv);
926 if(OcTreeIntersectRecurse(tree1,
nullptr, child_bv, tree2,
nullptr, bv2, tf1, tf2))
933 if(OcTreeIntersectRecurse(tree1,
nullptr, bv1, tree2,
nullptr, bv2, tf1, tf2))
939 else if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2))
941 if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
943 bool is_intersect =
false;
944 if(!crequest->enable_contact)
953 if(cresult->numContacts() < crequest->num_max_contacts)
954 cresult->addContact(
Contact<S>(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot()));
964 std::vector<ContactPoint<S>> contacts;
965 if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contacts))
968 if(crequest->num_max_contacts > cresult->numContacts())
970 const size_t free_space = crequest->num_max_contacts - cresult->numContacts();
971 size_t num_adding_contacts;
974 if (free_space < contacts.size())
976 std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth<S>, std::placeholders::_2, std::placeholders::_1));
977 num_adding_contacts = free_space;
981 num_adding_contacts = contacts.size();
984 for(
size_t i = 0; i < num_adding_contacts; ++i)
985 cresult->addContact(
Contact<S>(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth));
990 if(is_intersect && crequest->enable_cost)
1001 aabb1.
overlap(aabb2, overlap_part);
1002 cresult->addCostSource(
CostSource<S>(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources);
1005 return crequest->isSatisfied(*cresult);
1007 else if(!tree1->isNodeFree(root1) && !tree2->isNodeFree(root2) && crequest->enable_cost)
1024 aabb1.
overlap(aabb2, overlap_part);
1025 cresult->addCostSource(
CostSource<S>(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources);
1037 if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2))
return false;
1038 else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)) && !crequest->enable_cost)
return false;
1044 if(!obb1.
overlap(obb2))
return false;
1047 if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.
size() > bv2.
size())))
1049 for(
unsigned int i = 0; i < 8; ++i)
1051 if(tree1->nodeChildExists(root1, i))
1053 const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
1055 computeChildBV(bv1, i, child_bv);
1057 if(OcTreeIntersectRecurse(tree1, child, child_bv,
1062 else if(!tree2->isNodeFree(root2) && crequest->enable_cost)
1065 computeChildBV(bv1, i, child_bv);
1067 if(OcTreeIntersectRecurse(tree1,
nullptr, child_bv,
1076 for(
unsigned int i = 0; i < 8; ++i)
1078 if(tree2->nodeChildExists(root2, i))
1080 const typename OcTree<S>::OcTreeNode* child = tree2->getNodeChild(root2, i);
1082 computeChildBV(bv2, i, child_bv);
1084 if(OcTreeIntersectRecurse(tree1, root1, bv1,
1085 tree2, child, child_bv,
1089 else if(!tree1->isNodeFree(root1) && crequest->enable_cost)
1092 computeChildBV(bv2, i, child_bv);
1094 if(OcTreeIntersectRecurse(tree1, root1, bv1,
1095 tree2,
nullptr, child_bv,