quadric.hh 7 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

Philip Trettner's avatar
Philip Trettner committed
41
42
43
44
45
    // 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);
46

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

Philip Trettner's avatar
Philip Trettner committed
50
    scalar_t c = scalar_t(0);
51
52
53
54
55
56
57
58
59
60
61
62
63
64

public:
    constexpr quadric() = default;

    // 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
65
    void add_line(pos_t pos, vec_t normal, scalar_t sigma)
66
    {
Philip Trettner's avatar
Philip Trettner committed
67
        auto d = dot(pos - pos_t::zero, normal);
68
69
70
71
72
73
        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
74
75
        b0 += normal.x * d + pos.x * s2;
        b1 += normal.y * d + pos.y * s2;
76
77

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

81
82
83
84
85
86
87
    // add two quadrics
    void add(quadric const& rhs)
    {
        A00 += rhs.A00;
        A01 += rhs.A01;
        A11 += rhs.A11;

Philip Trettner's avatar
Philip Trettner committed
88
89
        b0 += rhs.b0;
        b1 += rhs.b1;
90

Philip Trettner's avatar
Philip Trettner committed
91
        c += rhs.c;
92
93
    }

Philip Trettner's avatar
Philip Trettner committed
94
95
96
97
98
99
100
101
    // Residual L2 error as given by x^T A x - 2 r^T x + c
    TG_NODISCARD constexpr ScalarT operator()(pos<2, ScalarT> const& p) const
    {
        vec<2, ScalarT> Ax = {
            A00 * p.x + A01 * p.y, //
            A01 * p.x + A11 * p.y, //
        };

Philip Trettner's avatar
Philip Trettner committed
102
103
104
        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
105
106
    }

107
    // TODO: fix me
108
    template <class T>
109
    friend constexpr T distance_sqr(pos<2, T> const& p, quadric<2, T> const& q);
110
    template <class T>
111
112
113
114
115
116
117
    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
118
119
    using vec_t = tg::vec<3, ScalarT>;
    using pos_t = tg::pos<3, ScalarT>;
Philip Trettner's avatar
Philip Trettner committed
120
    using mat_t = tg::mat<3, 3, ScalarT>;
121

Philip Trettner's avatar
Philip Trettner committed
122
123
124
125
126
127
128
129
    // 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);
130

Philip Trettner's avatar
Philip Trettner committed
131
132
133
    scalar_t b0 = scalar_t(0);
    scalar_t b1 = scalar_t(0);
    scalar_t b2 = scalar_t(0);
134

Philip Trettner's avatar
Philip Trettner committed
135
    scalar_t c = scalar_t(0);
136
137
138
139
140
141
142
143

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
144
145
146
147
148
149
150
151
152
    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
153
154
155
156
        q.b0 = b0;
        q.b1 = b1;
        q.b2 = b2;
        q.c = c;
Philip Trettner's avatar
Philip Trettner committed
157
158
        return q;
    }
Philip Trettner's avatar
Philip Trettner committed
159
    static constexpr quadric from_coefficients(mat_t const& A, vec_t const& b, scalar_t c)
Philip Trettner's avatar
Philip Trettner committed
160
161
162
163
164
165
166
167
    {
        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
168
169
170
171
        q.b0 = b[0];
        q.b1 = b[1];
        q.b2 = b[2];
        q.c = c;
Philip Trettner's avatar
Philip Trettner committed
172
173
174
        return q;
    }

175
public:
Philip Trettner's avatar
Philip Trettner committed
176
    constexpr mat<3, 3, ScalarT> const A() const
177
178
179
180
181
182
183
184
185
186
187
188
189
    {
        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
190
    constexpr vec<3, ScalarT> const b() const { return {b0, b1, b2}; }
191

192
193
194
195
196
    // 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
197
    void add_plane(pos_t pos, vec_t normal, scalar_t sigma)
198
    {
Philip Trettner's avatar
Philip Trettner committed
199
        auto d = dot(pos - pos_t::zero, normal);
200
201
202
203
204
205
206
207
208
        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
209
210
211
        b0 += normal.x * d + pos.x * s2;
        b1 += normal.y * d + pos.y * s2;
        b2 += normal.z * d + pos.z * s2;
212
213

        // TODO: maybe positional uncertainty
Philip Trettner's avatar
Philip Trettner committed
214
        c += d * d + distance_sqr_to_origin(pos) * s2;
215
216
217
218
219
220
221
222
223
224
225
226
    }

    /// 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
227
228
229
        b0 += rhs.b0;
        b1 += rhs.b1;
        b2 += rhs.b2;
230

Philip Trettner's avatar
Philip Trettner committed
231
        c += rhs.c;
232
233
    }

Philip Trettner's avatar
Philip Trettner committed
234
235
236
237
238
239
240
241
242
243
244
    // 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
245
246
247
        b0 += b.x;
        b1 += b.y;
        b2 += b.z;
Philip Trettner's avatar
Philip Trettner committed
248

Philip Trettner's avatar
Philip Trettner committed
249
        c += c;
Philip Trettner's avatar
Philip Trettner committed
250
251
    }

Philip Trettner's avatar
Philip Trettner committed
252
253
254
255
256
257
258
259
260
    // Residual L2 error as given by x^T A x - 2 r^T x + c
    TG_NODISCARD constexpr ScalarT operator()(pos<3, ScalarT> const& p) const
    {
        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
261
262
263
        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
264
265
    }

266
    // TODO: fix me
267
    template <class T>
268
    friend constexpr T distance_sqr(pos<3, T> const& p, quadric<3, T> const& q);
269
    template <class T>
270
271
272
    friend constexpr pos<3, T> closest_point(quadric<3, T> const& q);
};
}