Loading...
Searching...
No Matches
Error_quadric.h
/* This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
* See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
* Author(s): David Salinas
*
* Copyright (C) 2014 Inria
*
* Modification(s):
* - YYYY/MM Author: Description of the modification
*/
#ifndef GARLAND_HECKBERT_ERROR_QUADRIC_H_
#define GARLAND_HECKBERT_ERROR_QUADRIC_H_
#include <boost/optional/optional.hpp>
#include <vector>
#include <utility>
template <typename Point> class Error_quadric {
private:
double coeff[10];
public:
Error_quadric() {
clear();
}
Error_quadric(const Point & p0, const Point & p1, const Point & p2) {
Point normal(unit_normal(p0, p1, p2));
double a = normal[0];
double b = normal[1];
double c = normal[2];
double d = -a * p0[0] - b * p0[1] - c * p0[2];
coeff[0] = a*a;
coeff[1] = a*b;
coeff[2] = a*c;
coeff[3] = a*d;
coeff[4] = b*b;
coeff[5] = b*c;
coeff[6] = b*d;
coeff[7] = c*c;
coeff[8] = c*d;
coeff[9] = d*d;
double area_p0p1p2 = std::sqrt(squared_area(p0, p1, p2));
for (auto& x : coeff)
x *= area_p0p1p2;
}
inline double squared_area(const Point& p0, const Point& p1, const Point& p2) {
// if (x1,x2,x3) = p1-p0 and (y1,y2,y3) = p2-p0
// then the squared area is = (u^2+v^2+w^2)/4
// with: u = x2 * y3 - x3 * y2;
// v = x3 * y1 - x1 * y3;
// w = x1 * y2 - x2 * y1;
Point p0p1(p1 - p0);
Point p0p2(p2 - p0);
double A = p0p1[1] * p0p2[2] - p0p1[2] * p0p2[1];
double B = p0p1[2] * p0p2[0] - p0p1[0] * p0p2[2];
double C = p0p1[0] * p0p2[1] - p0p1[1] * p0p2[0];
return 1. / 4. * (A * A + B * B + C * C);
}
void clear() {
for (auto& x : coeff)
x = 0;
}
Error_quadric& operator+=(const Error_quadric& other) {
if (this != &other) {
for (int i = 0; i < 10; ++i)
coeff[i] += other.coeff[i];
}
return *this;
}
inline double cost(const Point& point) const {
double cost =
coeff[0] * point.x() * point.x() + coeff[4] * point.y() * point.y() + coeff[7] * point.z() * point.z()
+ 2 * (coeff[1] * point.x() * point.y() + coeff[5] * point.y() * point.z() + coeff[2] * point.z() * point.x())
+ 2 * (coeff[3] * point.x() + coeff[6] * point.y() + coeff[8] * point.z())
+ coeff[9];
if (cost < 0) {
return 0;
} else {
return cost;
}
}
inline double grad_determinant() const {
return
coeff[0] * coeff[4] * coeff[7]
- coeff[0] * coeff[5] * coeff[5]
- coeff[1] * coeff[1] * coeff[7]
+ 2 * coeff[1] * coeff[5] * coeff[2]
- coeff[4] * coeff[2] * coeff[2];
}
inline Point solve_linear_gradient(double det) const {
return Point({
(-coeff[1] * coeff[5] * coeff[8] + coeff[1] * coeff[7] * coeff[6] + coeff[2] * coeff[8] * coeff[4] -
coeff[2] * coeff[5] * coeff[6] - coeff[3] * coeff[4] * coeff[7] + coeff[3] * coeff[5] * coeff[5])
/ det,
(coeff[0] * coeff[5] * coeff[8] - coeff[0] * coeff[7] * coeff[6] - coeff[5] * coeff[2] * coeff[3] -
coeff[1] * coeff[2] * coeff[8] + coeff[6] * coeff[2] * coeff[2] + coeff[1] * coeff[3] * coeff[7])
/ det,
(-coeff[8] * coeff[0] * coeff[4] + coeff[8] * coeff[1] * coeff[1] + coeff[2] * coeff[3] * coeff[4] +
coeff[5] * coeff[0] * coeff[6] - coeff[5] * coeff[1] * coeff[3] - coeff[1] * coeff[2] * coeff[6])
/ det
});
}
boost::optional<Point> min_cost(double scale = 1) const {
// const double min_determinant = 1e-4 * scale*scale;
const double min_determinant = 1e-5;
boost::optional<Point> pt_res;
double det = grad_determinant();
if (std::abs(det) > min_determinant)
pt_res = solve_linear_gradient(det);
return pt_res;
}
friend std::ostream& operator<<(std::ostream& stream, const Error_quadric& quadric) {
stream << "\n[ " << quadric.coeff[0] << "," << quadric.coeff[1] << "," << quadric.coeff[2] << "," <<
quadric.coeff[3] << ";\n";
stream << " " << quadric.coeff[1] << "," << quadric.coeff[4] << "," << quadric.coeff[5] << "," <<
quadric.coeff[6] << ";\n";
stream << " " << quadric.coeff[2] << "," << quadric.coeff[5] << "," << quadric.coeff[7] << "," <<
quadric.coeff[8] << ";\n";
stream << " " << quadric.coeff[3] << "," << quadric.coeff[6] << "," << quadric.coeff[8] << "," <<
quadric.coeff[9] << "]";
return stream;
}
};
#endif // GARLAND_HECKBERT_ERROR_QUADRIC_H_