distance.hh 3.35 KB
Newer Older
1
#pragma once
2

3
#include <typed-geometry/common/scalar_math.hh>
Philip Trettner's avatar
Philip Trettner committed
4
5
6
7
8
9
10
11
#include <typed-geometry/detail/operators/ops_pos.hh>
#include <typed-geometry/detail/operators/ops_vec.hh>
#include <typed-geometry/detail/special_values.hh>
#include <typed-geometry/detail/tg_traits.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>
12
#include <typed-geometry/types/quadric.hh>
Philip Trettner's avatar
Philip Trettner committed
13
14
#include <typed-geometry/types/vec.hh>

15
#include "closest_points.hh"
16
17
18

namespace tg
{
19
// Base case for distance_sqr of point/point
Philip Trettner's avatar
Philip Trettner committed
20
template <int D, class ScalarA, class ScalarB>
21
TG_NODISCARD constexpr auto distance_sqr(pos<D, ScalarA> const& a, pos<D, ScalarB> const& b) -> decltype(length_sqr(a - b))
22
23
24
25
26
27
{
    return length_sqr(a - b);
}
template <int D, class ScalarA, class ScalarB>
[[deprecated("distance between vectors is ill-defined. did you mean distance_sqr(pos, pos)?")]] TG_NODISCARD constexpr auto distance_sqr(
    vec<D, ScalarA> const& a, vec<D, ScalarB> const& b) -> decltype(length_sqr(a - b))
28
{
29
    return length_sqr(a - b);
30
31
}

32
// Default implementation of distance as sqrt(distance_sqr)
33
template <class A, class B>
34
TG_NODISCARD constexpr auto distance(A const& a, B const& b) -> decltype(sqrt(distance_sqr(a, b)))
35
{
36
    return sqrt(distance_sqr(a, b));
37
38
}

39
// Default implementation of distance_sqr as distance_sqr(ca, cb) for closest points ca and cb
40
template <class A, class B>
41
TG_NODISCARD constexpr auto distance_sqr(A const& a, B const& b) -> decltype(length_sqr(closest_points(a, b).first - closest_points(a, b).second))
42
{
43
    auto cp = closest_points(a, b);
44
    return length_sqr(cp.first - cp.second);
45
46
}

47
// Convenience for distance to (0,0,0)
48
template <class Obj>
49
TG_NODISCARD constexpr auto distance_to_origin(Obj const& o) -> decltype(distance(o, pos_type_for<Obj>::zero))
50
51
52
53
{
    return distance(o, pos_type_for<Obj>::zero);
}
template <class Obj>
54
TG_NODISCARD constexpr auto distance_sqr_to_origin(Obj const& o) -> decltype(distance(o, pos_type_for<Obj>::zero))
55
{
56
    return distance_sqr(o, pos_type_for<Obj>::zero);
57
}
58

59

60
// =========== Object Implementations ===========
61
62

// signed distance is positive if p lies above pl, 0 if it lies on the plane and negative if below pl
63
64
template <int D, class ScalarT>
TG_NODISCARD constexpr fractional_result<ScalarT> signed_distance(pos<3, ScalarT> const& p, hyperplane<D, ScalarT> const& pl)
65
{
66
    return dot(p - pos<D, ScalarT>::zero, pl.normal) - pl.dis;
67
68
}

69
70
template <int D, class ScalarT>
TG_NODISCARD constexpr fractional_result<ScalarT> distance(pos<3, ScalarT> const& p, hyperplane<D, ScalarT> const& pl)
71
72
73
{
    return abs(signed_distance(p, pl));
}
74
75
76
77


// =========== Other Implementations ===========

Philip Trettner's avatar
Philip Trettner committed
78
79
80
81
82
83
84
template <class ScalarT, class = enable_if<is_scalar<ScalarT>>>
TG_NODISCARD constexpr ScalarT distance_sqr(ScalarT a, ScalarT b)
{
    auto const d = a - b;
    return d * d;
}

85
template <class ScalarT>
86
TG_NODISCARD constexpr ScalarT distance_sqr(pos<2, ScalarT> const& p, quadric<2, ScalarT> const& q)
87
{
Philip Trettner's avatar
Philip Trettner committed
88
    return q(p);
89
}
Philip Trettner's avatar
Philip Trettner committed
90

91
template <class ScalarT>
92
TG_NODISCARD constexpr ScalarT distance_sqr(pos<3, ScalarT> const& p, quadric<3, ScalarT> const& q)
93
{
Philip Trettner's avatar
Philip Trettner committed
94
    return q(p);
95
}
Philip Trettner's avatar
Philip Trettner committed
96

97
template <int D, class ScalarT>
98
TG_NODISCARD constexpr ScalarT distance_sqr(quadric<D, ScalarT> const& q, pos<D, ScalarT> const& p)
99
{
Philip Trettner's avatar
Philip Trettner committed
100
    return distance_sqr(p, q);
101
102
}

103
} // namespace tg