Added Matrix3x3T class. C++11 only.

ACG/Math/Matrix3x3T.hh

+#ifndef ACG_MATH_MATRIX3X3T_HH_
+#define ACG_MATH_MATRIX3X3T_HH_
+
+#include <array>
+#include <ostream>
+#include <ACG/Math/VectorT.hh>
+#include <algorithm>
+#include <cmath>
+
+namespace ACG {
+
+/**
+ * Small, kinda fast 3x3 matrix class.
+ */
+template<typename Scalar>
+class Matrix3x3T {
+    public:
+        typedef typename OpenMesh::VectorT<Scalar, 3> Vec3;
+
+        static Matrix3x3T<Scalar> fromColumns(Vec3 c1, Vec3 c2, Vec3 c3) {
+            return Matrix3x3T<Scalar> {{
+                c1[0], c2[0], c3[0],
+                c1[1], c2[1], c3[1],
+                c1[2], c2[2], c3[2],
+            }};
+        }
+
+        static Matrix3x3T<Scalar> fromRows(Vec3 r1, Vec3 r2, Vec3 r3) {
+            return Matrix3x3T<Scalar> {{
+                r1[0], r1[1], r1[2],
+                r2[0], r2[1], r2[2],
+                r3[0], r3[1], r3[2]
+            }};
+        }
+
+        static constexpr Matrix3x3T<Scalar> identity {{
+            1, 0, 0,
+            0, 1, 0,
+            0, 0, 1
+        }};
+        static constexpr Matrix3x3T<Scalar> zero {{
+            0, 0, 0,
+            0, 0, 0,
+            0, 0, 0
+        }};
+
+    public:
+        Matrix3x3T() = default;
+
+        /**
+         * Initialize matrix from array in row major format.
+         */
+        constexpr Matrix3x3T(std::array<Scalar, 9> row_major) :
+                values_(std::move(row_major)) {}
+
+        constexpr bool operator==(const Matrix3x3T &rhs) const {
+            return values_ == rhs.values_;
+        }
+
+        //Matrix3x3T(std::initializer_list<Scalar> 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[0] + (*this)(0, 1) * rhs[1] + (*this)(0, 2) * rhs[2],
+                (*this)(1, 0) * rhs[0] + (*this)(1, 1) * rhs[1] + (*this)(1, 2) * rhs[2],
+                (*this)(2, 0) * rhs[0] + (*this)(2, 1) * rhs[1] + (*this)(2, 2) * rhs[2]
+            );
+        }
+
+        constexpr friend Vec3 operator*(Vec3 v, const Matrix3x3T &rhs) {
+            return Vec3(
+                rhs(0, 0) * v[0] + rhs(0, 1) * v[1] + rhs(0, 2) * v[2],
+                rhs(1, 0) * v[0] + rhs(1, 1) * v[1] + rhs(1, 2) * v[2],
+                rhs(2, 0) * v[0] + rhs(2, 1) * v[1] + rhs(2, 2) * v[2]
+            );
+        }
+
+        constexpr Matrix3x3T operator*(Scalar c) const {
+            return Matrix3x3T {{
+                (*this)[0] * c, (*this)[1] * c, (*this)[2] * c,
+                (*this)[3] * c, (*this)[4] * c, (*this)[5] * c,
+                (*this)[6] * c, (*this)[7] * c, (*this)[8] * c,
+            }};
+        }
+
+        constexpr friend Matrix3x3T operator*(Scalar c, const Matrix3x3T &rhs) {
+            return Matrix3x3T {{
+                rhs[0] * c, rhs[1] * c, rhs[2] * c,
+                rhs[3] * c, rhs[4] * c, rhs[5] * c,
+                rhs[6] * c, rhs[7] * c, rhs[8] * c,
+            }};
+        }
+
+        constexpr Matrix3x3T operator+ (const Matrix3x3T &rhs) const {
+            return Matrix3x3T {{
+                (*this)[0] + rhs[0], (*this)[1] + rhs[1], (*this)[2] + rhs[2],
+                (*this)[3] + rhs[3], (*this)[4] + rhs[4], (*this)[5] + rhs[5],
+                (*this)[6] + rhs[6], (*this)[7] + rhs[7], (*this)[8] + rhs[8],
+            }};
+        }
+
+        constexpr Matrix3x3T operator- (const Matrix3x3T &rhs) const {
+            return Matrix3x3T {{
+                (*this)[0] - rhs[0], (*this)[1] - rhs[1], (*this)[2] - rhs[2],
+                (*this)[3] - rhs[3], (*this)[4] - rhs[4], (*this)[5] - rhs[5],
+                (*this)[6] - rhs[6], (*this)[7] - rhs[7], (*this)[8] - rhs[8],
+            }};
+        }
+
+        constexpr Matrix3x3T operator- () const {
+            return Matrix3x3T {{
+                -values_[0], -values_[1], -values_[2],
+                -values_[3], -values_[4], -values_[5],
+                -values_[6], -values_[7], -values_[8]
+            }};
+        }
+
+        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)[0] + (*this)[4] + (*this)[8];
+        }
+
+        void transpose() {
+            std::swap(values_[1], values_[3]);
+            std::swap(values_[2], values_[6]);
+            std::swap(values_[5], values_[7]);
+        }
+
+        constexpr Matrix3x3T transposed() const {
+            return Matrix3x3T {{
+                values_[0], values_[3], values_[6],
+                values_[1], values_[4], values_[7],
+                values_[2], values_[5], values_[8],
+            }};
+        }
+
+        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[0] << ", " << m[1] << ", " << m[2] << "], "
+                  "[" << m[3] << ", " << m[4] << ", " << m[5] << "], "
+                  "[" << m[6] << ", " << m[7] << ", " << m[8] << "]]";
+            return os;
+        }
+
+    private:
+        std::array<Scalar, 9> values_;
+};
+
+template<typename Scalar>
+constexpr Matrix3x3T<Scalar> Matrix3x3T<Scalar>::zero;
+template<typename Scalar>
+constexpr Matrix3x3T<Scalar> Matrix3x3T<Scalar>::identity;
+
+typedef Matrix3x3T<float> Matrix3x3f;
+typedef Matrix3x3T<double> Matrix3x3d;
+
+}
+
+#endif /* ACG_MATH_MATRIX3X3T_HH_ */

