264 lines
6.7 KiB
C++
264 lines
6.7 KiB
C++
#pragma once
|
|
|
|
#include <psemek/util/empty.hpp>
|
|
|
|
#include <psemek/geom/simplex.hpp>
|
|
#include <psemek/geom/ray.hpp>
|
|
#include <psemek/geom/box.hpp>
|
|
#include <psemek/geom/sphere.hpp>
|
|
#include <psemek/geom/cylinder.hpp>
|
|
#include <psemek/geom/orientation.hpp>
|
|
#include <psemek/geom/contains.hpp>
|
|
#include <psemek/geom/gauss.hpp>
|
|
|
|
#include <optional>
|
|
#include <variant>
|
|
#include <type_traits>
|
|
|
|
namespace psemek::geom
|
|
{
|
|
|
|
template <typename T>
|
|
bool intersect(segment<point<T, 2>> const & s0, segment<point<T, 2>> const & s1)
|
|
{
|
|
auto const o00 = orientation(s0[0], s0[1], s1[0]);
|
|
auto const o01 = orientation(s0[0], s0[1], s1[1]);
|
|
auto const o10 = orientation(s1[0], s1[1], s0[0]);
|
|
auto const o11 = orientation(s1[0], s1[1], s0[1]);
|
|
|
|
return ((o00 != o01) || (o00 == sign_t::zero)) && ((o10 != o11) || (o10 == sign_t::zero));
|
|
}
|
|
|
|
// TODO: robust implementation
|
|
template <typename T>
|
|
std::variant<util::empty, point<T, 2>, segment<point<T, 2>>> intersection(segment<point<T, 2>> const & s0, segment<point<T, 2>> const & s1)
|
|
{
|
|
auto const a0 = -det(s1[0] - s0[0], s1[1] - s1[0]);
|
|
auto const a1 = det(s0[1] - s0[0], s1[0] - s0[0]);
|
|
auto const b = -det(s0[1] - s0[0], s1[1] - s1[0]);
|
|
|
|
if (b != 0)
|
|
{
|
|
// general case
|
|
|
|
auto const t0 = a0 / b;
|
|
auto const t1 = a1 / b;
|
|
|
|
if (t0 < 0 || t0 > 1 || t1 < 0 || t1 > 1)
|
|
return util::empty{};
|
|
|
|
return s0[0] + t0 * (s0[1] - s0[0]);
|
|
}
|
|
else
|
|
{
|
|
// collinear segments
|
|
|
|
if (a0 != 0)
|
|
{
|
|
// segments do not lie on the same line: no intersection
|
|
return util::empty{};
|
|
}
|
|
|
|
// if segments are not Y-axis aligned, safe to use X-coordinates to sort them (k = 0)
|
|
// otherwise use Y-coordinates (k = 1)
|
|
std::size_t const k = (s0[0][0] != s0[1][0]) ? 0 : 1;
|
|
|
|
if (s0[0][k] > s0[1][k])
|
|
std::swap(s0[0], s0[1]);
|
|
|
|
if (s1[0][k] > s1[1][k])
|
|
std::swap(s1[0], s1[1]);
|
|
|
|
auto const r0 = std::max(s0[0][k], s1[0][k]);
|
|
auto const r1 = std::min(s0[1][k], s1[1][k]);
|
|
|
|
if (r0 > r1)
|
|
return util::empty{};
|
|
|
|
bool const s0_is_first = s0[0][k] < s1[0][k];
|
|
|
|
if (r0 == r1)
|
|
{
|
|
point<T, 2> p;
|
|
p[k] = r0;
|
|
p[1 - k] = s0_is_first ? s0[1][1 - k] : s1[1][1 - k];
|
|
return p;
|
|
}
|
|
else if (s0_is_first)
|
|
return simplex{ s1[0], s0[1] };
|
|
else
|
|
return simplex{ s0[0], s1[1] };
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
bool intersect(triangle<point<T, 2>> const & t0, triangle<point<T, 2>> const & t1)
|
|
{
|
|
if (contains(t0, t1[0]) || contains(t0, t1[1]) || contains(t0, t1[2])) return true;
|
|
|
|
if (contains(t1, t0[0]) || contains(t1, t0[1]) || contains(t1, t0[2])) return true;
|
|
|
|
if (intersect(simplex{t0[0], t0[1]}, simplex{t1[0], t1[1]})) return true;
|
|
if (intersect(simplex{t0[0], t0[1]}, simplex{t1[1], t1[2]})) return true;
|
|
if (intersect(simplex{t0[1], t0[2]}, simplex{t1[0], t1[1]})) return true;
|
|
if (intersect(simplex{t0[1], t0[2]}, simplex{t1[1], t1[2]})) return true;
|
|
if (intersect(simplex{t0[2], t0[0]}, simplex{t1[0], t1[1]})) return true;
|
|
if (intersect(simplex{t0[2], t0[0]}, simplex{t1[1], t1[2]})) return true;
|
|
|
|
return false;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
interval<T> intersection(ray<T, N> const & r, box<T, N> const & b)
|
|
{
|
|
auto t = interval<T>::full();
|
|
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
{
|
|
T tmin = (b[i].min - r.origin[i]) / r.direction[i];
|
|
T tmax = (b[i].max - r.origin[i]) / r.direction[i];
|
|
|
|
if (tmin > tmax) std::swap(tmin, tmax);
|
|
|
|
t &= interval<T>{tmin, tmax};
|
|
}
|
|
|
|
return t;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
bool intersect(ray<T, N> const & r, box<T, N> const & b)
|
|
{
|
|
return !intersection(r, b).empty();
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
interval<T> intersection(segment<point<T, N>> const & s, box<T, N> const & b)
|
|
{
|
|
auto t = interval<T>{T(0), T(1)};
|
|
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
{
|
|
T tmin = (b[i].min - s[0][i]) / (s[1][i] - s[0][i]);
|
|
T tmax = (b[i].max - s[0][i]) / (s[1][i] - s[0][i]);
|
|
|
|
if (tmin > tmax) std::swap(tmin, tmax);
|
|
|
|
t &= interval<T>{tmin, tmax};
|
|
}
|
|
|
|
return t;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
bool intersect(segment<point<T, N>> const & s, box<T, N> const & b)
|
|
{
|
|
return !intersection(s, b).empty();
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
std::optional<T> intersection(ray<T, N> const & r, simplex<point<T, N>, N - 1> const & s)
|
|
{
|
|
geom::matrix<T, N, N> m;
|
|
geom::vector<T, N> b;
|
|
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
{
|
|
m[i][0] = r.direction[i];
|
|
b[i] = s[0][i] - r.origin[i];
|
|
|
|
for (std::size_t j = 1; j < N; ++j)
|
|
{
|
|
m[i][j] = s[j][i] - s[0][i];
|
|
}
|
|
}
|
|
|
|
if (!geom::gauss(m, b))
|
|
return std::nullopt;
|
|
|
|
T sum{};
|
|
for (std::size_t j = 1; j < N; ++j)
|
|
{
|
|
if (b[j] >= T{}) return std::nullopt;
|
|
sum += (-b[j]);
|
|
}
|
|
|
|
if (sum > T{1}) return std::nullopt;
|
|
|
|
if (b[0] < T{}) return std::nullopt;
|
|
|
|
return b[0];
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
bool intersect(ray<T, N> const & r, simplex<point<T, N>, N - 1> const & s)
|
|
{
|
|
return static_cast<bool>(intersection(r, s));
|
|
}
|
|
|
|
template <typename T, std::size_t N, typename = std::enable_if_t<(N > 2)>>
|
|
std::optional<T> intersection(segment<point<T, N>> const & seg, simplex<point<T, N>, N - 1> const & s)
|
|
{
|
|
auto i = intersection(ray{seg[0], seg[1] - seg[0]}, s);
|
|
if (!i || *i < 0 || *i > 1)
|
|
return std::nullopt;
|
|
return i;
|
|
}
|
|
|
|
template <typename T, std::size_t N, typename = std::enable_if_t<(N > 2)>>
|
|
bool intersect(segment<point<T, N>> const & seg, simplex<T, N> const & s)
|
|
{
|
|
return static_cast<bool>(intersection(seg, s));
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
interval<T> intersection(ray<T, N> const & r, sphere<T, N> const & s)
|
|
{
|
|
auto d = r.origin - s.center;
|
|
|
|
auto ir = solve_quadratic(length_sqr(r.direction), T{2} * dot(r.direction, d), length_sqr(d) - sqr(s.radius));
|
|
if (!ir)
|
|
return {};
|
|
|
|
return {ir->first, ir->second};
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
interval<T> intersection(ray<T, N> const & r, cylinder<T, N> const & c)
|
|
{
|
|
auto Z2 = length_sqr(c.axis);
|
|
auto CZ = dot(c.center - r.origin, c.axis);
|
|
auto DZ = dot(r.direction, c.axis);
|
|
if (DZ == 0)
|
|
return {};
|
|
|
|
auto prmin = (-Z2 + CZ) / DZ;
|
|
auto prmax = ( Z2 + CZ) / DZ;
|
|
if (DZ < 0)
|
|
std::swap(prmin, prmax);
|
|
|
|
auto n = normalized(c.axis);
|
|
auto q = r.origin - c.center;
|
|
auto ir = solve_quadratic(length_sqr(r.direction) - sqr(dot(r.direction, n)),
|
|
2 * dot(q, r.direction) - 2 * dot(q, n) * dot(r.direction, n),
|
|
length_sqr(q) - sqr(dot(q, n)) - sqr(c.radius));
|
|
|
|
if (!ir)
|
|
return {};
|
|
|
|
return {std::max(ir->first, prmin), std::min(ir->second, prmax)};
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
bool intersect(ray<T, N> const & r, sphere<T, N> const & s)
|
|
{
|
|
return intersection(r, s).empty();
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
bool intersect(ray<T, N> const & r, cylinder<T, N> const & c)
|
|
{
|
|
return intersection(r, c).empty();
|
|
}
|
|
|
|
}
|