.. _program_listing_file_include_coal_internal_intersect.hxx:

Program Listing for File intersect.hxx
======================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_coal_internal_intersect.hxx>` (``include/coal/internal/intersect.hxx``)

.. |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 COAL_INTERSECT_HXX
   #define COAL_INTERSECT_HXX
   
   #include "coal/internal/intersect.h"
   #include "coal/internal/tools.h"
   
   #include <iostream>
   #include <limits>
   #include <vector>
   #include <cmath>
   
   namespace coal {
   
   template <typename Scalar>
   inline typename Project<Scalar>::ProjectResult Project<Scalar>::projectLine(
       const Vec3& a, const Vec3& b, const Vec3& p) {
     ProjectResult res;
   
     const Vec3 d = b - a;
     const Scalar l = d.squaredNorm();
   
     if (l > 0) {
       const Scalar t = (p - a).dot(d);
       res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l));
       res.parameterization[0] = 1 - res.parameterization[1];
       if (t >= l) {
         res.sqr_distance = (p - b).squaredNorm();
         res.encode = 2; /* 0x10 */
       } else if (t <= 0) {
         res.sqr_distance = (p - a).squaredNorm();
         res.encode = 1; /* 0x01 */
       } else {
         res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm();
         res.encode = 3; /* 0x00 */
       }
     }
   
     return res;
   }
   
   template <typename Scalar>
   inline typename Project<Scalar>::ProjectResult Project<Scalar>::projectTriangle(
       const Vec3& a, const Vec3& b, const Vec3& c, const Vec3& p) {
     ProjectResult res;
   
     static const size_t nexti[3] = {1, 2, 0};
     const Vec3* vt[] = {&a, &b, &c};
     const Vec3 dl[] = {a - b, b - c, c - a};
     const Vec3& n = dl[0].cross(dl[1]);
     const Scalar l = n.squaredNorm();
   
     if (l > 0) {
       Scalar mindist = -1;
       for (size_t i = 0; i < 3; ++i) {
         if ((*vt[i] - p).dot(dl[i].cross(n)) >
             0)  // origin is to the outside part of the triangle edge, then the
                 // optimal can only be on the edge
         {
           size_t j = nexti[i];
           ProjectResult res_line = projectLine(*vt[i], *vt[j], p);
   
           if (mindist < 0 || res_line.sqr_distance < mindist) {
             mindist = res_line.sqr_distance;
             res.encode =
                 static_cast<unsigned int>(((res_line.encode & 1) ? 1 << i : 0) +
                                           ((res_line.encode & 2) ? 1 << j : 0));
             res.parameterization[i] = res_line.parameterization[0];
             res.parameterization[j] = res_line.parameterization[1];
             res.parameterization[nexti[j]] = 0;
           }
         }
       }
   
       if (mindist < 0)  // the origin project is within the triangle
       {
         Scalar d = (a - p).dot(n);
         Scalar s = std::sqrt(l);
         Vec3 p_to_project = n * (d / l);
         mindist = p_to_project.squaredNorm();
         res.encode = 7;  // m = 0x111
         res.parameterization[0] = dl[1].cross(b - p - p_to_project).norm() / s;
         res.parameterization[1] = dl[2].cross(c - p - p_to_project).norm() / s;
         res.parameterization[2] =
             1 - res.parameterization[0] - res.parameterization[1];
       }
   
       res.sqr_distance = mindist;
     }
   
     return res;
   }
   
   template <typename Scalar>
   inline typename Project<Scalar>::ProjectResult
   Project<Scalar>::projectTetrahedra(const Vec3& a, const Vec3& b, const Vec3& c,
                                      const Vec3& d, const Vec3& p) {
     ProjectResult res;
   
     static const size_t nexti[] = {1, 2, 0};
     const Vec3* vt[] = {&a, &b, &c, &d};
     const Vec3 dl[3] = {a - d, b - d, c - d};
     Scalar vl = triple(dl[0], dl[1], dl[2]);
     bool ng = (vl * (a - p).dot((b - c).cross(a - b))) <= 0;
     if (ng &&
         std::abs(vl) > 0)  // abs(vl) == 0, the tetrahedron is degenerated; if ng
                            // is false, then the last vertex in the tetrahedron
                            // does not grow toward the origin (in fact origin is
                            // on the other side of the abc face)
     {
       Scalar mindist = -1;
   
       for (size_t i = 0; i < 3; ++i) {
         size_t j = nexti[i];
         Scalar s = vl * (d - p).dot(dl[i].cross(dl[j]));
         if (s > 0)  // the origin is to the outside part of a triangle face, then
                     // the optimal can only be on the triangle face
         {
           ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p);
           if (mindist < 0 || res_triangle.sqr_distance < mindist) {
             mindist = res_triangle.sqr_distance;
             res.encode =
                 static_cast<unsigned int>((res_triangle.encode & 1 ? 1 << i : 0) +
                                           (res_triangle.encode & 2 ? 1 << j : 0) +
                                           (res_triangle.encode & 4 ? 8 : 0));
             res.parameterization[i] = res_triangle.parameterization[0];
             res.parameterization[j] = res_triangle.parameterization[1];
             res.parameterization[nexti[j]] = 0;
             res.parameterization[3] = res_triangle.parameterization[2];
           }
         }
       }
   
       if (mindist < 0) {
         mindist = 0;
         res.encode = 15;
         res.parameterization[0] = triple(c - p, b - p, d - p) / vl;
         res.parameterization[1] = triple(a - p, c - p, d - p) / vl;
         res.parameterization[2] = triple(b - p, a - p, d - p) / vl;
         res.parameterization[3] =
             1 - (res.parameterization[0] + res.parameterization[1] +
                  res.parameterization[2]);
       }
   
       res.sqr_distance = mindist;
     } else if (!ng) {
       res = projectTriangle(a, b, c, p);
       res.parameterization[3] = 0;
     }
     return res;
   }
   
   template <typename Scalar>
   inline typename Project<Scalar>::ProjectResult
   Project<Scalar>::projectLineOrigin(const Vec3& a, const Vec3& b) {
     ProjectResult res;
   
     const Vec3 d = b - a;
     const Scalar l = d.squaredNorm();
   
     if (l > 0) {
       const Scalar t = -a.dot(d);
       res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l));
       res.parameterization[0] = 1 - res.parameterization[1];
       if (t >= l) {
         res.sqr_distance = b.squaredNorm();
         res.encode = 2; /* 0x10 */
       } else if (t <= 0) {
         res.sqr_distance = a.squaredNorm();
         res.encode = 1; /* 0x01 */
       } else {
         res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm();
         res.encode = 3; /* 0x00 */
       }
     }
   
     return res;
   }
   
   template <typename Scalar>
   inline typename Project<Scalar>::ProjectResult
   Project<Scalar>::projectTriangleOrigin(const Vec3& a, const Vec3& b,
                                          const Vec3& c) {
     ProjectResult res;
   
     static const size_t nexti[3] = {1, 2, 0};
     const Vec3* vt[] = {&a, &b, &c};
     const Vec3 dl[] = {a - b, b - c, c - a};
     const Vec3& n = dl[0].cross(dl[1]);
     const Scalar l = n.squaredNorm();
   
     if (l > 0) {
       Scalar mindist = -1;
       for (size_t i = 0; i < 3; ++i) {
         if (vt[i]->dot(dl[i].cross(n)) >
             0)  // origin is to the outside part of the triangle edge, then the
                 // optimal can only be on the edge
         {
           size_t j = nexti[i];
           ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]);
   
           if (mindist < 0 || res_line.sqr_distance < mindist) {
             mindist = res_line.sqr_distance;
             res.encode =
                 static_cast<unsigned int>(((res_line.encode & 1) ? 1 << i : 0) +
                                           ((res_line.encode & 2) ? 1 << j : 0));
             res.parameterization[i] = res_line.parameterization[0];
             res.parameterization[j] = res_line.parameterization[1];
             res.parameterization[nexti[j]] = 0;
           }
         }
       }
   
       if (mindist < 0)  // the origin project is within the triangle
       {
         Scalar d = a.dot(n);
         Scalar s = std::sqrt(l);
         Vec3 o_to_project = n * (d / l);
         mindist = o_to_project.squaredNorm();
         res.encode = 7;  // m = 0x111
         res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s;
         res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s;
         res.parameterization[2] =
             1 - res.parameterization[0] - res.parameterization[1];
       }
   
       res.sqr_distance = mindist;
     }
   
     return res;
   }
   
   template <typename Scalar>
   inline typename Project<Scalar>::ProjectResult
   Project<Scalar>::projectTetrahedraOrigin(const Vec3& a, const Vec3& b,
                                            const Vec3& c, const Vec3& d) {
     ProjectResult res;
   
     static const size_t nexti[] = {1, 2, 0};
     const Vec3* vt[] = {&a, &b, &c, &d};
     const Vec3 dl[3] = {a - d, b - d, c - d};
     Scalar vl = triple(dl[0], dl[1], dl[2]);
     bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0;
     if (ng &&
         std::abs(vl) > 0)  // abs(vl) == 0, the tetrahedron is degenerated; if ng
                            // is false, then the last vertex in the tetrahedron
                            // does not grow toward the origin (in fact origin is
                            // on the other side of the abc face)
     {
       Scalar mindist = -1;
   
       for (size_t i = 0; i < 3; ++i) {
         size_t j = nexti[i];
         Scalar s = vl * d.dot(dl[i].cross(dl[j]));
         if (s > 0)  // the origin is to the outside part of a triangle face, then
                     // the optimal can only be on the triangle face
         {
           ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d);
           if (mindist < 0 || res_triangle.sqr_distance < mindist) {
             mindist = res_triangle.sqr_distance;
             res.encode =
                 static_cast<unsigned int>((res_triangle.encode & 1 ? 1 << i : 0) +
                                           (res_triangle.encode & 2 ? 1 << j : 0) +
                                           (res_triangle.encode & 4 ? 8 : 0));
             res.parameterization[i] = res_triangle.parameterization[0];
             res.parameterization[j] = res_triangle.parameterization[1];
             res.parameterization[nexti[j]] = 0;
             res.parameterization[3] = res_triangle.parameterization[2];
           }
         }
       }
   
       if (mindist < 0) {
         mindist = 0;
         res.encode = 15;
         res.parameterization[0] = triple(c, b, d) / vl;
         res.parameterization[1] = triple(a, c, d) / vl;
         res.parameterization[2] = triple(b, a, d) / vl;
         res.parameterization[3] =
             1 - (res.parameterization[0] + res.parameterization[1] +
                  res.parameterization[2]);
       }
   
       res.sqr_distance = mindist;
     } else if (!ng) {
       res = projectTriangleOrigin(a, b, c);
       res.parameterization[3] = 0;
     }
     return res;
   }
   
   }  // namespace coal
   
   #endif  // COAL_INTERSECT_HXX