Commit 465a3dc3 authored by Philip Trettner's avatar Philip Trettner
Browse files

improved quadric API

parent f1e5a051
......@@ -55,9 +55,9 @@ TG_NODISCARD constexpr pos<3, ScalarT> closest_point(quadric<3, ScalarT> const&
auto d = q.A11;
auto e = q.A12;
auto f = q.A22;
auto r0 = q.r.x;
auto r1 = q.r.y;
auto r2 = q.r.z;
auto r0 = q.b0;
auto r1 = q.b1;
auto r2 = q.b2;
auto ad = a * d;
auto ae = a * e;
......@@ -92,8 +92,8 @@ TG_NODISCARD constexpr pos<2, ScalarT> closest_point(quadric<2, ScalarT> const&
auto c = q.A11;
auto denom = 1 / (a * c - b * b);
auto nom0 = q.r.x * c - q.r.y * b;
auto nom1 = q.r.y * a - q.r.x * b;
auto nom0 = q.b0 * c - q.b1 * b;
auto nom1 = q.b1 * a - q.b0 * b;
return {nom0 * denom, nom1 * denom};
}
......
......@@ -16,18 +16,15 @@ namespace tg
template <int D, class ScalarT>
TG_NODISCARD constexpr quadric<D, ScalarT> point_quadric(pos<D, ScalarT> const& p)
{
quadric<D, ScalarT> Q;
Q.add(mat<D, D, ScalarT>::identity, vec<D, ScalarT>(p), ScalarT(0));
return Q;
auto const v = tg::vec<D, ScalarT>(p);
return quadric<D, ScalarT>::from_coefficients(mat<D, D, ScalarT>::identity, v, dot(v, v));
}
template <int D, class ScalarT>
TG_NODISCARD constexpr quadric<D, ScalarT> plane_quadric(pos<D, ScalarT> const& p, vec<D, ScalarT> const& n)
{
auto const d = dot(p, n);
quadric<D, ScalarT> Q;
Q.add(self_outer_product(n), n * d, d * d);
return Q;
return quadric<D, ScalarT>::from_coefficients(self_outer_product(n), n * d, d * d);
}
template <int D, class ScalarT>
......@@ -49,9 +46,7 @@ TG_NODISCARD constexpr quadric<D, ScalarT> probabilistic_plane_quadric(pos<D, Sc
auto const b = mean_n * d + sn2 * p;
auto const c = d * d + sn2 * dot(p, p) + sp2 * dot(mean_n, mean_n) + sp2 * sn2;
quadric<D, ScalarT> Q;
Q.add(A, b, c);
return Q;
return quadric<D, ScalarT>::from_coefficients(A, b, c);
}
template <int D, class ScalarT>
......@@ -67,23 +62,19 @@ TG_NODISCARD constexpr quadric<D, ScalarT> probabilistic_plane_quadric(pos<D, Sc
auto const b = mean_n * d + sigma_n * p;
auto const c = d * d + dot(p, sigma_n * p) + dot(mean_n, sigma_p * mean_n) + trace_of_product(sigma_n, sigma_p);
quadric<D, ScalarT> Q;
Q.add(A, b, c);
return Q;
return quadric<D, ScalarT>::from_coefficients(A, b, c);
}
template <int D, class ScalarT>
TG_NODISCARD constexpr quadric<D, ScalarT> triangle_quadric(pos<D, ScalarT> const& p, pos<D, ScalarT> const& q, pos<D, ScalarT> const& r)
{
auto const pxq = cross(p, q);
auto const qxr = cross(q, r);
auto const rxp = cross(r, p);
auto const pxq = cross(vec(p), vec(q));
auto const qxr = cross(vec(q), vec(r));
auto const rxp = cross(vec(r), vec(p));
auto const xsum = pxq + qxr + rxp;
auto const det = dot(pxq, r);
quadric<D, ScalarT> Q;
Q.add(self_outer_product(xsum), xsum * det, det * det);
return Q;
return quadric<D, ScalarT>::from_coefficients(self_outer_product(xsum), xsum * det, det * det);
}
}
......@@ -35,18 +35,19 @@ template <class ScalarT>
struct quadric<2, ScalarT>
{
using scalar_t = ScalarT;
using vec2_t = tg::vec<2, ScalarT>;
using pos2_t = tg::pos<2, ScalarT>;
using dir2_t = tg::dir<2, ScalarT>;
using vec_t = tg::vec<2, ScalarT>;
using pos_t = tg::pos<2, ScalarT>;
private:
scalar_t A00 = 0;
scalar_t A01 = 0;
scalar_t A11 = 0;
// 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);
vec2_t r = vec2_t::zero;
scalar_t b0 = scalar_t(0);
scalar_t b1 = scalar_t(0);
scalar_t d_sqr = 0;
scalar_t c = scalar_t(0);
public:
constexpr quadric() = default;
......@@ -61,19 +62,20 @@ 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`
void add_line(pos2_t pos, vec2_t normal, scalar_t sigma)
void add_line(pos_t pos, vec_t normal, scalar_t sigma)
{
auto d = dot(pos - pos2_t::zero, normal);
auto d = dot(pos - pos_t::zero, normal);
auto s2 = sigma * sigma;
A00 += normal.x * normal.x + s2;
A01 += normal.x * normal.y;
A11 += normal.y * normal.y + s2;
r += normal * d + vec2_t(pos) * s2;
b0 += normal.x * d + pos.x * s2;
b1 += normal.y * d + pos.y * s2;
// TODO: maybe positional uncertainty
d_sqr += d * d + distance_sqr_to_origin(pos) * s2;
c += d * d + distance_sqr_to_origin(pos) * s2;
}
// add two quadrics
......@@ -83,9 +85,10 @@ public:
A01 += rhs.A01;
A11 += rhs.A11;
r += rhs.r;
b0 += rhs.b0;
b1 += rhs.b1;
d_sqr += rhs.d_sqr;
c += rhs.c;
}
// Residual L2 error as given by x^T A x - 2 r^T x + c
......@@ -96,9 +99,9 @@ public:
A01 * p.x + A11 * p.y, //
};
return dot(vec<2, ScalarT>(p), Ax) // x^T A x
- 2 * dot(vec<2, ScalarT>(p), r) // - 2 r^T x
+ d_sqr; // + c
return dot(vec<2, ScalarT>(p), Ax) // x^T A x
- 2 * (b0 * p.x + b1 * p.y) // - 2 r^T x
+ c; // + c
}
// TODO: fix me
......@@ -112,21 +115,23 @@ template <class ScalarT>
struct quadric<3, ScalarT>
{
using scalar_t = ScalarT;
using vec3_t = tg::vec<3, ScalarT>;
using pos3_t = tg::pos<3, ScalarT>;
using dir3_t = tg::dir<3, ScalarT>;
using vec_t = tg::vec<3, ScalarT>;
using pos_t = tg::pos<3, ScalarT>;
private:
scalar_t A00 = 0;
scalar_t A01 = 0;
scalar_t A02 = 0;
scalar_t A11 = 0;
scalar_t A12 = 0;
scalar_t A22 = 0;
// 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);
vec3_t r = vec3_t::zero;
scalar_t b0 = scalar_t(0);
scalar_t b1 = scalar_t(0);
scalar_t b2 = scalar_t(0);
scalar_t d_sqr = 0;
scalar_t c = scalar_t(0);
public:
constexpr quadric() = default;
......@@ -144,10 +149,10 @@ public:
q.A11 = A11;
q.A12 = A12;
q.A22 = A22;
q.r.x = b0;
q.r.y = b1;
q.r.z = b2;
q.d_sqr = c;
q.b0 = b0;
q.b1 = b1;
q.b2 = b2;
q.c = c;
return q;
}
static constexpr quadric from_coefficients(tg::mat3 const& A, tg::vec3 const& b, float c)
......@@ -159,15 +164,15 @@ public:
q.A11 = A[1][1];
q.A12 = A[1][2];
q.A22 = A[2][2];
q.r.x = b[0];
q.r.y = b[1];
q.r.z = b[2];
q.d_sqr = c;
q.b0 = b[0];
q.b1 = b[1];
q.b2 = b[2];
q.c = c;
return q;
}
public:
constexpr mat<3, 3, ScalarT> A() const
constexpr mat<3, 3, ScalarT> const A() const
{
mat<3, 3, ScalarT> m;
m[0][0] = A00;
......@@ -181,17 +186,16 @@ public:
m[2][2] = A22;
return m;
}
constexpr vec<3, ScalarT> b() const { return r; }
constexpr scalar_t c() const { return d_sqr; }
constexpr vec<3, ScalarT> const b() const { return {b0, b1, b2}; }
// 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`
void add_plane(pos3_t pos, vec3_t normal, scalar_t sigma)
void add_plane(pos_t pos, vec_t normal, scalar_t sigma)
{
auto d = dot(pos - pos3_t::zero, normal);
auto d = dot(pos - pos_t::zero, normal);
auto s2 = sigma * sigma;
A00 += normal.x * normal.x + s2;
......@@ -201,10 +205,12 @@ public:
A12 += normal.y * normal.z;
A22 += normal.z * normal.z + s2;
r += normal * d + vec3_t(pos) * s2;
b0 += normal.x * d + pos.x * s2;
b1 += normal.y * d + pos.y * s2;
b2 += normal.z * d + pos.z * s2;
// TODO: maybe positional uncertainty
d_sqr += d * d + distance_sqr_to_origin(pos) * s2;
c += d * d + distance_sqr_to_origin(pos) * s2;
}
/// Adds two quadrics
......@@ -217,9 +223,11 @@ public:
A12 += rhs.A12;
A22 += rhs.A22;
r += rhs.r;
b0 += rhs.b0;
b1 += rhs.b1;
b2 += rhs.b2;
d_sqr += rhs.d_sqr;
c += rhs.c;
}
// adds a quadric given in matrix form (x^T A x + b^T x + c)
......@@ -233,9 +241,11 @@ public:
A12 += A[1][2];
A22 += A[2][2];
r += b;
b0 += b.x;
b1 += b.y;
b2 += b.z;
d_sqr += c;
c += c;
}
// Residual L2 error as given by x^T A x - 2 r^T x + c
......@@ -247,9 +257,9 @@ public:
A02 * p.x + A12 * p.y + A22 * p.z, //
};
return dot(vec<3, ScalarT>(p), Ax) // x^T A x
- 2 * dot(vec<3, ScalarT>(p), r) // - 2 r^T x
+ d_sqr; // + c
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
}
// TODO: fix me
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment