💿🐜 Antkeeper source code
https://antkeeper.com
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
303 lines
8.1 KiB
303 lines
8.1 KiB
/*
|
|
* Copyright (C) 2023 Christopher J. Howard
|
|
*
|
|
* This file is part of Antkeeper source code.
|
|
*
|
|
* Antkeeper source code is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* Antkeeper source code is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with Antkeeper source code. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#ifndef ANTKEEPER_GEOM_CLOSEST_POINT_HPP
|
|
#define ANTKEEPER_GEOM_CLOSEST_POINT_HPP
|
|
|
|
#include <engine/geom/primitives/hypercapsule.hpp>
|
|
#include <engine/geom/primitives/hyperplane.hpp>
|
|
#include <engine/geom/primitives/hyperrectangle.hpp>
|
|
#include <engine/geom/primitives/hypersphere.hpp>
|
|
#include <engine/geom/primitives/line-segment.hpp>
|
|
#include <engine/geom/primitives/point.hpp>
|
|
#include <engine/geom/primitives/ray.hpp>
|
|
#include <engine/geom/primitives/triangle.hpp>
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <tuple>
|
|
|
|
namespace geom {
|
|
|
|
/**
|
|
* Calculates the closest point on a ray to a point.
|
|
*
|
|
* @tparam T Real type.
|
|
* @tparam N Number of dimensions.
|
|
*
|
|
* @param a Ray a.
|
|
* @param b Point b.
|
|
*
|
|
* @return Closest point on ray a to point b.
|
|
*/
|
|
template <class T, std::size_t N>
|
|
[[nodiscard]] constexpr point<T, N> closest_point(const ray<T, N>& a, const point<T, N>& b) noexcept
|
|
{
|
|
return a.extrapolate(std::max<T>(T{0}, math::dot(b - a.origin, a.direction)));
|
|
}
|
|
|
|
/**
|
|
* Calculates the closest point on a line segment to a point.
|
|
*
|
|
* @tparam T Real type.
|
|
* @tparam N Number of dimensions.
|
|
*
|
|
* @param ab Line segment ab.
|
|
* @param c Point c.
|
|
*
|
|
* @return Closest point on line segment ab to point c.
|
|
*/
|
|
template <class T, std::size_t N>
|
|
[[nodiscard]] constexpr point<T, N> closest_point(const line_segment<T, N>& ab, const point<T, N>& c) noexcept
|
|
{
|
|
const auto direction_ab = ab.b - ab.a;
|
|
|
|
const auto distance_ab = math::dot(c - ab.a, direction_ab);
|
|
if (distance_ab <= T{0})
|
|
{
|
|
return ab.a;
|
|
}
|
|
else
|
|
{
|
|
const auto sqr_length_ab = math::sqr_length(direction_ab);
|
|
if (distance_ab >= sqr_length_ab)
|
|
{
|
|
return ab.b;
|
|
}
|
|
else
|
|
{
|
|
return ab.a + direction_ab * (distance_ab / sqr_length_ab);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Calculates the closest points on two line segments.
|
|
*
|
|
* @tparam T Real type.
|
|
* @tparam N Number of dimensions.
|
|
*
|
|
* @param ab Line segment ab.
|
|
* @param cd Line segment cd.
|
|
*
|
|
* @return Tuple containing the closest point on segment ab to segment cd, followed by the closest point on segment cd to segment ab.
|
|
*/
|
|
template <class T, std::size_t N>
|
|
[[nodiscard]] constexpr std::tuple<point<T, N>, point<T, N>> closest_point(const line_segment<T, N>& ab, const line_segment<T, N>& cd) noexcept
|
|
{
|
|
const auto direction_ab = ab.b - ab.a;
|
|
const auto direction_cd = cd.b - cd.a;
|
|
const auto direction_ca = ab.a - cd.a;
|
|
|
|
const auto sqr_length_ab = math::sqr_length(direction_ab);
|
|
const auto sqr_length_cd = math::sqr_length(direction_cd);
|
|
const auto cd_dot_ca = math::dot(direction_cd, direction_ca);
|
|
|
|
if (sqr_length_ab <= T{0})
|
|
{
|
|
if (sqr_length_cd <= T{0})
|
|
{
|
|
// Both segments are points
|
|
return {ab.a, cd.a};
|
|
}
|
|
else
|
|
{
|
|
// Segment ab is a point
|
|
return
|
|
{
|
|
ab.a,
|
|
cd.a + direction_cd * std::min<T>(std::max<T>(cd_dot_ca / sqr_length_cd, T{0}), T{1})
|
|
};
|
|
}
|
|
}
|
|
else
|
|
{
|
|
const auto ab_dot_ca = math::dot(direction_ab, direction_ca);
|
|
|
|
if (sqr_length_cd <= T{0})
|
|
{
|
|
// Segment cd is a point
|
|
return
|
|
{
|
|
ab.a + direction_ab * std::min<T>(std::max<T>(-ab_dot_ca / sqr_length_ab, T{0}), T{1}),
|
|
cd.a
|
|
};
|
|
}
|
|
else
|
|
{
|
|
const auto ab_dot_cd = math::dot(direction_ab, direction_cd);
|
|
|
|
const auto den = sqr_length_ab * sqr_length_cd - ab_dot_cd * ab_dot_cd;
|
|
|
|
auto distance_ab = (den) ? std::min<T>(std::max<T>((ab_dot_cd * cd_dot_ca - ab_dot_ca * sqr_length_cd) / den, T{0}), T{1}) : T{0};
|
|
auto distance_cd = (ab_dot_cd * distance_ab + cd_dot_ca) / sqr_length_cd;
|
|
|
|
if (distance_cd < T{0})
|
|
{
|
|
return
|
|
{
|
|
ab.a + direction_ab * std::min<T>(std::max<T>(-ab_dot_ca / sqr_length_ab, T{0}), T{1}),
|
|
cd.a
|
|
};
|
|
}
|
|
else if (distance_cd > T{1})
|
|
{
|
|
return
|
|
{
|
|
ab.a + direction_ab * std::min<T>(std::max<T>((ab_dot_cd - ab_dot_ca) / sqr_length_ab, T{0}), T{1}),
|
|
cd.b
|
|
};
|
|
}
|
|
|
|
return
|
|
{
|
|
ab.a + direction_ab * distance_ab,
|
|
cd.a + direction_cd * distance_cd
|
|
};
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Calculates the closest point on a hyperplane to a point.
|
|
*
|
|
* @tparam T Real type.
|
|
* @tparam N Number of dimensions.
|
|
*
|
|
* @param a Hyperplane a.
|
|
* @param b Point b.
|
|
*
|
|
* @return Closest point on hyperplane a to point b.
|
|
*/
|
|
template <class T, std::size_t N>
|
|
[[nodiscard]] constexpr point<T, N> closest_point(const hyperplane<T, N>& a, const point<T, N>& b) noexcept
|
|
{
|
|
return b - a.normal * (math::dot(a.normal, b) + a.constant);
|
|
}
|
|
|
|
/**
|
|
* Calculates the closest point on a triangle to a point.
|
|
*
|
|
* @tparam T Real type.
|
|
*
|
|
* @param tri Triangle.
|
|
* @param a First point of triangle.
|
|
* @param b Second point of triangle.
|
|
* @param c Third point of triangle.
|
|
* @param p Point.
|
|
*
|
|
* @return Closest point on the triangle to point @p p, followed by the index of the edge on which the point lies (`-1` if not on an edge).
|
|
*/
|
|
/// @{
|
|
template <class T>
|
|
[[nodiscard]] constexpr point<T, 3> closest_point(const point<T, 3>& a, const point<T, 3>& b, const point<T, 3>& c, const point<T, 3>& p) noexcept
|
|
{
|
|
const auto ab = b - a;
|
|
const auto ca = a - c;
|
|
const auto ap = p - a;
|
|
const auto n = math::cross(ab, ca);
|
|
const auto d = math::sqr_length(n);
|
|
const auto q = math::cross(n, ap);
|
|
|
|
T u, v, w;
|
|
if ((w = math::dot(q, ab) / d) < T{0})
|
|
{
|
|
v = std::min<T>(std::max<T>(math::dot(ab, ap) / math::sqr_length(ab), T{0}), T{1});
|
|
return {a * (T{1} - v) + b * v};
|
|
}
|
|
else if ((v = math::dot(q, ca) / d) < T{0})
|
|
{
|
|
u = std::min<T>(std::max<T>(math::dot(ca, p - c) / math::sqr_length(ca), T{0}), T{1});
|
|
return {a * u + c * (T{1} - u)};
|
|
}
|
|
else if ((u = T{1} - v - w) < T{0})
|
|
{
|
|
const auto bc = c - b;
|
|
w = std::min<T>(std::max<T>(math::dot(bc, p - b) / math::sqr_length(bc), T{0}), T{1});
|
|
return {b * (T{1} - w) + c * w};
|
|
}
|
|
|
|
return {a * u + b * v + c * w};
|
|
}
|
|
|
|
template <class T>
|
|
[[nodiscard]] inline constexpr point<T, 3> closest_point(const triangle<T, 3>& tri, const point<T, 3>& p) noexcept
|
|
{
|
|
return closest_point(tri.a, tri.b, tri.c, p);
|
|
}
|
|
/// @}
|
|
|
|
/**
|
|
* Calculates the closest point on or in a hypersphere to a point.
|
|
*
|
|
* @tparam T Real type.
|
|
* @tparam N Number of dimensions.
|
|
*
|
|
* @param a Hypersphere a.
|
|
* @param b Point b.
|
|
*
|
|
* @return Closest point on or in hypersphere a to point b.
|
|
*/
|
|
template <class T, std::size_t N>
|
|
[[nodiscard]] point<T, N> closest_point(const hypersphere<T, N>& a, const point<T, N>& b)
|
|
{
|
|
const auto ab = b - a.center;
|
|
const auto d = math::sqr_length(ab);
|
|
return d > a.radius * a.radius ? a.center + ab * (a.radius / std::sqrt(d)) : b;
|
|
}
|
|
|
|
/**
|
|
* Calculates the closest point on or in a hypercapsule to a point.
|
|
*
|
|
* @tparam T Real type.
|
|
* @tparam N Number of dimensions.
|
|
*
|
|
* @param a Hypercapsule a.
|
|
* @param b Point b.
|
|
*
|
|
* @return Closest point on or in hypercapsule a to point b.
|
|
*/
|
|
template <class T, std::size_t N>
|
|
[[nodiscard]] point<T, N> closest_point(const hypercapsule<T, N>& a, const point<T, N>& b)
|
|
{
|
|
const auto c = closest_point(a.segment, b);
|
|
const auto cb = b - c;
|
|
const auto d = math::sqr_length(cb);
|
|
return d > a.radius * a.radius ? c + cb * (a.radius / std::sqrt(d)) : b;
|
|
}
|
|
|
|
/**
|
|
* Calculates the closest point on or in a hyperrectangle to a point.
|
|
*
|
|
* @tparam T Real type.
|
|
* @tparam N Number of dimensions.
|
|
*
|
|
* @param a Hyperrectangle a.
|
|
* @param b Point b.
|
|
*
|
|
* @return Closest point on or in hyperrectangle a to point b.
|
|
*/
|
|
template <class T, std::size_t N>
|
|
[[nodiscard]] constexpr point<T, N> closest_point(const hyperrectangle<T, N>& a, const point<T, N>& b) noexcept
|
|
{
|
|
return math::min(math::max(b, a.min), a.max);
|
|
}
|
|
|
|
} // namespace geom
|
|
|
|
#endif // ANTKEEPER_GEOM_CLOSEST_POINT_HPP
|