intersect.hxx

Go to the documentation of this file.

/*
 * 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