quadric.hh 9.15 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
    using mat4_t = tg::mat<4, 4, ScalarT>;
135

Philip Trettner's avatar
Philip Trettner committed
136
137
138
139
140
141
142
143
    // 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);
144

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

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

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
158
159
160
161
162
163
164
165
166
    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
167
168
169
170
        q.b0 = b0;
        q.b1 = b1;
        q.b2 = b2;
        q.c = c;
Philip Trettner's avatar
Philip Trettner committed
171
172
        return q;
    }
Philip Trettner's avatar
Philip Trettner committed
173
    static constexpr quadric from_coefficients(mat_t const& A, vec_t const& b, scalar_t c)
Philip Trettner's avatar
Philip Trettner committed
174
175
176
177
178
179
180
181
    {
        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
182
183
184
185
        q.b0 = b[0];
        q.b1 = b[1];
        q.b2 = b[2];
        q.c = c;
Philip Trettner's avatar
Philip Trettner committed
186
187
        return q;
    }
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
    static constexpr quadric from_coefficients(mat4_t const& Q)
    {
        quadric q;
        q.A00 = Q[0][0];
        q.A01 = Q[0][1];
        q.A02 = Q[0][2];
        q.A11 = Q[1][1];
        q.A12 = Q[1][2];
        q.A22 = Q[2][2];
        q.b0 = -Q[3][0];
        q.b1 = -Q[3][1];
        q.b2 = -Q[3][2];
        q.c = Q[3][3];
        return q;
    }
Philip Trettner's avatar
Philip Trettner committed
203

204
public:
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
    // x^T Q x
    constexpr mat<4, 4, ScalarT> const Q() const
    {
        mat<4, 4, 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;
        m[3][0] = -b0;
        m[3][1] = -b1;
        m[3][2] = -b2;
        m[0][3] = -b0;
        m[1][3] = -b1;
        m[2][3] = -b2;
        m[3][3] = c;
        return m;
    }
Philip Trettner's avatar
Philip Trettner committed
227
    constexpr mat<3, 3, ScalarT> const A() const
228
229
230
231
232
233
234
235
236
237
238
239
240
    {
        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
241
    constexpr vec<3, ScalarT> const b() const { return {b0, b1, b2}; }
242

243
244
245
246
247
    // 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
248
    void add_plane(pos_t pos, vec_t normal, scalar_t sigma)
249
    {
Philip Trettner's avatar
Philip Trettner committed
250
        auto d = dot(pos - pos_t::zero, normal);
251
252
253
254
255
256
257
258
259
        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
260
261
262
        b0 += normal.x * d + pos.x * s2;
        b1 += normal.y * d + pos.y * s2;
        b2 += normal.z * d + pos.z * s2;
263
264

        // TODO: maybe positional uncertainty
Philip Trettner's avatar
Philip Trettner committed
265
        c += d * d + distance_sqr_to_origin(pos) * s2;
266
267
268
269
270
271
272
273
274
275
276
277
    }

    /// 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
278
279
280
        b0 += rhs.b0;
        b1 += rhs.b1;
        b2 += rhs.b2;
281

Philip Trettner's avatar
Philip Trettner committed
282
        c += rhs.c;
283
284
    }

Philip Trettner's avatar
Philip Trettner committed
285
286
287
288
289
290
291
292
293
294
295
    // 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
296
297
298
        b0 += b.x;
        b1 += b.y;
        b2 += b.z;
Philip Trettner's avatar
Philip Trettner committed
299

Philip Trettner's avatar
Philip Trettner committed
300
        c += c;
Philip Trettner's avatar
Philip Trettner committed
301
302
    }

Philip Trettner's avatar
Philip Trettner committed
303
    // Residual L2 error as given by x^T A x - 2 r^T x + c
304
    [[nodiscard]] constexpr ScalarT operator()(pos<3, ScalarT> const& p) const
Philip Trettner's avatar
Philip Trettner committed
305
306
307
308
309
310
311
    {
        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
312
313
314
        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
315
316
    }

Philip Trettner's avatar
Philip Trettner committed
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
    [[nodiscard]] constexpr quadric operator-() const
    {
        quadric q;
        q.A00 = -A00;
        q.A11 = -A11;
        q.A22 = -A22;
        q.A01 = -A01;
        q.A02 = -A02;
        q.A12 = -A12;
        q.b0 = -b0;
        q.b1 = -b1;
        q.b2 = -b2;
        q.c = -c;
        return q;
    }

333
    // TODO: fix me
334
    template <class T>
335
    friend constexpr T distance_sqr(pos<3, T> const& p, quadric<3, T> const& q);
336
    template <class T>
337
338
    friend constexpr pos<3, T> closest_point(quadric<3, T> const& q);
};
Philip Trettner's avatar
Philip Trettner committed
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368

// 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");
}
369
}