quadric.hh 7.97 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
#pragma once

#include "dir.hh"
#include "mat.hh"
#include "pos.hh"
#include "vec.hh"

namespace tg
{
/**
 * An error quadric in 2D or 3D
 *
 * Notable functions:
 *   - distance(pos, quadric)   for residual L2 error
15
 *   - distance_sqr(pos, quadric)  for residual squared L2 error
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
 *   - closest_point(quadric)   for the position minimizing the distance
 *   - operator + * /
 */
template <int D, class ScalarT>
struct quadric;

// Common quadric types
using quadric2 = quadric<2, f32>;
using fquadric2 = quadric<2, f32>;
using dquadric2 = quadric<2, f64>;

using quadric3 = quadric<3, f32>;
using fquadric3 = quadric<3, f32>;
using dquadric3 = quadric<3, f64>;


// ======== IMPLEMENTATION ========

template <class ScalarT>
struct quadric<2, ScalarT>
{
    using scalar_t = ScalarT;
Philip Trettner's avatar
Philip Trettner committed
38
39
    using vec_t = tg::vec<2, ScalarT>;
    using pos_t = tg::pos<2, ScalarT>;
40
    using mat_t = tg::mat<2, 2, ScalarT>;
41

Philip Trettner's avatar
Philip Trettner committed
42
43
44
45
46
    // x^T A x - 2 b^T x + c
public:
    scalar_t A00 = scalar_t(0);
    scalar_t A01 = scalar_t(0);
    scalar_t A11 = scalar_t(0);
47

Philip Trettner's avatar
Philip Trettner committed
48
49
    scalar_t b0 = scalar_t(0);
    scalar_t b1 = scalar_t(0);
50

Philip Trettner's avatar
Philip Trettner committed
51
    scalar_t c = scalar_t(0);
52
53
54
55

public:
    constexpr quadric() = default;

56
57
58
59
60
61
62
63
64
65
66
67
    static constexpr quadric from_coefficients(mat_t const& A, vec_t const& b, scalar_t c)
    {
        quadric q;
        q.A00 = A[0][0];
        q.A01 = A[0][1];
        q.A11 = A[1][1];
        q.b0 = b[0];
        q.b1 = b[1];
        q.c = c;
        return q;
    }

68
69
70
71
72
73
74
75
76
77
    // TODO: constexpr explicit quadric(mat<3, 3, ScalarT> const& m);
    // TODO: constexpr explicit operator mat<3, 3, ScalarT>() const;

    // TODO: operator== and !=

    // TODO: find better place for these?
public:
    /// Adds a line (pos, normal) with a given uncertainty sigma
    /// `normal` should be normalized
    /// `sigma` is the standard deviation of the normal distribution added to `normal`
Philip Trettner's avatar
Philip Trettner committed
78
    void add_line(pos_t pos, vec_t normal, scalar_t sigma)
79
    {
Philip Trettner's avatar
Philip Trettner committed
80
        auto d = dot(pos - pos_t::zero, normal);
81
82
83
84
85
86
        auto s2 = sigma * sigma;

        A00 += normal.x * normal.x + s2;
        A01 += normal.x * normal.y;
        A11 += normal.y * normal.y + s2;

Philip Trettner's avatar
Philip Trettner committed
87
88
        b0 += normal.x * d + pos.x * s2;
        b1 += normal.y * d + pos.y * s2;
89
90

        // TODO: maybe positional uncertainty
Philip Trettner's avatar
Philip Trettner committed
91
        c += d * d + distance_sqr_to_origin(pos) * s2;
92
93
    }

94
95
96
97
98
99
100
    // add two quadrics
    void add(quadric const& rhs)
    {
        A00 += rhs.A00;
        A01 += rhs.A01;
        A11 += rhs.A11;

Philip Trettner's avatar
Philip Trettner committed
101
102
        b0 += rhs.b0;
        b1 += rhs.b1;
103

Philip Trettner's avatar
Philip Trettner committed
104
        c += rhs.c;
105
106
    }

Philip Trettner's avatar
Philip Trettner committed
107
    // Residual L2 error as given by x^T A x - 2 r^T x + c
108
    [[nodiscard]] constexpr ScalarT operator()(pos<2, ScalarT> const& p) const
Philip Trettner's avatar
Philip Trettner committed
109
110
111
112
113
114
    {
        vec<2, ScalarT> Ax = {
            A00 * p.x + A01 * p.y, //
            A01 * p.x + A11 * p.y, //
        };

Philip Trettner's avatar
Philip Trettner committed
115
116
117
        return dot(vec<2, ScalarT>(p), Ax) // x^T A x
               - 2 * (b0 * p.x + b1 * p.y) // - 2 r^T x
               + c;                        // + c
Philip Trettner's avatar
Philip Trettner committed
118
119
    }

120
    // TODO: fix me
121
    template <class T>
122
    friend constexpr T distance_sqr(pos<2, T> const& p, quadric<2, T> const& q);
123
    template <class T>
124
125
126
127
128
129
130
    friend constexpr pos<2, T> closest_point(quadric<2, T> const& q);
};

template <class ScalarT>
struct quadric<3, ScalarT>
{
    using scalar_t = ScalarT;
Philip Trettner's avatar
Philip Trettner committed
131
132
    using vec_t = tg::vec<3, ScalarT>;
    using pos_t = tg::pos<3, ScalarT>;
Philip Trettner's avatar
Philip Trettner committed
133
    using mat_t = tg::mat<3, 3, ScalarT>;
134

Philip Trettner's avatar
Philip Trettner committed
135
136
137
138
139
140
141
142
    // x^T A x - 2 b^T x + c
public:
    scalar_t A00 = scalar_t(0);
    scalar_t A01 = scalar_t(0);
    scalar_t A02 = scalar_t(0);
    scalar_t A11 = scalar_t(0);
    scalar_t A12 = scalar_t(0);
    scalar_t A22 = scalar_t(0);
143

Philip Trettner's avatar
Philip Trettner committed
144
145
146
    scalar_t b0 = scalar_t(0);
    scalar_t b1 = scalar_t(0);
    scalar_t b2 = scalar_t(0);
147

Philip Trettner's avatar
Philip Trettner committed
148
    scalar_t c = scalar_t(0);
149
150
151
152
153
154
155
156

public:
    constexpr quadric() = default;
    // TODO: constexpr explicit quadric(mat<4, 4, ScalarT> const& m);
    // TODO: constexpr explicit operator mat<4, 4, ScalarT>() const;

