Commit 1fdfc9af authored by Aaron Grabowy's avatar Aaron Grabowy
Browse files

Improved distance between segements; Added distance between line and segement

parent 636f3284
......@@ -2,14 +2,9 @@
#include <typed-geometry/detail/utility.hh>
#include <typed-geometry/types/objects/line.hh>
#include <typed-geometry/types/objects/plane.hh>
#include <typed-geometry/types/objects/segment.hh>
#include <typed-geometry/types/pos.hh>
#include <typed-geometry/types/quadric.hh>
#include <typed-geometry/functions/basic/mix.hh>
#include "coordinates.hh"
#include "project.hh"
// closest_points(a, b) returns points {p_a, p_b} such that contains(a, p_a) and contains(b, p_b) and |p_a - p_b| is minimal
......@@ -45,7 +40,7 @@ template <class ScalarT>
auto d0d1 = dot(l0.dir, l1.dir);
auto b0 = dot(l1.pos - l0.pos, l0.dir);
auto b1 = dot(l1.pos - l0.pos, l1.dir);
auto [t0, t1] = inverse(tg::mat<2, 2, ScalarT>::from_cols({1, d0d1}, {-d0d1, -1})) * tg::vec2(b0, b1);
auto [t0, t1] = inverse(mat<2, 2, ScalarT>::from_cols({1, d0d1}, {-d0d1, -1})) * vec2(b0, b1);
return {t0, t1};
}
......
......@@ -91,45 +91,59 @@ template <class ScalarT>
}
template <class ScalarT>
[[nodiscard]] constexpr fractional_result<ScalarT> distance(segment<2, ScalarT> const& s0, segment<2, ScalarT> const& s1)
[[nodiscard]] constexpr fractional_result<ScalarT> distance_sqr(segment<2, ScalarT> const& s0, segment<2, ScalarT> const& s1)
{
auto l0 = line<2, ScalarT>::from_points(s0.pos0, s0.pos1);
auto l1 = line<2, ScalarT>::from_points(s1.pos0, s1.pos1);
auto sl0 = distance(s0.pos0, s0.pos1);
auto sl1 = distance(s1.pos0, s1.pos1);
auto l0 = inf_of(s0);
auto l1 = inf_of(s1);
auto len0 = length(s0);
auto len1 = length(s1);
auto [t0, t1] = intersection_parameters(l0, l1);
if (ScalarT(0) <= t0 && t0 <= ScalarT(sl0) && //
ScalarT(0) <= t1 && t1 <= ScalarT(sl1))
if (ScalarT(0) <= t0 && t0 <= ScalarT(len0) && //
ScalarT(0) <= t1 && t1 <= ScalarT(len1))
return ScalarT(0); // intersects
auto p0 = t0 < ScalarT(0) ? s0.pos0 : s0.pos1;
auto p1 = t1 < ScalarT(0) ? s1.pos0 : s1.pos1;
auto p0 = t0 * ScalarT(2) < len0 ? s0.pos0 : s0.pos1;
auto p1 = t1 * ScalarT(2) < len1 ? s1.pos0 : s1.pos1;
return min(distance(p0, s1), distance(p1, s0));
return min(distance_sqr(p0, s1), distance_sqr(p1, s0));
}
template <class ScalarT>
[[nodiscard]] constexpr fractional_result<ScalarT> distance(segment<3, ScalarT> const& s0, segment<3, ScalarT> const& s1)
[[nodiscard]] constexpr fractional_result<ScalarT> distance_sqr(segment<3, ScalarT> const& s0, segment<3, ScalarT> const& s1)
{
auto l0 = line<3, ScalarT>::from_points(s0.pos0, s0.pos1);
auto l1 = line<3, ScalarT>::from_points(s1.pos0, s1.pos1);
auto sl0 = distance(s0.pos0, s0.pos1);
auto sl1 = distance(s1.pos0, s1.pos1);
auto l0 = inf_of(s0);
auto l1 = inf_of(s1);
auto len0 = length(s0);
auto len1 = length(s1);
auto [t0, t1] = closest_points_parameters(l0, l1);
if (ScalarT(0) <= t0 && t0 <= ScalarT(sl0) && //
ScalarT(0) <= t1 && t1 <= ScalarT(sl1))
return distance(l0[t0], l1[t1]); // closest points is inside segments
if (ScalarT(0) <= t0 && t0 <= ScalarT(len0) && //
ScalarT(0) <= t1 && t1 <= ScalarT(len1))
return distance_sqr(l0[t0], l1[t1]); // closest points is inside segments
auto p0 = t0 * ScalarT(2) < len0 ? s0.pos0 : s0.pos1;
auto p1 = t1 * ScalarT(2) < len1 ? s1.pos0 : s1.pos1;
return min(distance_sqr(p0, s1), distance_sqr(p1, s0));
}
auto p0 = t0 < ScalarT(0) ? s0.pos0 : s0.pos1;
auto p1 = t1 < ScalarT(0) ? s1.pos0 : s1.pos1;
template <class ScalarT>
[[nodiscard]] constexpr fractional_result<ScalarT> distance_sqr(segment<3, ScalarT> const& s, line<3, ScalarT> const& l)
{
auto ls = inf_of(s);
auto len = length(s);
return min(distance(p0, s1), distance(p1, s0));
auto [ts, tl] = closest_points_parameters(ls, l);
auto tClamped = clamp(ts, ScalarT(0), len);
return distance_sqr(ls[tClamped], l[tl]);
}
template <class ScalarT>
[[nodiscard]] constexpr fractional_result<ScalarT> distance_sqr(line<3, ScalarT> const& l, segment<3, ScalarT> const& s)
{
return distance_sqr(s, l);
}
// TODO: use GJK or something?
......
......@@ -1109,7 +1109,7 @@ template <class ScalarT>
template <class ScalarT>
[[nodiscard]] constexpr pair<ScalarT, ScalarT> intersection_parameters(line<2, ScalarT> const& l0, line<2, ScalarT> const& l1)
{
auto M = tg::mat<2, 2, ScalarT>::from_cols(l0.dir, -l1.dir);
auto M = mat<2, 2, ScalarT>::from_cols(l0.dir, -l1.dir);
auto t = inverse(M) * (l1.pos - l0.pos);
return {t.x, t.y};
}
......@@ -1535,8 +1535,9 @@ template <class ScalarT>
return true;
// now only intersections between aabb edges and capsule mantle remain
auto const r2 = c.radius * c.radius;
for (auto const& edge : edges_of(b))
if (distance(edge, c.axis) <= c.radius)
if (distance_sqr(edge, c.axis) <= r2)
return true;
return false;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment