💿🐜 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

  1. /*
  2. * Copyright (C) 2023 Christopher J. Howard
  3. *
  4. * This file is part of Antkeeper source code.
  5. *
  6. * Antkeeper source code is free software: you can redistribute it and/or modify
  7. * it under the terms of the GNU General Public License as published by
  8. * the Free Software Foundation, either version 3 of the License, or
  9. * (at your option) any later version.
  10. *
  11. * Antkeeper source code is distributed in the hope that it will be useful,
  12. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  14. * GNU General Public License for more details.
  15. *
  16. * You should have received a copy of the GNU General Public License
  17. * along with Antkeeper source code. If not, see <http://www.gnu.org/licenses/>.
  18. */
  19. #ifndef ANTKEEPER_GEOM_CLOSEST_POINT_HPP
  20. #define ANTKEEPER_GEOM_CLOSEST_POINT_HPP
  21. #include <engine/geom/primitives/hypercapsule.hpp>
  22. #include <engine/geom/primitives/hyperplane.hpp>
  23. #include <engine/geom/primitives/hyperrectangle.hpp>
  24. #include <engine/geom/primitives/hypersphere.hpp>
  25. #include <engine/geom/primitives/line-segment.hpp>
  26. #include <engine/geom/primitives/point.hpp>
  27. #include <engine/geom/primitives/ray.hpp>
  28. #include <engine/geom/primitives/triangle.hpp>
  29. #include <algorithm>
  30. #include <cmath>
  31. #include <tuple>
  32. namespace geom {
  33. /**
  34. * Calculates the closest point on a ray to a point.
  35. *
  36. * @tparam T Real type.
  37. * @tparam N Number of dimensions.
  38. *
  39. * @param a Ray a.
  40. * @param b Point b.
  41. *
  42. * @return Closest point on ray a to point b.
  43. */
  44. template <class T, std::size_t N>
  45. [[nodiscard]] constexpr point<T, N> closest_point(const ray<T, N>& a, const point<T, N>& b) noexcept
  46. {
  47. return a.extrapolate(std::max<T>(T{0}, math::dot(b - a.origin, a.direction)));
  48. }
  49. /**
  50. * Calculates the closest point on a line segment to a point.
  51. *
  52. * @tparam T Real type.
  53. * @tparam N Number of dimensions.
  54. *
  55. * @param ab Line segment ab.
  56. * @param c Point c.
  57. *
  58. * @return Closest point on line segment ab to point c.
  59. */
  60. template <class T, std::size_t N>
  61. [[nodiscard]] constexpr point<T, N> closest_point(const line_segment<T, N>& ab, const point<T, N>& c) noexcept
  62. {
  63. const auto direction_ab = ab.b - ab.a;
  64. const auto distance_ab = math::dot(c - ab.a, direction_ab);
  65. if (distance_ab <= T{0})
  66. {
  67. return ab.a;
  68. }
  69. else
  70. {
  71. const auto sqr_length_ab = math::sqr_length(direction_ab);
  72. if (distance_ab >= sqr_length_ab)
  73. {
  74. return ab.b;
  75. }
  76. else
  77. {
  78. return ab.a + direction_ab * (distance_ab / sqr_length_ab);
  79. }
  80. }
  81. }
  82. /**
  83. * Calculates the closest points on two line segments.
  84. *
  85. * @tparam T Real type.
  86. * @tparam N Number of dimensions.
  87. *
  88. * @param ab Line segment ab.
  89. * @param cd Line segment cd.
  90. *
  91. * @return Tuple containing the closest point on segment ab to segment cd, followed by the closest point on segment cd to segment ab.
  92. */
  93. template <class T, std::size_t N>
  94. [[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
  95. {
  96. const auto direction_ab = ab.b - ab.a;
  97. const auto direction_cd = cd.b - cd.a;
  98. const auto direction_ca = ab.a - cd.a;
  99. const auto sqr_length_ab = math::sqr_length(direction_ab);
  100. const auto sqr_length_cd = math::sqr_length(direction_cd);
  101. const auto cd_dot_ca = math::dot(direction_cd, direction_ca);
  102. if (sqr_length_ab <= T{0})
  103. {
  104. if (sqr_length_cd <= T{0})
  105. {
  106. // Both segments are points
  107. return {ab.a, cd.a};
  108. }
  109. else
  110. {
  111. // Segment ab is a point
  112. return
  113. {
  114. ab.a,
  115. cd.a + direction_cd * std::min<T>(std::max<T>(cd_dot_ca / sqr_length_cd, T{0}), T{1})
  116. };
  117. }
  118. }
  119. else
  120. {
  121. const auto ab_dot_ca = math::dot(direction_ab, direction_ca);
  122. if (sqr_length_cd <= T{0})
  123. {
  124. // Segment cd is a point
  125. return
  126. {
  127. ab.a + direction_ab * std::min<T>(std::max<T>(-ab_dot_ca / sqr_length_ab, T{0}), T{1}),
  128. cd.a
  129. };
  130. }
  131. else
  132. {
  133. const auto ab_dot_cd = math::dot(direction_ab, direction_cd);
  134. const auto den = sqr_length_ab * sqr_length_cd - ab_dot_cd * ab_dot_cd;
  135. 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};
  136. auto distance_cd = (ab_dot_cd * distance_ab + cd_dot_ca) / sqr_length_cd;
  137. if (distance_cd < T{0})
  138. {
  139. return
  140. {
  141. ab.a + direction_ab * std::min<T>(std::max<T>(-ab_dot_ca / sqr_length_ab, T{0}), T{1}),
  142. cd.a
  143. };
  144. }
  145. else if (distance_cd > T{1})
  146. {
  147. return
  148. {
  149. ab.a + direction_ab * std::min<T>(std::max<T>((ab_dot_cd - ab_dot_ca) / sqr_length_ab, T{0}), T{1}),
  150. cd.b
  151. };
  152. }
  153. return
  154. {
  155. ab.a + direction_ab * distance_ab,
  156. cd.a + direction_cd * distance_cd
  157. };
  158. }
  159. }
  160. }
  161. /**
  162. * Calculates the closest point on a hyperplane to a point.
  163. *
  164. * @tparam T Real type.
  165. * @tparam N Number of dimensions.
  166. *
  167. * @param a Hyperplane a.
  168. * @param b Point b.
  169. *
  170. * @return Closest point on hyperplane a to point b.
  171. */
  172. template <class T, std::size_t N>
  173. [[nodiscard]] constexpr point<T, N> closest_point(const hyperplane<T, N>& a, const point<T, N>& b) noexcept
  174. {
  175. return b - a.normal * (math::dot(a.normal, b) + a.constant);
  176. }
  177. /**
  178. * Calculates the closest point on a triangle to a point.
  179. *
  180. * @tparam T Real type.
  181. *
  182. * @param tri Triangle.
  183. * @param a First point of triangle.
  184. * @param b Second point of triangle.
  185. * @param c Third point of triangle.
  186. * @param p Point.
  187. *
  188. * @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).
  189. */
  190. /// @{
  191. template <class T>
  192. [[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
  193. {
  194. const auto ab = b - a;
  195. const auto ca = a - c;
  196. const auto ap = p - a;
  197. const auto n = math::cross(ab, ca);
  198. const auto d = math::sqr_length(n);
  199. const auto q = math::cross(n, ap);
  200. T u, v, w;
  201. if ((w = math::dot(q, ab) / d) < T{0})
  202. {
  203. v = std::min<T>(std::max<T>(math::dot(ab, ap) / math::sqr_length(ab), T{0}), T{1});
  204. return {a * (T{1} - v) + b * v};
  205. }
  206. else if ((v = math::dot(q, ca) / d) < T{0})
  207. {
  208. u = std::min<T>(std::max<T>(math::dot(ca, p - c) / math::sqr_length(ca), T{0}), T{1});
  209. return {a * u + c * (T{1} - u)};
  210. }
  211. else if ((u = T{1} - v - w) < T{0})
  212. {
  213. const auto bc = c - b;
  214. w = std::min<T>(std::max<T>(math::dot(bc, p - b) / math::sqr_length(bc), T{0}), T{1});
  215. return {b * (T{1} - w) + c * w};
  216. }
  217. return {a * u + b * v + c * w};
  218. }
  219. template <class T>
  220. [[nodiscard]] inline constexpr point<T, 3> closest_point(const triangle<T, 3>& tri, const point<T, 3>& p) noexcept
  221. {
  222. return closest_point(tri.a, tri.b, tri.c, p);
  223. }
  224. /// @}
  225. /**
  226. * Calculates the closest point on or in a hypersphere to a point.
  227. *
  228. * @tparam T Real type.
  229. * @tparam N Number of dimensions.
  230. *
  231. * @param a Hypersphere a.
  232. * @param b Point b.
  233. *
  234. * @return Closest point on or in hypersphere a to point b.
  235. */
  236. template <class T, std::size_t N>
  237. [[nodiscard]] point<T, N> closest_point(const hypersphere<T, N>& a, const point<T, N>& b)
  238. {
  239. const auto ab = b - a.center;
  240. const auto d = math::sqr_length(ab);
  241. return d > a.radius * a.radius ? a.center + ab * (a.radius / std::sqrt(d)) : b;
  242. }
  243. /**
  244. * Calculates the closest point on or in a hypercapsule to a point.
  245. *
  246. * @tparam T Real type.
  247. * @tparam N Number of dimensions.
  248. *
  249. * @param a Hypercapsule a.
  250. * @param b Point b.
  251. *
  252. * @return Closest point on or in hypercapsule a to point b.
  253. */
  254. template <class T, std::size_t N>
  255. [[nodiscard]] point<T, N> closest_point(const hypercapsule<T, N>& a, const point<T, N>& b)
  256. {
  257. const auto c = closest_point(a.segment, b);
  258. const auto cb = b - c;
  259. const auto d = math::sqr_length(cb);
  260. return d > a.radius * a.radius ? c + cb * (a.radius / std::sqrt(d)) : b;
  261. }
  262. /**
  263. * Calculates the closest point on or in a hyperrectangle to a point.
  264. *
  265. * @tparam T Real type.
  266. * @tparam N Number of dimensions.
  267. *
  268. * @param a Hyperrectangle a.
  269. * @param b Point b.
  270. *
  271. * @return Closest point on or in hyperrectangle a to point b.
  272. */
  273. template <class T, std::size_t N>
  274. [[nodiscard]] constexpr point<T, N> closest_point(const hyperrectangle<T, N>& a, const point<T, N>& b) noexcept
  275. {
  276. return math::min(math::max(b, a.min), a.max);
  277. }
  278. } // namespace geom
  279. #endif // ANTKEEPER_GEOM_CLOSEST_POINT_HPP