Commit ffead239 authored by Philip Trettner's avatar Philip Trettner

more distance functions

parent c74c60f0
......@@ -13,6 +13,7 @@
#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
// closest_points_parameters(a, b) return parameters {t_a, t_b} such that {a[t_a], b[t_a]} are the closest points
namespace tg
{
......@@ -38,7 +39,22 @@ template <int D, class ScalarT, class ObjectT>
// =========== Object Implementations ===========
// TODO
template <class ScalarT>
[[nodiscard]] constexpr pair<ScalarT, ScalarT> closest_points_parameters(line<3, ScalarT> const& l0, line<3, ScalarT> const& l1)
{
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);
return {t0, t1};
}
template <class ScalarT>
[[nodiscard]] constexpr pair<pos<3, ScalarT>, pos<3, ScalarT>> closest_points(line<3, ScalarT> const& l0, line<3, ScalarT> const& l1)
{
auto [t0, t1] = closest_points_parameters(l0, l1);
return {l0[t0], l1[t1]};
}
// =========== Other Implementations ===========
......
......@@ -5,6 +5,8 @@
#include <typed-geometry/detail/special_values.hh>
#include <typed-geometry/detail/tg_traits.hh>
#include <typed-geometry/functions/basic/scalar_math.hh>
#include <typed-geometry/functions/objects/edges.hh>
#include <typed-geometry/functions/objects/vertices.hh>
#include <typed-geometry/functions/vector/distance.hh>
#include <typed-geometry/types/objects/line.hh>
#include <typed-geometry/types/objects/plane.hh>
......@@ -14,6 +16,7 @@
#include <typed-geometry/types/vec.hh>
#include "closest_points.hh"
#include "intersection.hh"
namespace tg
{
......@@ -50,23 +53,102 @@ template <class Obj>
// signed distance is positive if p lies above pl, 0 if it lies on the plane and negative if below pl
template <int D, class ScalarT>
[[nodiscard]] constexpr fractional_result<ScalarT> signed_distance(pos<3, ScalarT> const& p, plane<D, ScalarT> const& pl)
[[nodiscard]] constexpr fractional_result<ScalarT> signed_distance(pos<D, ScalarT> const& p, plane<D, ScalarT> const& pl)
{
return dot(p - pos<D, ScalarT>::zero, pl.normal) - pl.dis;
}
template <int D, class ScalarT>
[[nodiscard]] constexpr fractional_result<ScalarT> distance(pos<3, ScalarT> const& p, plane<D, ScalarT> const& pl)
[[nodiscard]] constexpr fractional_result<ScalarT> distance(pos<D, ScalarT> const& p, plane<D, ScalarT> const& pl)
{
return abs(signed_distance(p, pl));
}
template <int D, class ScalarT>
[[nodiscard]] constexpr auto signed_distance(pos<D, ScalarT> const& p, sphere_boundary<D, ScalarT> const& s) -> decltype(distance(p, s.center) - s.radius)
[[nodiscard]] constexpr fractional_result<ScalarT> signed_distance(pos<D, ScalarT> const& p, sphere_boundary<D, ScalarT> const& s)
{
return distance(p, s.center) - s.radius;
}
template <class ScalarT>
[[nodiscard]] constexpr fractional_result<ScalarT> distance(line<3, ScalarT> const& l0, line<3, ScalarT> const& l1)
{
auto n = cross(l0.dir, l1.dir);
return abs(dot(l0.pos - l1.pos, n) / length(n));
}
template <class ScalarT>
[[nodiscard]] constexpr fractional_result<ScalarT> distance(segment<2, ScalarT> const& s0, segment<2, ScalarT> const& s1)
{
auto l0 = tg::line<2, ScalarT>::from_points(s0.pos0, s0.pos1);
auto l1 = tg::line<2, ScalarT>::from_points(s1.pos0, s1.pos1);
auto sl0 = distance(s0.pos0, s0.pos1);
auto sl1 = distance(s1.pos0, s1.pos1);
auto [t0, t1] = intersection_parameters(l0, l1);
if (ScalarT(0) <= t0 && t0 <= ScalarT(sl0) && //
ScalarT(0) <= t1 && t1 <= ScalarT(sl1))
return ScalarT(0); // intersects
auto p0 = t0 < ScalarT(0) ? s0.pos0 : s0.pos1;
auto p1 = t1 < ScalarT(0) ? s1.pos0 : s1.pos1;
return min(distance(p0, s1), distance(p1, s0));
}
template <class ScalarT>
[[nodiscard]] constexpr fractional_result<ScalarT> distance(segment<3, ScalarT> const& s0, segment<3, ScalarT> const& s1)
{
auto l0 = tg::line<3, ScalarT>::from_points(s0.pos0, s0.pos1);
auto l1 = tg::line<3, ScalarT>::from_points(s1.pos0, s1.pos1);
auto sl0 = distance(s0.pos0, s0.pos1);
auto sl1 = distance(s1.pos0, s1.pos1);
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
auto p0 = t0 < ScalarT(0) ? s0.pos0 : s0.pos1;
auto p1 = t1 < ScalarT(0) ? s1.pos0 : s1.pos1;
return min(distance(p0, s1), distance(p1, s0));
}
// TODO: use GJK or something?
template <class ScalarT>
[[nodiscard]] constexpr fractional_result<ScalarT> distance(aabb<3, ScalarT> const& bb, triangle<3, ScalarT> const& t)
{
if (intersects(bb, t))
return fractional_result<ScalarT>(0);
auto d = tg::max<ScalarT>();
// tri vertices to bb
for (auto p : vertices(t))
d = min(d, distance(bb, p));
// bb vertices to tri
for (auto p : vertices(bb))
d = min(d, distance(t, p));
// edges to edges
for (auto e0 : edges(t))
for (auto e1 : edges(bb))
d = min(d, distance(e0, e1));
return d;
}
template <class ScalarT>
[[nodiscard]] constexpr fractional_result<ScalarT> distance(triangle<3, ScalarT> const& t, aabb<3, ScalarT> const& bb)
{
return distance(bb, t);
}
// =========== Other Implementations ===========
......
#pragma once
#include <typed-geometry/types/array.hh>
#include <typed-geometry/types/objects/aabb.hh>
#include <typed-geometry/types/objects/segment.hh>
#include <typed-geometry/types/objects/triangle.hh>
namespace tg
{
template <int D, class T>
[[nodiscard]] array<segment<D, T>, 3> edges(triangle<D, T> const& t)
template <int D, class ScalarT>
[[nodiscard]] array<segment<D, ScalarT>, 3> edges(triangle<D, ScalarT> const& t)
{
return {{{t.pos0, t.pos1}, {t.pos1, t.pos2}, {t.pos2, t.pos0}}};
}
template <class ScalarT>
[[nodiscard]] array<segment<2, ScalarT>, 4> edges(aabb<2, ScalarT> const& bb)
{
auto p00 = pos<3, ScalarT>(bb.min.x, bb.min.y);
auto p01 = pos<3, ScalarT>(bb.min.x, bb.max.y);
auto p10 = pos<3, ScalarT>(bb.max.x, bb.min.y);
auto p11 = pos<3, ScalarT>(bb.max.x, bb.max.y);
return {{{p00, p01}, {p01, p10}, {p10, p11}, {p11, p00}}};
}
template <class ScalarT>
[[nodiscard]] array<segment<3, ScalarT>, 12> edges(aabb<3, ScalarT> const& bb)
{
auto p000 = pos<3, ScalarT>(bb.min.x, bb.min.y, bb.min.z);
auto p001 = pos<3, ScalarT>(bb.min.x, bb.min.y, bb.max.z);
auto p010 = pos<3, ScalarT>(bb.min.x, bb.max.y, bb.min.z);
auto p011 = pos<3, ScalarT>(bb.min.x, bb.max.y, bb.max.z);
auto p100 = pos<3, ScalarT>(bb.max.x, bb.min.y, bb.min.z);
auto p101 = pos<3, ScalarT>(bb.max.x, bb.min.y, bb.max.z);
auto p110 = pos<3, ScalarT>(bb.max.x, bb.max.y, bb.min.z);
auto p111 = pos<3, ScalarT>(bb.max.x, bb.max.y, bb.max.z);
return {{
{p000, p100},
{p001, p101},
{p010, p110},
{p011, p111}, // x dir
{p000, p010},
{p001, p011},
{p100, p110},
{p101, p111}, // y dir
{p000, p001},
{p010, p011},
{p100, p101},
{p110, p111}, // z dir
}};
}
}
......@@ -91,6 +91,7 @@ private:
// ====================================== Default Implementations ======================================
// TODO: intersection_parameter from intersection_parameters
// returns whether two objects intersect
template <class A, class B>
......@@ -517,35 +518,50 @@ template <class ScalarT>
}
template <class ScalarT>
[[nodiscard]] constexpr optional<tg::pos<2, ScalarT>> intersection(segment<2, ScalarT> const& seg_0, segment<2, ScalarT> const& seg_1)
[[nodiscard]] constexpr optional<pair<ScalarT, ScalarT>> intersection_parameters(segment<2, ScalarT> const& seg_0, segment<2, ScalarT> const& seg_1)
{
/// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
auto const denominator
= (seg_0.pos0.x - seg_0.pos1.x) * (seg_1.pos0.y - seg_1.pos1.y) - (seg_0.pos0.y - seg_0.pos1.y) * (seg_1.pos0.x - seg_1.pos1.x);
auto const denom = (seg_0.pos0.x - seg_0.pos1.x) * (seg_1.pos0.y - seg_1.pos1.y) - (seg_0.pos0.y - seg_0.pos1.y) * (seg_1.pos0.x - seg_1.pos1.x);
// todo: might want to check == 0 with an epsilon corridor
// todo: colinear line segments can still intersect in a point or a line segment.
// This might require api changes, as either a point or a line segment can be returned!
// Possible solution: return a segment where pos0 == pos1
if (denominator == ScalarT(0))
if (denom == ScalarT(0))
return {}; // colinear
auto const numerator = (seg_0.pos0.x - seg_1.pos0.x) * (seg_1.pos0.y - seg_1.pos1.y) - (seg_0.pos0.y - seg_1.pos0.y) * (seg_1.pos0.x - seg_1.pos1.x);
auto const t = numerator / denominator;
if (ScalarT(0) <= t && t <= ScalarT(1))
{
// intersection
return seg_0.pos0 + t * (seg_0.pos1 - seg_0.pos0);
}
auto const num0 = (seg_0.pos0.x - seg_1.pos0.x) * (seg_1.pos0.y - seg_1.pos1.y) - (seg_0.pos0.y - seg_1.pos0.y) * (seg_1.pos0.x - seg_1.pos1.x);
auto const num1 = (seg_0.pos0.x - seg_0.pos1.x) * (seg_0.pos0.y - seg_1.pos0.y) - (seg_0.pos0.y - seg_0.pos1.y) * (seg_0.pos0.x - seg_1.pos0.x);
auto const t = num0 / denom;
auto const u = -num1 / denom;
if (ScalarT(0) <= t && t <= ScalarT(1) && ScalarT(0) <= u && u <= ScalarT(1))
return pair<ScalarT, ScalarT>{t, u};
return {};
}
template <class ScalarT>
[[nodiscard]] constexpr pos<2, ScalarT> intersection(line<2, ScalarT> const& l0, line<2, ScalarT> const& l1)
[[nodiscard]] constexpr optional<ScalarT> intersection_parameter(segment<2, ScalarT> const& seg_0, segment<2, ScalarT> const& seg_1)
{
auto ip = intersection_parameters(seg_0, seg_1);
if (ip.has_value())
return ip.value().first;
return {};
}
template <class ScalarT>
[[nodiscard]] constexpr ScalarT intersection_parameter(line<2, ScalarT> const& l0, line<2, ScalarT> const& l1)
{
auto M = tg::mat<2, 2, ScalarT>::from_cols(l0.dir, -l1.dir);
auto t = inverse(M) * (l1.pos - l0.pos);
return l0[t.x];
return t.x;
}
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 t = inverse(M) * (l1.pos - l0.pos);
return {t.x, t.y};
}
template <int D, class ScalarT>
......@@ -817,4 +833,114 @@ template <class ScalarT>
return intersects(b, a);
}
// NOTE: does NOT work for integer objects
template <class ScalarT>
[[nodiscard]] constexpr bool intersects(triangle<3, ScalarT> const& tri_in, aabb<3, ScalarT> const& bb_in)
{
using pos_t = pos<3, ScalarT>;
using vec_t = vec<3, ScalarT>;
auto const center = (bb_in.max + bb_in.min) / ScalarT(2);
auto const amin = pos_t(bb_in.min - center);
auto const amax = pos_t(bb_in.max - center);
auto const bb = aabb<3, ScalarT>(amin, amax);
auto const p0 = pos_t(tri_in.pos0 - center);
auto const p1 = pos_t(tri_in.pos1 - center);
auto const p2 = pos_t(tri_in.pos2 - center);
// early out: AABB vs tri AABB
auto tri_aabb = aabb_of(p0, p1, p2);
if (tri_aabb.max.x < amin.x || tri_aabb.max.y < amin.y || tri_aabb.max.z < amin.z || //
tri_aabb.min.x > amax.x || tri_aabb.min.y > amax.y || tri_aabb.min.z > amax.z)
return false;
auto const proper_contains = [](aabb<3, ScalarT> const& b, pos_t const& p) {
return b.min.x < p.x && p.x < b.max.x && //
b.min.y < p.y && p.y < b.max.y && //
b.min.z < p.z && p.z < b.max.z;
};
auto const contains_p0 = proper_contains(bb, p0);
auto const contains_p1 = proper_contains(bb, p1);
auto const contains_p2 = proper_contains(bb, p2);
// early in: tri points vs AABB
if (contains_p0 || contains_p1 || contains_p2)
return true;
// get adjusted tri base plane
auto plane = tg::plane<3, ScalarT>(normal(tri_in), p0);
// fast plane / AABB test
{
auto pn = plane.normal;
auto bn = abs(pn.x * amax.x) + abs(pn.y * amax.y) + abs(pn.z * amax.z);
// min dis: d - bn
if (bn < -plane.dis)
return false;
// max dis: d + bn
if (-plane.dis < -bn)
return false;
}
// 9 axis SAT test
{
auto const is_seperating = [amax](vec<3, ScalarT> const& n, pos_t const& tp0, pos_t const& tp1) -> bool {
if (tg::is_zero_vector(n))
return false; // not a real candidate axis
// fast point / AABB separation test
auto bn = abs(n.x * amax.x) + abs(n.y * amax.y) + abs(n.z * amax.z);
auto tn0 = dot(n, tp0);
auto tn1 = dot(n, tp1);
auto tmin = min(tn0, tn1);
auto tmax = max(tn0, tn1);
auto bmin = -bn;
auto bmax = bn;
if (tmax < bmin)
return true;
if (bmax < tmin)
return true;
return false;
};
if (is_seperating(cross(p1 - p0, vec_t::unit_x), p0, p2))
return false;
if (is_seperating(cross(p1 - p0, vec_t::unit_y), p0, p2))
return false;
if (is_seperating(cross(p1 - p0, vec_t::unit_z), p0, p2))
return false;
if (is_seperating(cross(p2 - p0, vec_t::unit_x), p0, p1))
return false;
if (is_seperating(cross(p2 - p0, vec_t::unit_y), p0, p1))
return false;
if (is_seperating(cross(p2 - p0, vec_t::unit_z), p0, p1))
return false;
if (is_seperating(cross(p1 - p2, vec_t::unit_x), p0, p2))
return false;
if (is_seperating(cross(p1 - p2, vec_t::unit_y), p0, p2))
return false;
if (is_seperating(cross(p1 - p2, vec_t::unit_z), p0, p2))
return false;
}
// found no separating axis? -> intersection
return true;
}
template <class ScalarT>
[[nodiscard]] constexpr bool intersects(aabb<3, ScalarT> const& a, triangle<3, ScalarT> const& b)
{
return intersects(b, a);
}
} // namespace tg
#pragma once
#include <typed-geometry/types/array.hh>
#include <typed-geometry/types/objects/aabb.hh>
#include <typed-geometry/types/objects/segment.hh>
#include <typed-geometry/types/objects/triangle.hh>
namespace tg
{
template <int D, class T>
[[nodiscard]] array<pos<D, T>, 3> vertices(triangle<D, T> const& t)
template <int D, class ScalarT>
[[nodiscard]] array<pos<D, ScalarT>, 3> vertices(triangle<D, ScalarT> const& t)
{
return {{t.pos0, t.pos1, t.pos2}};
}
template <int D, class ScalarT>
[[nodiscard]] array<pos<D, ScalarT>, 2> vertices(segment<D, ScalarT> const& s)
{
return {{s.pos0, s.pos1}};
}
template <class ScalarT>
[[nodiscard]] array<pos<2, ScalarT>, 4> vertices(aabb<2, ScalarT> const& bb)
{
auto p00 = pos<3, ScalarT>(bb.min.x, bb.min.y);
auto p01 = pos<3, ScalarT>(bb.min.x, bb.max.y);
auto p10 = pos<3, ScalarT>(bb.max.x, bb.min.y);
auto p11 = pos<3, ScalarT>(bb.max.x, bb.max.y);
return {{p00, p01, p10, p11}};
}
template <class ScalarT>
[[nodiscard]] array<pos<3, ScalarT>, 8> vertices(aabb<3, ScalarT> const& bb)
{
auto p000 = pos<3, ScalarT>(bb.min.x, bb.min.y, bb.min.z);
auto p001 = pos<3, ScalarT>(bb.min.x, bb.min.y, bb.max.z);
auto p010 = pos<3, ScalarT>(bb.min.x, bb.max.y, bb.min.z);
auto p011 = pos<3, ScalarT>(bb.min.x, bb.max.y, bb.max.z);
auto p100 = pos<3, ScalarT>(bb.max.x, bb.min.y, bb.min.z);
auto p101 = pos<3, ScalarT>(bb.max.x, bb.min.y, bb.max.z);
auto p110 = pos<3, ScalarT>(bb.max.x, bb.max.y, bb.min.z);
auto p111 = pos<3, ScalarT>(bb.max.x, bb.max.y, bb.max.z);
return {{p000, p001, p010, p011, p100, p101, p110, p111}};
}
}
......@@ -60,6 +60,9 @@ struct line
{
}
/// constructs a line such that
/// pos = a
/// dir = normalize(b - a)
static constexpr line from_points(pos_t a, pos_t b);
[[nodiscard]] constexpr pos_t operator[](ScalarT t) const;
......
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