    // TODO: operator== and !=

Philip Trettner's avatar
Philip Trettner committed
157
158
159
160
161
162
163
164
165
    static constexpr quadric from_coefficients(scalar_t A00, scalar_t A01, scalar_t A02, scalar_t A11, scalar_t A12, scalar_t A22, scalar_t b0, scalar_t b1, scalar_t b2, scalar_t c)
    {
        quadric q;
        q.A00 = A00;
        q.A01 = A01;
        q.A02 = A02;
        q.A11 = A11;
        q.A12 = A12;
        q.A22 = A22;
Philip Trettner's avatar
Philip Trettner committed
166
167
168
169
        q.b0 = b0;
        q.b1 = b1;
        q.b2 = b2;
        q.c = c;
Philip Trettner's avatar
Philip Trettner committed
170
171
        return q;
    }
Philip Trettner's avatar
Philip Trettner committed
172
    static constexpr quadric from_coefficients(mat_t const& A, vec_t const& b, scalar_t c)
Philip Trettner's avatar
Philip Trettner committed
173
174
175
176
177
178
179
180
    {
        quadric q;
        q.A00 = A[0][0];
        q.A01 = A[0][1];
        q.A02 = A[0][2];
        q.A11 = A[1][1];
        q.A12 = A[1][2];
        q.A22 = A[2][2];
Philip Trettner's avatar
Philip Trettner committed
181
182
183
184
        q.b0 = b[0];
        q.b1 = b[1];
        q.b2 = b[2];
        q.c = c;
Philip Trettner's avatar
Philip Trettner committed
185
186
187
        return q;
    }

188
public:
Philip Trettner's avatar
Philip Trettner committed
189
    constexpr mat<3, 3, ScalarT> const A() const
190
191
192
193
194
195
196
197
198
199
200
201
202
    {
        mat<3, 3, ScalarT> m;
        m[0][0] = A00;
        m[0][1] = A01;
        m[0][2] = A02;
        m[1][0] = A01;
        m[1][1] = A11;
        m[1][2] = A12;
        m[2][0] = A02;
        m[2][1] = A12;
        m[2][2] = A22;
        return m;
    }
Philip Trettner's avatar
Philip Trettner committed
203
    constexpr vec<3, ScalarT> const b() const { return {b0, b1, b2}; }
204

205
206
207
208
209
    // TODO: find better place for these?
public:
    /// Adds a plane (pos, normal) with a given uncertainty sigma
    /// `normal` should be normalized
    /// `sigma` is the standard deviation of the normal distribution added to `normal`
Philip Trettner's avatar
Philip Trettner committed
210
    void add_plane(pos_t pos, vec_t normal, scalar_t sigma)
211
    {
Philip Trettner's avatar
Philip Trettner committed
212
        auto d = dot(pos - pos_t::zero, normal);
213
214
215
216
217
218
219
220
221
        auto s2 = sigma * sigma;

        A00 += normal.x * normal.x + s2;
        A01 += normal.x * normal.y;
        A02 += normal.x * normal.z;
        A11 += normal.y * normal.y + s2;
        A12 += normal.y * normal.z;
        A22 += normal.z * normal.z + s2;

Philip Trettner's avatar
Philip Trettner committed
222
223
224
        b0 += normal.x * d + pos.x * s2;
        b1 += normal.y * d + pos.y * s2;
        b2 += normal.z * d + pos.z * s2;
225
226

        // TODO: maybe positional uncertainty
Philip Trettner's avatar
Philip Trettner committed
227
        c += d * d + distance_sqr_to_origin(pos) * s2;
228
229
230
231
232
233
234
235
236
237
238
239
    }

