Commit 23bc8952 authored by Hans-Christian Ebke's avatar Hans-Christian Ebke
Browse files

Added Matrix3x3T class. C++11 only.

parent f73479eb
#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_ */
#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 */
/*
* 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_ */
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