Method rayTriangleIntersect

grid_map_pcl/src/GridMapPclConverter.cpp:83–133

}

bool GridMapPclConverter::rayTriangleIntersect(const Eigen::Vector3f& point, const Eigen::Vector3f& ray,
                                               const Eigen::Matrix3f& triangleVertexMatrix, Eigen::Vector3f& intersectionPoint) {
  // Algorithm here is adapted from:
  // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
  //
  // Original copyright notice:
  // Copyright 2001, softSurfer (www.softsurfer.com)
  // This code may be freely used and modified for any purpose
  // providing that this copyright notice is included with it.

  const Eigen::Vector3f a = triangleVertexMatrix.row(0);
  const Eigen::Vector3f b = triangleVertexMatrix.row(1);
  const Eigen::Vector3f c = triangleVertexMatrix.row(2);
  const Eigen::Vector3f u = b - a;
  const Eigen::Vector3f v = c - a;
  const Eigen::Vector3f n = u.cross(v);
  const float n_dot_ray = n.dot(ray);

  if (std::fabs(n_dot_ray) < 1e-9) {
    return false;
  }

  const float r = n.dot(a - point) / n_dot_ray;

  if (r < 0) {
    return false;
  }

  // Note(alexmillane): The addition of this comparison delta (not in the
  // original algorithm) means that rays intersecting the edge of triangles are
  // treated at hits.
  constexpr float delta = 1e-5;

  const Eigen::Vector3f w = point + r * ray - a;
  const float denominator = u.dot(v) * u.dot(v) - u.dot(u) * v.dot(v);
  const float s_numerator = u.dot(v) * w.dot(v) - v.dot(v) * w.dot(u);
  const float s = s_numerator / denominator;
  if (s < (0 - delta) || s > (1 + delta)) {
    return false;
  }

  const float t_numerator = u.dot(v) * w.dot(u) - u.dot(u) * w.dot(v);
  const float t = t_numerator / denominator;
  if (t < (0 - delta) || s + t > (1 + delta)) {
    return false;
  }

  intersectionPoint = a + s * u + t * v;

  return true;
}

} /* namespace grid_map */