    /// Adds two quadrics
    void add(quadric const& rhs)
    {
        A00 += rhs.A00;
        A01 += rhs.A01;
        A02 += rhs.A02;
        A11 += rhs.A11;
        A12 += rhs.A12;
        A22 += rhs.A22;

Philip Trettner's avatar
Philip Trettner committed
240
241
242
        b0 += rhs.b0;
        b1 += rhs.b1;
        b2 += rhs.b2;
243

Philip Trettner's avatar
Philip Trettner committed
244
        c += rhs.c;
245
246
    }

Philip Trettner's avatar
Philip Trettner committed
247
248
249
250
251
252
253
254
255
256
257
    // adds a quadric given in matrix form (x^T A x + b^T x + c)
    // optionally with a c value
    void add(mat<3, 3, scalar_t> const& A, vec<3, scalar_t> const& b, scalar_t c = {})
    {
        A00 += A[0][0];
        A01 += A[0][1];
        A02 += A[0][2];
        A11 += A[1][1];
        A12 += A[1][2];
        A22 += A[2][2];

Philip Trettner's avatar
Philip Trettner committed
258
259
260
        b0 += b.x;
        b1 += b.y;
        b2 += b.z;
Philip Trettner's avatar
Philip Trettner committed
261

Philip Trettner's avatar
Philip Trettner committed
262
        c += c;
Philip Trettner's avatar
Philip Trettner committed
263
264
    }

Philip Trettner's avatar
Philip Trettner committed
265
    // Residual L2 error as given by x^T A x - 2 r^T x + c
266
    [[nodiscard]] constexpr ScalarT operator()(pos<3, ScalarT> const& p) const
Philip Trettner's avatar
Philip Trettner committed
267
268
269
270
271
272
273
    {
        vec<3, ScalarT> Ax = {
            A00 * p.x + A01 * p.y + A02 * p.z, //
            A01 * p.x + A11 * p.y + A12 * p.z, //
            A02 * p.x + A12 * p.y + A22 * p.z, //
        };

Philip Trettner's avatar
Philip Trettner committed
274
275
276
        return dot(vec<3, ScalarT>(p), Ax)            // x^T A x
               - 2 * (p.x * b0 + p.y * b1 + p.z * b2) // - 2 r^T x
               + c;                                   // + c
Philip Trettner's avatar
Philip Trettner committed
277
278
    }

279
    // TODO: fix me
280
    template <class T>
281
    friend constexpr T distance_sqr(pos<3, T> const& p, quadric<3, T> const& q);
282
    template <class T>
283
284
    friend constexpr pos<3, T> closest_point(quadric<3, T> const& q);
};
Philip Trettner's avatar
Philip Trettner committed
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314

// reflection
template <class I, int D, class ScalarT>
constexpr void introspect(I&& i, quadric<D, ScalarT>& v)
{
    if constexpr (D == 2)
    {
        i(v.A00, "A00");
        i(v.A01, "A01");
        i(v.A11, "A11");
        i(v.b0, "b0");
        i(v.b1, "b1");
        i(v.c, "c");
    }
    else if constexpr (D == 3)
    {
        i(v.A00, "A00");
        i(v.A01, "A01");
        i(v.A02, "A02");
        i(v.A11, "A11");
        i(v.A12, "A12");
        i(v.A22, "A22");
        i(v.b0, "b0");
        i(v.b1, "b1");
        i(v.b2, "b2");
        i(v.c, "c");
    }
    else
        static_assert(always_false<D>, "quadrics are only defined for 2D and 3D");
}
315
}