Geometry.hpp

Go to the documentation of this file.

// Copyright 2025 Intelligent Robotics Lab
//
// This file is part of the project Easy Navigation (EasyNav in short)
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

 
#ifndef NAVMAP_CORE__GEOMETRY_HPP_
#define NAVMAP_CORE__GEOMETRY_HPP_
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>
#include <cmath>
#include <algorithm>
#include <utility>
 
namespace navmap
{

using Vec3 = Eigen::Vector3f;
 
// -----------------------------------------------------------------------------
// Basic triangle geometry
// -----------------------------------------------------------------------------

inline float triangle_area(const Vec3 & a, const Vec3 & b, const Vec3 & c)
{
  return 0.5f * ((b - a).cross(c - a)).norm();
}

inline Vec3 triangle_normal(const Vec3 & a, const Vec3 & b, const Vec3 & c)
{
  Vec3 n = (b - a).cross(c - a);
  float len = n.norm();
  return len > 1e-12f ? n / len : Vec3(0.0f, 0.0f, 1.0f);
}

inline bool ray_triangle_intersect(
  const Vec3 & orig,
  const Vec3 & dir,
  const Vec3 & v0,
  const Vec3 & v1,
  const Vec3 & v2,
  float & t,
  float & u,
  float & v)
{
  const float kEps = 1e-8f;
  // Barycentric slack, relative: a ray through a shared edge hits both NavCels, never neither
  const float kBary = 1e-5f;
  Vec3 e1 = v1 - v0;
  Vec3 e2 = v2 - v0;
  Vec3 pvec = dir.cross(e2);
  float det = e1.dot(pvec);
  if (std::fabs(det) < kEps) {
    return false;
  }
  float inv_det = 1.0f / det;
  Vec3 tvec = orig - v0;
  u = tvec.dot(pvec) * inv_det;
  if (u < -kBary || u > 1.0f + kBary) {
    return false;
  }
  Vec3 qvec = tvec.cross(e1);
  v = dir.dot(qvec) * inv_det;
  if (v < -kBary || (u + v) > 1.0f + kBary) {
    return false;
  }
  t = e2.dot(qvec) * inv_det;
  return t > kEps;
}
 
// -----------------------------------------------------------------------------
// Axis-aligned bounding box (AABB)
// -----------------------------------------------------------------------------

inline float point_segment_distance(const Vec3 & p, const Vec3 & a, const Vec3 & b)
{
  const Vec3 ab = b - a;
  const float len2 = ab.squaredNorm();
  const float t = len2 > 0.0f ? std::clamp((p - a).dot(ab) / len2, 0.0f, 1.0f) : 0.0f;
  return (p - (a + t * ab)).norm();
}

struct AABB
{
  Vec3 min{std::numeric_limits<float>::infinity(),
    std::numeric_limits<float>::infinity(),
    std::numeric_limits<float>::infinity()};

  Vec3 max{-std::numeric_limits<float>::infinity(),
    -std::numeric_limits<float>::infinity(),
    -std::numeric_limits<float>::infinity()};

  inline void expand(const Vec3 & p)
  {
    min = min.cwiseMin(p);
    max = max.cwiseMax(p);
  }

  inline void expand(const AABB & b)
  {
    min = min.cwiseMin(b.min);
    max = max.cwiseMax(b.max);
  }

  inline int longest_axis() const
  {
    Vec3 d = max - min;
    if (d.x() >= d.y() && d.x() >= d.z()) {return 0;}
    if (d.y() >= d.x() && d.y() >= d.z()) {return 1;}
    return 2;
  }

  inline bool intersects_ray(
    const Vec3 & o,
    const Vec3 & d,
    float tmax = 1e30f) const
  {
    const float kEps = 1e-12f;
    float tmin_local = 0.0f;
 
    for (int i = 0; i < 3; ++i) {
      const float di = d[i];
      const float oi = o[i];
      const float min_i = min[i];
      const float max_i = max[i];
 
      if (std::fabs(di) < kEps) {
        // Parallel to this slab: must be inside [min_i, max_i]
        if (oi < min_i || oi > max_i) {return false;}
        continue;
      }
 
      float inv = 1.0f / di;
      float t0 = (min_i - oi) * inv;
      float t1 = (max_i - oi) * inv;
      if (t0 > t1) {std::swap(t0, t1);}
 
      if (t0 > tmin_local) {tmin_local = t0;}
      if (t1 < tmax) {tmax = t1;}
 
      if (tmax < tmin_local) {return false;}
    }
    return tmax >= tmin_local && tmax > 0.0f;
  }

  inline bool contains_xy(const Vec3 & p, float z_eps) const
  {
    return p.x() >= min.x() && p.x() <= max.x() &&
           p.y() >= min.y() && p.y() <= max.y() &&
           p.z() >= min.z() - z_eps && p.z() <= max.z() + z_eps;
  }

  inline bool contains_xy_only(const Vec3 & p) const
  {
    return p.x() >= min.x() && p.x() <= max.x() &&
           p.y() >= min.y() && p.y() <= max.y();
  }
};
 
// -----------------------------------------------------------------------------
// Closest point on triangle and point-triangle distance
// -----------------------------------------------------------------------------

inline Vec3 closest_point_on_triangle(
  const Vec3 & p,
  const Vec3 & a,
  const Vec3 & b,
  const Vec3 & c)
{
  const Vec3 ab = b - a;
  const Vec3 ac = c - a;
  const Vec3 ap = p - a;
 
  float d1 = ab.dot(ap);
  float d2 = ac.dot(ap);
  if (d1 <= 0.0f && d2 <= 0.0f) {
    return a;
  }
 
  const Vec3 bp = p - b;
  float d3 = ab.dot(bp);
  float d4 = ac.dot(bp);
  if (d3 >= 0.0f && d4 <= d3) {
    return b;
  }
 
  float vc = d1 * d4 - d3 * d2;
  if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
    float v = d1 / (d1 - d3);
    return a + v * ab;
  }
 
  const Vec3 cp = p - c;
  float d5 = ab.dot(cp);
  float d6 = ac.dot(cp);
  if (d6 >= 0.0f && d5 <= d6) {
    return c;
  }
 
  float vb = d5 * d2 - d1 * d6;
  if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
    float w = d2 / (d2 - d6);
    return a + w * ac;
  }
 
  float va = d3 * d6 - d5 * d4;
  if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
    float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
    return b + w * (c - b);
  }
 
  float ab2 = ab.dot(ab);
  float ac2 = ac.dot(ac);
  float abac = ab.dot(ac);
  float denom = (ab2 * ac2 - abac * abac);
  if (std::fabs(denom) < 1e-20f) {
    float da = (p - a).squaredNorm();
    float db = (p - b).squaredNorm();
    float dc = (p - c).squaredNorm();
    if (da <= db && da <= dc) {return a;}
    if (db <= dc) {return b;}
    return c;
  }
  float v = (ac2 * d1 - abac * d2) / denom;
  float w = (ab2 * d2 - abac * d1) / denom;
  return a + v * ab + w * ac;
}

inline float point_triangle_squared_distance(
  const Vec3 & p,
  const Vec3 & a,
  const Vec3 & b,
  const Vec3 & c,
  Vec3 * closest = nullptr)
{
  Vec3 q = closest_point_on_triangle(p, a, b, c);
  if (closest) {*closest = q;}
  return (q - p).squaredNorm();
}
 
}  // namespace navmap
 
#endif  // NAVMAP_CORE__GEOMETRY_HPP_