.. _program_listing_file__tmp_ws_src_hpp-fcl_include_hpp_fcl_internal_traversal_node_octree.h:

Program Listing for File traversal_node_octree.h
================================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_hpp-fcl_include_hpp_fcl_internal_traversal_node_octree.h>` (``/tmp/ws/src/hpp-fcl/include/hpp/fcl/internal/traversal_node_octree.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * Software License Agreement (BSD License)
    *
    *  Copyright (c) 2011-2014, Willow Garage, Inc.
    *  Copyright (c) 2014-2015, Open Source Robotics Foundation
    *  All rights reserved.
    *
    *  Redistribution and use in source and binary forms, with or without
    *  modification, are permitted provided that the following conditions
    *  are met:
    *
    *   * Redistributions of source code must retain the above copyright
    *     notice, this list of conditions and the following disclaimer.
    *   * Redistributions in binary form must reproduce the above
    *     copyright notice, this list of conditions and the following
    *     disclaimer in the documentation and/or other materials provided
    *     with the distribution.
    *   * Neither the name of Open Source Robotics Foundation nor the names of its
    *     contributors may be used to endorse or promote products derived
    *     from this software without specific prior written permission.
    *
    *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
    *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    *  POSSIBILITY OF SUCH DAMAGE.
    */
   
   #ifndef HPP_FCL_TRAVERSAL_NODE_OCTREE_H
   #define HPP_FCL_TRAVERSAL_NODE_OCTREE_H
   
   
   #include <hpp/fcl/collision_data.h>
   #include <hpp/fcl/internal/traversal_node_base.h>
   #include <hpp/fcl/narrowphase/narrowphase.h>
   #include <hpp/fcl/octree.h>
   #include <hpp/fcl/BVH/BVH_model.h>
   #include <hpp/fcl/shape/geometric_shapes_utility.h>
   #include <hpp/fcl/internal/shape_shape_func.h>
   
   namespace hpp {
   namespace fcl {
   
   class HPP_FCL_DLLAPI OcTreeSolver {
    private:
     const GJKSolver* solver;
   
     mutable const CollisionRequest* crequest;
     mutable const DistanceRequest* drequest;
   
     mutable CollisionResult* cresult;
     mutable DistanceResult* dresult;
   
    public:
     OcTreeSolver(const GJKSolver* solver_)
         : solver(solver_),
           crequest(NULL),
           drequest(NULL),
           cresult(NULL),
           dresult(NULL) {}
   
     void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
                          const Transform3f& tf1, const Transform3f& tf2,
                          const CollisionRequest& request_,
                          CollisionResult& result_) const {
       crequest = &request_;
       cresult = &result_;
   
       OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
                              tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
     }
   
     void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
                         const Transform3f& tf1, const Transform3f& tf2,
                         const DistanceRequest& request_,
                         DistanceResult& result_) const {
       drequest = &request_;
       dresult = &result_;
   
       OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
                             tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
     }
   
     template <typename BV>
     void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
                              const Transform3f& tf1, const Transform3f& tf2,
                              const CollisionRequest& request_,
                              CollisionResult& result_) const {
       crequest = &request_;
       cresult = &result_;
   
       OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
                                  tree2, 0, tf1, tf2);
     }
   
     template <typename BV>
     void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
                             const Transform3f& tf1, const Transform3f& tf2,
                             const DistanceRequest& request_,
                             DistanceResult& result_) const {
       drequest = &request_;
       dresult = &result_;
   
       OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
                                 tree2, 0, tf1, tf2);
     }
   
     template <typename BV>
     void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
                              const Transform3f& tf1, const Transform3f& tf2,
                              const CollisionRequest& request_,
                              CollisionResult& result_) const
   
     {
       crequest = &request_;
       cresult = &result_;
   
       OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
                                  tree1, 0, tf2, tf1);
     }
   
     template <typename BV>
     void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
                             const Transform3f& tf1, const Transform3f& tf2,
                             const DistanceRequest& request_,
                             DistanceResult& result_) const {
       drequest = &request_;
       dresult = &result_;
   
       OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(),
                                 tree2->getRootBV(), tf1, tf2);
     }
   
     template <typename S>
     void OcTreeShapeIntersect(const OcTree* tree, const S& s,
                               const Transform3f& tf1, const Transform3f& tf2,
                               const CollisionRequest& request_,
                               CollisionResult& result_) const {
       crequest = &request_;
       cresult = &result_;
   
       AABB bv2;
       computeBV<AABB>(s, Transform3f(), bv2);
       OBB obb2;
       convertBV(bv2, tf2, obb2);
       OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
                                   obb2, tf1, tf2);
     }
   
     template <typename S>
     void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
                               const Transform3f& tf1, const Transform3f& tf2,
                               const CollisionRequest& request_,
                               CollisionResult& result_) const {
       crequest = &request_;
       cresult = &result_;
   
       AABB bv1;
       computeBV<AABB>(s, Transform3f(), bv1);
       OBB obb1;
       convertBV(bv1, tf1, obb1);
       OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
                                   obb1, tf2, tf1);
     }
   
     template <typename S>
     void OcTreeShapeDistance(const OcTree* tree, const S& s,
                              const Transform3f& tf1, const Transform3f& tf2,
                              const DistanceRequest& request_,
                              DistanceResult& result_) const {
       drequest = &request_;
       dresult = &result_;
   
       AABB aabb2;
       computeBV<AABB>(s, tf2, aabb2);
       OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
                                  aabb2, tf1, tf2);
     }
   
     template <typename S>
     void ShapeOcTreeDistance(const S& s, const OcTree* tree,
                              const Transform3f& tf1, const Transform3f& tf2,
                              const DistanceRequest& request_,
                              DistanceResult& result_) const {
       drequest = &request_;
       dresult = &result_;
   
       AABB aabb1;
       computeBV<AABB>(s, tf1, aabb1);
       OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
                                  aabb1, tf2, tf1);
     }
   
    private:
     template <typename S>
     bool OcTreeShapeDistanceRecurse(const OcTree* tree1,
                                     const OcTree::OcTreeNode* root1,
                                     const AABB& bv1, const S& s,
                                     const AABB& aabb2, const Transform3f& tf1,
                                     const Transform3f& tf2) const {
       if (!tree1->nodeHasChildren(root1)) {
         if (tree1->isNodeOccupied(root1)) {
           Box box;
           Transform3f box_tf;
           constructBox(bv1, tf1, box, box_tf);
   
           FCL_REAL dist;
           Vec3f closest_p1, closest_p2, normal;
           solver->shapeDistance(box, box_tf, s, tf2, dist, closest_p1, closest_p2,
                                 normal);
   
           dresult->update(dist, tree1, &s, (int)(root1 - tree1->getRoot()),
                           DistanceResult::NONE, closest_p1, closest_p2, normal);
   
           return drequest->isSatisfied(*dresult);
         } else
           return false;
       }
   
       if (!tree1->isNodeOccupied(root1)) return false;
   
       for (unsigned int i = 0; i < 8; ++i) {
         if (tree1->nodeChildExists(root1, i)) {
           const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
           AABB child_bv;
           computeChildBV(bv1, i, child_bv);
   
           AABB aabb1;
           convertBV(child_bv, tf1, aabb1);
           FCL_REAL d = aabb1.distance(aabb2);
           if (d < dresult->min_distance) {
             if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1,
                                            tf2))
               return true;
           }
         }
       }
   
       return false;
     }
   
     template <typename S>
     bool OcTreeShapeIntersectRecurse(const OcTree* tree1,
                                      const OcTree::OcTreeNode* root1,
                                      const AABB& bv1, const S& s, const OBB& obb2,
                                      const Transform3f& tf1,
                                      const Transform3f& tf2) const {
       // Empty OcTree is considered free.
       if (!root1) return false;
   
       if (tree1->isNodeFree(root1))
         return false;
       else if ((tree1->isNodeUncertain(root1) || s.isUncertain()))
         return false;
       else {
         OBB obb1;
         convertBV(bv1, tf1, obb1);
         FCL_REAL sqrDistLowerBound;
         if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
           internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
                                                    sqrDistLowerBound);
           return false;
         }
       }
   
       if (!tree1->nodeHasChildren(root1)) {
         assert(tree1->isNodeOccupied(root1));  // it isn't free nor uncertain.
   
         Box box;
         Transform3f box_tf;
         constructBox(bv1, tf1, box, box_tf);
   
         bool contactNotAdded =
             (cresult->numContacts() >= crequest->num_max_contacts);
         std::size_t ncontact = ShapeShapeCollide<Box, S>(
             &box, box_tf, &s, tf2, solver, *crequest, *cresult);
         assert(ncontact == 0 || ncontact == 1);
         if (!contactNotAdded && ncontact == 1) {
           // Update contact information.
           const Contact& c = cresult->getContact(cresult->numContacts() - 1);
           cresult->setContact(
               cresult->numContacts() - 1,
               Contact(tree1, c.o2, static_cast<int>(root1 - tree1->getRoot()),
                       c.b2, c.pos, c.normal, c.penetration_depth));
         }
   
         return crequest->isSatisfied(*cresult);
       }
   
       for (unsigned int i = 0; i < 8; ++i) {
         if (tree1->nodeChildExists(root1, i)) {
           const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
           AABB child_bv;
           computeChildBV(bv1, i, child_bv);
   
           if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1,
                                           tf2))
             return true;
         }
       }
   
       return false;
     }
   
     template <typename BV>
     bool OcTreeMeshDistanceRecurse(const OcTree* tree1,
                                    const OcTree::OcTreeNode* root1,
                                    const AABB& bv1, const BVHModel<BV>* tree2,
                                    unsigned int root2, const Transform3f& tf1,
                                    const Transform3f& tf2) const {
       if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) {
         if (tree1->isNodeOccupied(root1)) {
           Box box;
           Transform3f box_tf;
           constructBox(bv1, tf1, box, box_tf);
   
           int primitive_id = tree2->getBV(root2).primitiveId();
           const Triangle& tri_id = tree2->tri_indices[primitive_id];
           const Vec3f& p1 = tree2->vertices[tri_id[0]];
           const Vec3f& p2 = tree2->vertices[tri_id[1]];
           const Vec3f& p3 = tree2->vertices[tri_id[2]];
   
           FCL_REAL dist;
           Vec3f closest_p1, closest_p2, normal;
           solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, dist,
                                            closest_p1, closest_p2, normal);
   
           dresult->update(dist, tree1, tree2, (int)(root1 - tree1->getRoot()),
                           primitive_id, closest_p1, closest_p2, normal);
   
           return drequest->isSatisfied(*dresult);
         } else
           return false;
       }
   
       if (!tree1->isNodeOccupied(root1)) return false;
   
       if (tree2->getBV(root2).isLeaf() ||
           (tree1->nodeHasChildren(root1) &&
            (bv1.size() > tree2->getBV(root2).bv.size()))) {
         for (unsigned int i = 0; i < 8; ++i) {
           if (tree1->nodeChildExists(root1, i)) {
             const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
             AABB child_bv;
             computeChildBV(bv1, i, child_bv);
   
             FCL_REAL d;
             AABB aabb1, aabb2;
             convertBV(child_bv, tf1, aabb1);
             convertBV(tree2->getBV(root2).bv, tf2, aabb2);
             d = aabb1.distance(aabb2);
   
             if (d < dresult->min_distance) {
               if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2,
                                             tf1, tf2))
                 return true;
             }
           }
         }
       } else {
         FCL_REAL d;
         AABB aabb1, aabb2;
         convertBV(bv1, tf1, aabb1);
         unsigned int child = (unsigned int)tree2->getBV(root2).leftChild();
         convertBV(tree2->getBV(child).bv, tf2, aabb2);
         d = aabb1.distance(aabb2);
   
         if (d < dresult->min_distance) {
           if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
                                         tf2))
             return true;
         }
   
         child = (unsigned int)tree2->getBV(root2).rightChild();
         convertBV(tree2->getBV(child).bv, tf2, aabb2);
         d = aabb1.distance(aabb2);
   
         if (d < dresult->min_distance) {
           if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
                                         tf2))
             return true;
         }
       }
   
       return false;
     }
   
     template <typename BV>
     bool OcTreeMeshIntersectRecurse(const OcTree* tree1,
                                     const OcTree::OcTreeNode* root1,
                                     const AABB& bv1, const BVHModel<BV>* tree2,
                                     unsigned int root2, const Transform3f& tf1,
                                     const Transform3f& tf2) const {
       // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
       // code in this if(!root1) did not output anything so the empty OcTree is
       // considered free. Should an empty OcTree be considered free ?
   
       // Empty OcTree is considered free.
       if (!root1) return false;
       BVNode<BV> const& bvn2 = tree2->getBV(root2);
   
       if (tree1->isNodeFree(root1))
         return false;
       else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
         return false;
       else {
         OBB obb1, obb2;
         convertBV(bv1, tf1, obb1);
         convertBV(bvn2.bv, tf2, obb2);
         FCL_REAL sqrDistLowerBound;
         if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
           internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
                                                    sqrDistLowerBound);
           return false;
         }
       }
   
       // Check if leaf collides.
       if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
         assert(tree1->isNodeOccupied(root1));  // it isn't free nor uncertain.
         Box box;
         Transform3f box_tf;
         constructBox(bv1, tf1, box, box_tf);
   
         int primitive_id = bvn2.primitiveId();
         const Triangle& tri_id = tree2->tri_indices[primitive_id];
         const Vec3f& p1 = tree2->vertices[tri_id[0]];
         const Vec3f& p2 = tree2->vertices[tri_id[1]];
         const Vec3f& p3 = tree2->vertices[tri_id[2]];
   
         Vec3f c1, c2, normal;
         FCL_REAL distance;
   
         bool collision = solver->shapeTriangleInteraction(
             box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal);
         FCL_REAL distToCollision = distance - crequest->security_margin;
   
         if (cresult->numContacts() < crequest->num_max_contacts) {
           if (collision) {
             cresult->addContact(Contact(tree1, tree2,
                                         (int)(root1 - tree1->getRoot()),
                                         primitive_id, c1, normal, -distance));
           } else if (distToCollision < 0) {
             cresult->addContact(Contact(
                 tree1, tree2, (int)(root1 - tree1->getRoot()), primitive_id,
                 .5 * (c1 + c2), (c2 - c1).normalized(), -distance));
           }
         }
         internal::updateDistanceLowerBoundFromLeaf(*crequest, *cresult,
                                                    distToCollision, c1, c2);
   
         return crequest->isSatisfied(*cresult);
       }
   
       // Determine which tree to traverse first.
       if (bvn2.isLeaf() ||
           (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
         for (unsigned int i = 0; i < 8; ++i) {
           if (tree1->nodeChildExists(root1, i)) {
             const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
             AABB child_bv;
             computeChildBV(bv1, i, child_bv);
   
             if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2,
                                            tf1, tf2))
               return true;
           }
         }
       } else {
         if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
                                        (unsigned int)bvn2.leftChild(), tf1, tf2))
           return true;
   
         if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
                                        (unsigned int)bvn2.rightChild(), tf1, tf2))
           return true;
       }
   
       return false;
     }
   
     bool OcTreeDistanceRecurse(const OcTree* tree1,
                                const OcTree::OcTreeNode* root1, const AABB& bv1,
                                const OcTree* tree2,
                                const OcTree::OcTreeNode* root2, const AABB& bv2,
                                const Transform3f& tf1,
                                const Transform3f& tf2) const {
       if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) {
         if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) {
           Box box1, box2;
           Transform3f box1_tf, box2_tf;
           constructBox(bv1, tf1, box1, box1_tf);
           constructBox(bv2, tf2, box2, box2_tf);
   
           FCL_REAL dist;
           Vec3f closest_p1, closest_p2, normal;
           solver->shapeDistance(box1, box1_tf, box2, box2_tf, dist, closest_p1,
                                 closest_p2, normal);
   
           dresult->update(dist, tree1, tree2, (int)(root1 - tree1->getRoot()),
                           (int)(root2 - tree2->getRoot()), closest_p1, closest_p2,
                           normal);
   
           return drequest->isSatisfied(*dresult);
         } else
           return false;
       }
   
       if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2))
         return false;
   
       if (!tree2->nodeHasChildren(root2) ||
           (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
         for (unsigned int i = 0; i < 8; ++i) {
           if (tree1->nodeChildExists(root1, i)) {
             const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
             AABB child_bv;
             computeChildBV(bv1, i, child_bv);
   
             FCL_REAL d;
             AABB aabb1, aabb2;
             convertBV(bv1, tf1, aabb1);
             convertBV(bv2, tf2, aabb2);
             d = aabb1.distance(aabb2);
   
             if (d < dresult->min_distance) {
               if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2,
                                         tf1, tf2))
                 return true;
             }
           }
         }
       } else {
         for (unsigned int i = 0; i < 8; ++i) {
           if (tree2->nodeChildExists(root2, i)) {
             const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
             AABB child_bv;
             computeChildBV(bv2, i, child_bv);
   
             FCL_REAL d;
             AABB aabb1, aabb2;
             convertBV(bv1, tf1, aabb1);
             convertBV(bv2, tf2, aabb2);
             d = aabb1.distance(aabb2);
   
             if (d < dresult->min_distance) {
               if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv,
                                         tf1, tf2))
                 return true;
             }
           }
         }
       }
   
       return false;
     }
   
     bool OcTreeIntersectRecurse(const OcTree* tree1,
                                 const OcTree::OcTreeNode* root1, const AABB& bv1,
                                 const OcTree* tree2,
                                 const OcTree::OcTreeNode* root2, const AABB& bv2,
                                 const Transform3f& tf1,
                                 const Transform3f& tf2) const {
       // Empty OcTree is considered free.
       if (!root1) return false;
       if (!root2) return false;
   
       if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2))
         return false;
       else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
         return false;
   
       bool bothAreLeaves =
           (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2));
       if (!bothAreLeaves || !crequest->enable_contact) {
         OBB obb1, obb2;
         convertBV(bv1, tf1, obb1);
         convertBV(bv2, tf2, obb2);
         FCL_REAL sqrDistLowerBound;
         if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
           if (cresult->distance_lower_bound > 0 &&
               sqrDistLowerBound <
                   cresult->distance_lower_bound * cresult->distance_lower_bound)
             cresult->distance_lower_bound =
                 sqrt(sqrDistLowerBound) - crequest->security_margin;
           return false;
         }
         if (!crequest->enable_contact) {  // Overlap
           if (cresult->numContacts() < crequest->num_max_contacts)
             cresult->addContact(
                 Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
                         static_cast<int>(root2 - tree2->getRoot())));
           return crequest->isSatisfied(*cresult);
         }
       }
   
       // Both node are leaves
       if (bothAreLeaves) {
         assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2));
   
         Box box1, box2;
         Transform3f box1_tf, box2_tf;
         constructBox(bv1, tf1, box1, box1_tf);
         constructBox(bv2, tf2, box2, box2_tf);
   
         FCL_REAL distance;
         Vec3f c1, c2, normal;
         bool collision = solver->shapeDistance(box1, box1_tf, box2, box2_tf,
                                                distance, c1, c2, normal);
         FCL_REAL distToCollision = distance - crequest->security_margin;
   
         if (cresult->numContacts() < crequest->num_max_contacts) {
           if (collision)
             cresult->addContact(
                 Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
                         static_cast<int>(root2 - tree2->getRoot()), c1, normal,
                         -distance));
           else if (distToCollision <= 0)
             cresult->addContact(
                 Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
                         static_cast<int>(root2 - tree2->getRoot()),
                         .5 * (c1 + c2), (c2 - c1).normalized(), -distance));
         }
         internal::updateDistanceLowerBoundFromLeaf(*crequest, *cresult,
                                                    distToCollision, c1, c2);
   
         return crequest->isSatisfied(*cresult);
       }
   
       // Determine which tree to traverse first.
       if (!tree2->nodeHasChildren(root2) ||
           (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
         for (unsigned int i = 0; i < 8; ++i) {
           if (tree1->nodeChildExists(root1, i)) {
             const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
             AABB child_bv;
             computeChildBV(bv1, i, child_bv);
   
             if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2,
                                        tf1, tf2))
               return true;
           }
         }
       } else {
         for (unsigned int i = 0; i < 8; ++i) {
           if (tree2->nodeChildExists(root2, i)) {
             const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
             AABB child_bv;
             computeChildBV(bv2, i, child_bv);
   
             if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv,
                                        tf1, tf2))
               return true;
           }
         }
       }
   
       return false;
     }
   };
   
   
   class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode
       : public CollisionTraversalNodeBase {
    public:
     OcTreeCollisionTraversalNode(const CollisionRequest& request)
         : CollisionTraversalNodeBase(request) {
       model1 = NULL;
       model2 = NULL;
   
       otsolver = NULL;
     }
   
     bool BVDisjoints(unsigned, unsigned, FCL_REAL&) const { return false; }
   
     void leafCollides(unsigned, unsigned, FCL_REAL& sqrDistLowerBound) const {
       otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
       sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
       sqrDistLowerBound *= sqrDistLowerBound;
     }
   
     const OcTree* model1;
     const OcTree* model2;
   
     Transform3f tf1, tf2;
   
     const OcTreeSolver* otsolver;
   };
   
   template <typename S>
   class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode
       : public CollisionTraversalNodeBase {
    public:
     ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request)
         : CollisionTraversalNodeBase(request) {
       model1 = NULL;
       model2 = NULL;
   
       otsolver = NULL;
     }
   
     bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
       return false;
     }
   
     void leafCollides(unsigned int, unsigned int,
                       FCL_REAL& sqrDistLowerBound) const {
       otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
       sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
       sqrDistLowerBound *= sqrDistLowerBound;
     }
   
     const S* model1;
     const OcTree* model2;
   
     Transform3f tf1, tf2;
   
     const OcTreeSolver* otsolver;
   };
   
   
   template <typename S>
   class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode
       : public CollisionTraversalNodeBase {
    public:
     OcTreeShapeCollisionTraversalNode(const CollisionRequest& request)
         : CollisionTraversalNodeBase(request) {
       model1 = NULL;
       model2 = NULL;
   
       otsolver = NULL;
     }
   
     bool BVDisjoints(unsigned int, unsigned int, fcl::FCL_REAL&) const {
       return false;
     }
   
     void leafCollides(unsigned int, unsigned int,
                       FCL_REAL& sqrDistLowerBound) const {
       otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
       sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
       sqrDistLowerBound *= sqrDistLowerBound;
     }
   
     const OcTree* model1;
     const S* model2;
   
     Transform3f tf1, tf2;
   
     const OcTreeSolver* otsolver;
   };
   
   template <typename BV>
   class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode
       : public CollisionTraversalNodeBase {
    public:
     MeshOcTreeCollisionTraversalNode(const CollisionRequest& request)
         : CollisionTraversalNodeBase(request) {
       model1 = NULL;
       model2 = NULL;
   
       otsolver = NULL;
     }
   
     bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
       return false;
     }
   
     void leafCollides(unsigned int, unsigned int,
                       FCL_REAL& sqrDistLowerBound) const {
       otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
       sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
       sqrDistLowerBound *= sqrDistLowerBound;
     }
   
     const BVHModel<BV>* model1;
     const OcTree* model2;
   
     Transform3f tf1, tf2;
   
     const OcTreeSolver* otsolver;
   };
   
   template <typename BV>
   class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode
       : public CollisionTraversalNodeBase {
    public:
     OcTreeMeshCollisionTraversalNode(const CollisionRequest& request)
         : CollisionTraversalNodeBase(request) {
       model1 = NULL;
       model2 = NULL;
   
       otsolver = NULL;
     }
   
     bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
       return false;
     }
   
     void leafCollides(unsigned int, unsigned int,
                       FCL_REAL& sqrDistLowerBound) const {
       std::cout << "leafCollides" << std::endl;
       otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
       sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
       sqrDistLowerBound *= sqrDistLowerBound;
     }
   
     const OcTree* model1;
     const BVHModel<BV>* model2;
   
     Transform3f tf1, tf2;
   
     const OcTreeSolver* otsolver;
   };
   
   
   
   class HPP_FCL_DLLAPI OcTreeDistanceTraversalNode
       : public DistanceTraversalNodeBase {
    public:
     OcTreeDistanceTraversalNode() {
       model1 = NULL;
       model2 = NULL;
   
       otsolver = NULL;
     }
   
     FCL_REAL BVDistanceLowerBound(unsigned, unsigned) const { return -1; }
   
     bool BVDistanceLowerBound(unsigned, unsigned, FCL_REAL&) const {
       return false;
     }
   
     void leafComputeDistance(unsigned, unsigned int) const {
       otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
     }
   
     const OcTree* model1;
     const OcTree* model2;
   
     const OcTreeSolver* otsolver;
   };
   
   template <typename Shape>
   class HPP_FCL_DLLAPI ShapeOcTreeDistanceTraversalNode
       : public DistanceTraversalNodeBase {
    public:
     ShapeOcTreeDistanceTraversalNode() {
       model1 = NULL;
       model2 = NULL;
   
       otsolver = NULL;
     }
   
     FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
   
     void leafComputeDistance(unsigned int, unsigned int) const {
       otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
     }
   
     const Shape* model1;
     const OcTree* model2;
   
     const OcTreeSolver* otsolver;
   };
   
   template <typename Shape>
   class HPP_FCL_DLLAPI OcTreeShapeDistanceTraversalNode
       : public DistanceTraversalNodeBase {
    public:
     OcTreeShapeDistanceTraversalNode() {
       model1 = NULL;
       model2 = NULL;
   
       otsolver = NULL;
     }
   
     FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
   
     void leafComputeDistance(unsigned int, unsigned int) const {
       otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
     }
   
     const OcTree* model1;
     const Shape* model2;
   
     const OcTreeSolver* otsolver;
   };
   
   template <typename BV>
   class HPP_FCL_DLLAPI MeshOcTreeDistanceTraversalNode
       : public DistanceTraversalNodeBase {
    public:
     MeshOcTreeDistanceTraversalNode() {
       model1 = NULL;
       model2 = NULL;
   
       otsolver = NULL;
     }
   
     FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
   
     void leafComputeDistance(unsigned int, unsigned int) const {
       otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
     }
   
     const BVHModel<BV>* model1;
     const OcTree* model2;
   
     const OcTreeSolver* otsolver;
   };
   
   template <typename BV>
   class HPP_FCL_DLLAPI OcTreeMeshDistanceTraversalNode
       : public DistanceTraversalNodeBase {
    public:
     OcTreeMeshDistanceTraversalNode() {
       model1 = NULL;
       model2 = NULL;
   
       otsolver = NULL;
     }
   
     FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
   
     void leafComputeDistance(unsigned int, unsigned int) const {
       otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
     }
   
     const OcTree* model1;
     const BVHModel<BV>* model2;
   
     const OcTreeSolver* otsolver;
   };
   
   
   }  // namespace fcl
   
   }  // namespace hpp
   
   
   #endif