Merge branch 'c++11_matrix' into 'master'

C++11 matrix

Added a modern (C++11) 3x3 matrix class and a closest point in triangle query that builds on top of it.

See merge request !148
parents d03842e6 ae668cf2
Pipeline #3358 passed with stage
in 57 minutes and 37 seconds
 ... ... @@ -61,6 +61,8 @@ #include #include #include "../Math/Matrix3x3T.hh" namespace ACG { namespace Geometry { ... ... @@ -633,6 +635,71 @@ roundness( const VectorT& _v0, /** @} */ template Vector closestPointLineSegment(Vector x, Vector p1, Vector p2) { const auto delta = ((p2-p1)|(x-p1)) / (p2-p1).sqrnorm(); //std::cout << "\x1b[32mdelta = " << delta << "\x1b[0m" << std::endl; if (delta <= 0) { //std::cout << "\x1b[43mdelta <= 0\x1b[0m" << std::endl; return p1; } else if (delta >= 1) { //std::cout << "\x1b[43mdelta >= 1\x1b[0m" << std::endl; return p2; } else if (delta != delta) { // p1 = p2 or x = p1 //std::cout << "\x1b[43mdelta != delta\x1b[0m" << std::endl; return (x-p1).sqrnorm() < (x-p2).sqrnorm() ? p1 : p2; } else { //std::cout << "\x1b[43mdelta \\in [0, 1]\x1b[0m" << std::endl; return (1 - delta) * p1 + delta * p2; } }; template Vector closestPointTri(Vector p, Vector a, Vector b, Vector c) { constexpr double thresh = 1e-8; const auto n = ((b - a) % (c - a)); // normalization unnecessary if ((b-a).sqrnorm() < thresh || (c-a).sqrnorm() < thresh || n.sqrnorm() < thresh) { //std::cout << "\x1b[42mDegenerate case.\x1b[0m" << std::endl; // Degenerate triangle. Find distance to longest segment. std::array max_segment = {a, b}; double max_len = (b-a).sqrnorm(); if ((c-a).sqrnorm() > max_len) max_segment = {a, c}; if ((c-b).sqrnorm() > max_len) max_segment = {b, c}; // closestPointLineSegment is stable, even if the segment is super short return closestPointLineSegment(p, max_segment, max_segment); } const auto abd = Matrix3x3d::fromColumns(a-c, b-c, n).inverse() * (p - c); const bool alpha = (abd >= 0.0), beta = (abd >= 0.0), gamma = (1.0-abd-abd >= 0.0); if (alpha && beta && gamma) { //std::cout << "\x1b[42mInside case.\x1b[0m" << std::endl; // Inside triangle. return abd * a + abd * b + (1.0 - abd - abd) * c; } else if (!alpha) { //std::cout << "\x1b[42m!alpha case.\x1b[0m" << std::endl; // Closest to line segment (b, c). return closestPointLineSegment(p, b, c); } else if (!beta) { //std::cout << "\x1b[42m!beta case.\x1b[0m" << std::endl; // Closest to line segment (a, c). return closestPointLineSegment(p, a, c); } else if (!gamma) { //std::cout << "\x1b[42m!gamma case.\x1b[0m" << std::endl; // Closest to line segment (a, b). return closestPointLineSegment(p, a, b); } else { throw std::logic_error("This cannot happen."); } } //============================================================================= } // namespace Geometry } // namespace ACG ... ...
 #ifndef ACG_MATH_MATRIX3X3T_HH_ #define ACG_MATH_MATRIX3X3T_HH_ #include #include #include #include #include #if defined(_MSC_VER) && _MSC_VER < 1900 #define constexpr typedef unsigned char uint_fast8_t; #endif namespace ACG { /** * Small, kinda fast 3x3 matrix class. */ template class Matrix3x3T { public: typedef typename OpenMesh::VectorT Vec3; static Matrix3x3T fromColumns(Vec3 c1, Vec3 c2, Vec3 c3) { return Matrix3x3T {{ c1, c2, c3, c1, c2, c3, c1, c2, c3, }}; } static Matrix3x3T fromRows(Vec3 r1, Vec3 r2, Vec3 r3) { return Matrix3x3T {{ r1, r1, r1, r2, r2, r2, r3, r3, r3 }}; } static constexpr Matrix3x3T identity() { return {{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }}; } static constexpr Matrix3x3T zero() { return {{ 0, 0, 0, 0, 0, 0, 0, 0, 0 }}; } public: Matrix3x3T() = default; /** * Initialize matrix from array in row major format. */ constexpr Matrix3x3T(std::array row_major) : values_(std::move(row_major)) {} constexpr bool operator==(const Matrix3x3T &rhs) const { return values_ == rhs.values_; } //Matrix3x3T(std::initializer_list row_major) { // static_assert(row_major.size() == 9, "Need exactly 9 values."); // std::copy(row_major.begin(), row_major.end(), this->begin()); //} /// Map row/column index to linearized index. constexpr static uint_fast8_t indexof(uint_fast8_t r, uint_fast8_t c) { return r*3+c; } constexpr const Scalar &operator() (uint_fast8_t r, uint_fast8_t c) const { return values_[indexof(r, c)]; } Scalar &operator() (uint_fast8_t r, uint_fast8_t c) { return values_[indexof(r, c)]; } /// Linearized row major access. constexpr const Scalar &operator[] (uint_fast8_t i) const { return values_[i]; } /// Linearized row major access. Scalar &operator[] (uint_fast8_t i) { return values_[i]; } constexpr Matrix3x3T operator*(const Matrix3x3T &rhs) const { return Matrix3x3T {{ (*this)(0, 0) * rhs(0, 0) + (*this)(0, 1) * rhs(1, 0) + (*this)(0, 2) * rhs(2, 0), (*this)(0, 0) * rhs(0, 1) + (*this)(0, 1) * rhs(1, 1) + (*this)(0, 2) * rhs(2, 1), (*this)(0, 0) * rhs(0, 2) + (*this)(0, 1) * rhs(1, 2) + (*this)(0, 2) * rhs(2, 2), (*this)(1, 0) * rhs(0, 0) + (*this)(1, 1) * rhs(1, 0) + (*this)(1, 2) * rhs(2, 0), (*this)(1, 0) * rhs(0, 1) + (*this)(1, 1) * rhs(1, 1) + (*this)(1, 2) * rhs(2, 1), (*this)(1, 0) * rhs(0, 2) + (*this)(1, 1) * rhs(1, 2) + (*this)(1, 2) * rhs(2, 2), (*this)(2, 0) * rhs(0, 0) + (*this)(2, 1) * rhs(1, 0) + (*this)(2, 2) * rhs(2, 0), (*this)(2, 0) * rhs(0, 1) + (*this)(2, 1) * rhs(1, 1) + (*this)(2, 2) * rhs(2, 1), (*this)(2, 0) * rhs(0, 2) + (*this)(2, 1) * rhs(1, 2) + (*this)(2, 2) * rhs(2, 2), }}; } constexpr Vec3 operator*(const Vec3 &rhs) const { return Vec3( (*this)(0, 0) * rhs + (*this)(0, 1) * rhs + (*this)(0, 2) * rhs, (*this)(1, 0) * rhs + (*this)(1, 1) * rhs + (*this)(1, 2) * rhs, (*this)(2, 0) * rhs + (*this)(2, 1) * rhs + (*this)(2, 2) * rhs ); } constexpr friend Vec3 operator*(Vec3 v, const Matrix3x3T &rhs) { return Vec3( rhs(0, 0) * v + rhs(0, 1) * v + rhs(0, 2) * v, rhs(1, 0) * v + rhs(1, 1) * v + rhs(1, 2) * v, rhs(2, 0) * v + rhs(2, 1) * v + rhs(2, 2) * v ); } constexpr Matrix3x3T operator*(Scalar c) const { return Matrix3x3T {{ (*this) * c, (*this) * c, (*this) * c, (*this) * c, (*this) * c, (*this) * c, (*this) * c, (*this) * c, (*this) * c, }}; } constexpr friend Matrix3x3T operator*(Scalar c, const Matrix3x3T &rhs) { return Matrix3x3T {{ rhs * c, rhs * c, rhs * c, rhs * c, rhs * c, rhs * c, rhs * c, rhs * c, rhs * c, }}; } constexpr Matrix3x3T operator+ (const Matrix3x3T &rhs) const { return Matrix3x3T {{ (*this) + rhs, (*this) + rhs, (*this) + rhs, (*this) + rhs, (*this) + rhs, (*this) + rhs, (*this) + rhs, (*this) + rhs, (*this) + rhs, }}; } constexpr Matrix3x3T operator- (const Matrix3x3T &rhs) const { return Matrix3x3T {{ (*this) - rhs, (*this) - rhs, (*this) - rhs, (*this) - rhs, (*this) - rhs, (*this) - rhs, (*this) - rhs, (*this) - rhs, (*this) - rhs, }}; } constexpr Matrix3x3T operator- () const { return Matrix3x3T {{ -values_, -values_, -values_, -values_, -values_, -values_, -values_, -values_, -values_ }}; } const Matrix3x3T &operator*=(const Matrix3x3T &rhs) { (*this) = operator*(rhs); return *this; } constexpr Scalar det() const { return (*this)(0, 0) * ((*this)(1, 1) * (*this)(2, 2) - (*this)(2, 1) * (*this)(1, 2)) - (*this)(0, 1) * ((*this)(1, 0) * (*this)(2, 2) - (*this)(1, 2) * (*this)(2, 0)) + (*this)(0, 2) * ((*this)(1, 0) * (*this)(2, 1) - (*this)(1, 1) * (*this)(2, 0)); /* return (*this)(0, 0) * (*this)(1, 1) * (*this)(2, 2) + (*this)(1, 0) * (*this)(2, 1) * (*this)(0, 2) + (*this)(2, 0) * (*this)(0, 1) * (*this)(1, 2) - (*this)(0, 0) * (*this)(2, 1) * (*this)(1, 2) - (*this)(2, 0) * (*this)(1, 1) * (*this)(0, 2) - (*this)(1, 0) * (*this)(0, 1) * (*this)(2, 2) */ } constexpr Scalar trace() const { return (*this) + (*this) + (*this); } void transpose() { std::swap(values_, values_); std::swap(values_, values_); std::swap(values_, values_); } constexpr Matrix3x3T transposed() const { return Matrix3x3T {{ values_, values_, values_, values_, values_, values_, values_, values_, values_, }}; } void invert() { *this = inverse(); } Matrix3x3T inverse() const { const Scalar invdet = 1.0 / det(); return Matrix3x3T {{ ((*this)(1, 1) * (*this)(2, 2) - (*this)(2, 1) * (*this)(1, 2)) * invdet, ((*this)(0, 2) * (*this)(2, 1) - (*this)(0, 1) * (*this)(2, 2)) * invdet, ((*this)(0, 1) * (*this)(1, 2) - (*this)(0, 2) * (*this)(1, 1)) * invdet, ((*this)(1, 2) * (*this)(2, 0) - (*this)(1, 0) * (*this)(2, 2)) * invdet, ((*this)(0, 0) * (*this)(2, 2) - (*this)(0, 2) * (*this)(2, 0)) * invdet, ((*this)(1, 0) * (*this)(0, 2) - (*this)(0, 0) * (*this)(1, 2)) * invdet, ((*this)(1, 0) * (*this)(2, 1) - (*this)(2, 0) * (*this)(1, 1)) * invdet, ((*this)(2, 0) * (*this)(0, 1) - (*this)(0, 0) * (*this)(2, 1)) * invdet, ((*this)(0, 0) * (*this)(1, 1) - (*this)(1, 0) * (*this)(0, 1)) * invdet, }}; } constexpr Scalar frobeniusSquared() const { return std::inner_product( values_.begin(), values_.end(), values_.begin(), Scalar(0.0)); } constexpr double frobenius() const { return std::sqrt(frobeniusSquared()); } friend std::ostream &operator<< (std::ostream &os, const Matrix3x3T &m) { os << "[[" << m << ", " << m << ", " << m << "], " "[" << m << ", " << m << ", " << m << "], " "[" << m << ", " << m << ", " << m << "]]"; return os; } private: std::array values_; }; typedef Matrix3x3T Matrix3x3f; typedef Matrix3x3T Matrix3x3d; } /* namespace ACG */ #if defined(_MSC_VER) && _MSC_VER < 1900 #undef constexpr #endif #endif /* ACG_MATH_MATRIX3X3T_HH_ */