ACG/tests/Math/Matrix3x3.cc

+#include <gtest/gtest.h>
+
+#include <ACG/Math/Matrix3x3T.hh>
+#include "MatrixTestHelper.hh"
+
+namespace {
+
+template<class Scalar>
+class Matrix3x3Test: public testing::Test {
+    public:
+        Matrix3x3Test() {}
+        virtual ~Matrix3x3Test() {}
+
+        typedef typename ACG::Matrix3x3T<Scalar> Matrix3x3;
+};
+
+typedef testing::Types<double, float> Implementations;
+
+TYPED_TEST_CASE(Matrix3x3Test, Implementations);
+
+TYPED_TEST(Matrix3x3Test, access) {
+    using Matrix3x3 = typename Matrix3x3Test<TypeParam>::Matrix3x3;
+
+    Matrix3x3 m {{
+        1, 2, 3,
+        4, 5, 6,
+        7, 8, 9 }};
+
+    EXPECT_EQ(1, m[0]);
+    EXPECT_EQ(1, m(0, 0));
+    EXPECT_EQ(2, m[1]);
+    EXPECT_EQ(2, m(0, 1));
+    EXPECT_EQ(3, m[2]);
+    EXPECT_EQ(3, m(0, 2));
+
+    EXPECT_EQ(4, m[3]);
+    EXPECT_EQ(4, m(1, 0));
+    EXPECT_EQ(5, m[4]);
+    EXPECT_EQ(5, m(1, 1));
+    EXPECT_EQ(6, m[5]);
+    EXPECT_EQ(6, m(1, 2));
+
+    EXPECT_EQ(7, m[6]);
+    EXPECT_EQ(7, m(2, 0));
+    EXPECT_EQ(8, m[7]);
+    EXPECT_EQ(8, m(2, 1));
+    EXPECT_EQ(9, m[8]);
+    EXPECT_EQ(9, m(2, 2));
+}
+
+TYPED_TEST(Matrix3x3Test, construction) {
+
+    using Matrix3x3 = typename Matrix3x3Test<TypeParam>::Matrix3x3;
+    using Vec3 = typename Matrix3x3::Vec3;
+
+    Matrix3x3 m {{
+        1, 2, 3,
+        4, 5, 6,
+        7, 8, 9 }};
+
+    EXPECT_EQ(m, Matrix3x3::fromColumns(
+            Vec3(1, 4, 7),
+            Vec3(2, 5, 8),
+            Vec3(3, 6, 9)));
+
+    EXPECT_EQ(m, Matrix3x3::fromRows(
+            Vec3(1, 2, 3),
+            Vec3(4, 5, 6),
+            Vec3(7, 8, 9)));
+}
+
+TYPED_TEST(Matrix3x3Test, transpose) {
+
+    using Matrix3x3 = typename Matrix3x3Test<TypeParam>::Matrix3x3;
+
+    Matrix3x3 m {{
+        1, 2, 3,
+        4, 5, 6,
+        7, 8, 9 }};
+
+    EXPECT_EQ(Matrix3x3({
+        1, 4, 7,
+        2, 5, 8,
+        3, 6, 9}),
+        m.transposed());
+
+    m.transpose();
+
+    EXPECT_EQ(Matrix3x3({
+        1, 4, 7,
+        2, 5, 8,
+        3, 6, 9}),
+        m);
+}
+
+TYPED_TEST(Matrix3x3Test, det) {
+
+    using Matrix3x3 = typename Matrix3x3Test<TypeParam>::Matrix3x3;
+
+    ASSERT_NEAR(1.0, Matrix3x3::identity.det(), 1e-8);
+    ASSERT_NEAR(0.0, Matrix3x3::zero.det(), 1e-8);
+
+    {
+        Matrix3x3 m {{ 1, 2, 3, 4, 5, 6, 7, 8, 9 }};
+        ASSERT_NEAR(0.0, m.det(), 1e-8);
+    }
+    {
+        Matrix3x3 m {{
+            0.540963817892, 0.63815313458, 0.232901321176,
+            0.427852547119, 0.290360892496, 0.790216925959,
+            0.521293721788, 0.126183253064, 0.878791594258 }};
+        ASSERT_NEAR(0.0843528547941, m.det(), 1e-8);
+    }
+    {
+        Matrix3x3 m {{
+            0.55770108832, 0.698229653416, 0.213133500513,
+            0.554922251751, 0.110301921695, 0.961776097631,
+            0.324829998436, 0.473104184394, 0.475331624799 }};
+        ASSERT_NEAR(-0.142243235281, m.det(), 1e-8);
+    }
+}
+
+TYPED_TEST(Matrix3x3Test, invert_inverted) {
+
+    using Matrix3x3 = typename Matrix3x3Test<TypeParam>::Matrix3x3;
+
+    EXPECT_EQ(Matrix3x3::identity, Matrix3x3::identity.inverse());
+    {
+        Matrix3x3 m {{
+            0.828277802518, 0.265835425799, 0.764172058766,
+            0.460819591568, 0.0582725933838, 0.682623689065,
+            0.00010136137219, 0.549667442228, 0.71426378955 }};
+        EXPECT_TRUE(areClose({{
+            1.95965938707, -1.35207106222, -0.804410387772,
+            1.93312797956, -3.47488258913, 1.25275115042,
+            -1.48793227563, 2.67431570876, 0.436092407449 }}, m.inverse()));
+        Matrix3x3 minv = m; minv.invert();
+        EXPECT_EQ(m.inverse(), minv);
+        EXPECT_TRUE(areClose(minv.inverse(), m));
+    }
+    {
+        Matrix3x3 m {{
+            0.177907401463, 0.801552028587, 0.537416435716,
+            0.998303071804, 0.3525500305, 0.779702329831,
+            0.00697248858758, 0.927937880557, 0.917278319035 }};
+        EXPECT_TRUE(areClose({{
+            1.3148934724, 0.777368523682, -1.43114841493,
+            2.9913570641, -0.523959200394, -1.30720656662,
+            -3.03611407354, 0.524139060932, 2.42345764719 }}, m.inverse()));
+        Matrix3x3 minv = m; minv.invert();
+        EXPECT_EQ(m.inverse(), minv);
+        EXPECT_TRUE(areClose(minv.inverse(), m));
+    }
+}
+
+TYPED_TEST(Matrix3x3Test, matrix_multiplication) {
+    using Matrix3x3 = typename Matrix3x3Test<TypeParam>::Matrix3x3;
+
+    EXPECT_EQ(Matrix3x3::identity, Matrix3x3::identity * Matrix3x3::identity);
+    EXPECT_EQ(Matrix3x3::zero, Matrix3x3::identity * Matrix3x3::zero);
+    {
+        Matrix3x3 a {{
+            0.0198677492323, 0.335847288905, 0.903519116051,
+            0.00663887675374, 0.24203377376, 0.860915878917,
+            0.159079677011, 0.51176099262, 0.754881431552 }};
+        Matrix3x3 b {{
+            0.685605402853, 0.550061681341, 0.963595467664,
+            0.31801733629, 0.219196960585, 0.881308916676,
+            0.593421354113, 0.562284336992, 0.0822415517585 }};
+        EXPECT_EQ(a, a * Matrix3x3::identity);
+        EXPECT_EQ(Matrix3x3::zero, a * Matrix3x3::zero);
+        EXPECT_EQ(Matrix3x3::zero, Matrix3x3::zero * a);
+        EXPECT_TRUE(areClose({{
+            0.656594233747, 0.592579839624, 0.389436497614,
+            0.592408452439, 0.540784373459, 0.290506772318,
+            0.719777515039, 0.62413809398, 0.666390602093 }}, a * b));
+        auto ab = a; ab *= b; EXPECT_EQ(ab, a*b);
+    }
+    {
+        Matrix3x3 a {{
+            -0.69449806209, 0.782467649048, -0.665216245975,
+            -0.285290453681, 0.395547604054, 0.635321636096,
+            -0.463882724822, 0.197102669096, -0.972470374827 }};
+        Matrix3x3 b {{
+            0.510414656025, 0.738189497916, 0.9901980894,
+            -0.06427439106, 0.706676712526, 0.550975704042,
+            -0.468791241495, -0.70849622459, 0.313163366829 }};
+        EXPECT_TRUE(areClose({{
+            -0.0929270713252, 0.511583688938, -0.464891349608,
+            -0.468873228703, -0.381197116857, 0.134402520046,
+            0.206444398874, 0.485846099589, -0.655279102673 }}, a * b));
+        auto ab = a; ab *= b; EXPECT_EQ(ab, a*b);
+    }
+    {
+        Matrix3x3 a {{
+            -4.33305043354, -2.68416159097, 6.12099479706,
+            -2.56865371689, 2.45552630644, -3.71903598774,
+            -1.46747079278, 4.91597319165, 1.04193027614 }};
+        Matrix3x3 b {{
+            9.52441107801, -0.222280392529, 8.43656371755,
+            9.3234433143, -1.272702334, -2.20088570156,
+            -2.76499303176, -4.57362467379, 3.2829486774 }};
+        EXPECT_TRUE(areClose({{
+            -83.2198899519, -23.6158419591, -10.5536114343,
+            8.71215499908, 14.4552820513, -39.2843477657,
+            28.9760723585, -10.6957785904, -19.7793023317 }}, a * b));
+        auto ab = a; ab *= b; EXPECT_EQ(ab, a*b);
+    }
+}
+
+TYPED_TEST(Matrix3x3Test, vector_multiplication) {
+    using Matrix3x3 = typename Matrix3x3Test<TypeParam>::Matrix3x3;
+    using Vec3 = typename Matrix3x3::Vec3;
+
+    {
+        const Matrix3x3 m {{
+            0.637256867597, 0.496729074271, 0.875025689854,
+            -0.0790360016434, -0.372010734739, -0.900793564733,
+            -0.150797220981, -0.679495918006, -0.995588120053 }};
+        const auto v = Vec3(0.0128269540114,-0.406921319051,0.42309802041);
+        EXPECT_TRUE(areClose(Vec3(0.176266051606,-0.230758666314,-0.146664256512), m * v));
+        EXPECT_EQ(m * v, v * m);
+
+        EXPECT_EQ(v, Matrix3x3::identity * v);
+        EXPECT_EQ(v, v * Matrix3x3::identity);
+        EXPECT_EQ(Vec3(0), Matrix3x3::zero * v);
+        EXPECT_EQ(Vec3(0), v * Matrix3x3::zero);
+    }
+}
+
+} /* anonymous namespace */

ACG/tests/Math/MatrixTestHelper.hh

+/*
+ * MatrixTestHelper.hh
+ *
+ *  Created on: Oct 5, 2016
+ *      Author: ebke
+ */
+
+#ifndef ACG_TESTS_MATH_MATRIXTESTHELPER_HH_
+#define ACG_TESTS_MATH_MATRIXTESTHELPER_HH_
+
+#include <ACG/Math/Matrix3x3T.hh>
+#include <gtest/gtest.h>
+#include <cmath>
+
+template<typename Scalar>
+::testing::AssertionResult areClose(OpenMesh::VectorT<Scalar, 3> a,
+        OpenMesh::VectorT<Scalar, 3> b, double threshold = 1e-8) {
+
+  if ((a-b).sqrnorm() > threshold) {
+      return ::testing::AssertionFailure()
+      << "ACG::Vec3d(" << a << ") and ACG::Vec3d(" << b << ") have distance "
+      << (a-b).norm() << ". Threshold: " << std::sqrt(threshold);
+  } else {
+    return ::testing::AssertionSuccess();
+  }
+}
+
+template<typename Scalar>
+::testing::AssertionResult areClose(ACG::Matrix3x3T<Scalar> a, ACG::Matrix3x3T<Scalar> b, double threshold = 1e-8) {
+  if ((a-b).frobeniusSquared() > threshold) {
+      return ::testing::AssertionFailure()
+      << "ACG::Matrix3x3T(" << a << ") and ACG::Matrix3x3T(" << b << ") have frobenius distance "
+      << (a-b).frobenius() << ". Threshold: " << std::sqrt(threshold);
+  } else {
+    return ::testing::AssertionSuccess();
+  }
+}
+
+#endif /* ACG_TESTS_MATH_MATRIXTESTHELPER_HH_ */