libcruft-util/matrix.cpp

461 lines
16 KiB
C++
Raw Normal View History

/*
* This file is part of libgim.
*
* libgim is free software: you can redistribute it and/or modify it under the
* terms of the GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option) any later
* version.
2014-08-18 22:14:31 +10:00
*
* libgim is distributed in the hope that it will be useful, but WITHOUT ANY
* WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
* FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
* details.
2014-08-18 22:14:31 +10:00
*
* You should have received a copy of the GNU General Public License
* along with libgim. If not, see <http://www.gnu.org/licenses/>.
*
2014-07-07 15:17:45 +10:00
* Copyright 2011-2014 Danny Robson <danny@nerdcruft.net>
*/
2011-05-23 17:18:52 +10:00
#include "matrix.hpp"
#include "debug.hpp"
#include <cstring>
2011-05-23 17:18:52 +10:00
2011-08-15 20:11:42 +10:00
using namespace util;
2011-05-23 17:18:52 +10:00
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
template <typename T>
2011-05-23 17:18:52 +10:00
void
2014-07-07 15:17:45 +10:00
matrix<T>::scale (T x, T y, T z) {
2012-05-11 12:34:21 +10:00
CHECK_HARD (is_affine ());
values[0][0] *= x;
values[1][1] *= y;
values[2][2] *= z;
2011-05-23 17:18:52 +10:00
}
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
template <typename T>
void
2014-07-07 15:17:45 +10:00
matrix<T>::translate (T x, T y, T z) {
2012-05-11 12:34:21 +10:00
CHECK_HARD (is_affine ());
values[0][3] += x;
values[1][3] += y;
values[2][3] += z;
2011-05-23 17:18:52 +10:00
}
2014-12-15 13:42:44 +11:00
//-----------------------------------------------------------------------------
template <typename T>
matrix<T>
matrix<T>::transposed (void) const
{
matrix<T> m;
for (size_t i = 0; i < 4; ++i)
for (size_t j = 0; j < 4; ++j)
m.values[i][j] = values[j][i];
return m;
}
//-----------------------------------------------------------------------------
template <typename T>
matrix<T>&
matrix<T>::transpose (void)
{
for (size_t i = 0; i < 4; ++i)
for (size_t j = i + 1; j < 4; ++j)
std::swap (values[i][j], values[j][i]);
return *this;
}
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
template <typename T>
matrix<T>
matrix<T>::inverse (void) const {
2014-08-19 20:45:28 +10:00
matrix<T> m;
T d = det ();
if (almost_zero (d))
throw std::runtime_error ("non-singular matrix");
auto v = values;
m.values[0][0] = v[1][2] * v[2][3] * v[3][1] -
v[1][3] * v[2][2] * v[3][1] +
v[1][3] * v[2][1] * v[3][2] -
v[1][1] * v[2][3] * v[3][2] -
v[1][2] * v[2][1] * v[3][3] +
v[1][1] * v[2][2] * v[3][3];
m.values[0][1] = v[0][3] * v[2][2] * v[3][1] -
v[0][2] * v[2][3] * v[3][1] -
v[0][3] * v[2][1] * v[3][2] +
v[0][1] * v[2][3] * v[3][2] +
v[0][2] * v[2][1] * v[3][3] -
v[0][1] * v[2][2] * v[3][3];
m.values[0][2] = v[0][2] * v[1][3] * v[3][1] -
v[0][3] * v[1][2] * v[3][1] +
v[0][3] * v[1][1] * v[3][2] -
v[0][1] * v[1][3] * v[3][2] -
v[0][2] * v[1][1] * v[3][3] +
v[0][1] * v[1][2] * v[3][3];
m.values[0][3] = v[0][3] * v[1][2] * v[2][1] -
v[0][2] * v[1][3] * v[2][1] -
v[0][3] * v[1][1] * v[2][2] +
v[0][1] * v[1][3] * v[2][2] +
v[0][2] * v[1][1] * v[2][3] -
v[0][1] * v[1][2] * v[2][3];
m.values[1][0] = v[1][3] * v[2][2] * v[3][0] -
v[1][2] * v[2][3] * v[3][0] -
v[1][3] * v[2][0] * v[3][2] +
v[1][0] * v[2][3] * v[3][2] +
v[1][2] * v[2][0] * v[3][3] -
v[1][0] * v[2][2] * v[3][3];
m.values[1][1] = v[0][2] * v[2][3] * v[3][0] -
v[0][3] * v[2][2] * v[3][0] +
v[0][3] * v[2][0] * v[3][2] -
v[0][0] * v[2][3] * v[3][2] -
v[0][2] * v[2][0] * v[3][3] +
v[0][0] * v[2][2] * v[3][3];
m.values[1][2] = v[0][3] * v[1][2] * v[3][0] -
v[0][2] * v[1][3] * v[3][0] -
v[0][3] * v[1][0] * v[3][2] +
v[0][0] * v[1][3] * v[3][2] +
v[0][2] * v[1][0] * v[3][3] -
v[0][0] * v[1][2] * v[3][3];
m.values[1][3] = v[0][2] * v[1][3] * v[2][0] -
v[0][3] * v[1][2] * v[2][0] +
v[0][3] * v[1][0] * v[2][2] -
v[0][0] * v[1][3] * v[2][2] -
v[0][2] * v[1][0] * v[2][3] +
v[0][0] * v[1][2] * v[2][3];
m.values[2][0] = v[1][1] * v[2][3] * v[3][0] -
v[1][3] * v[2][1] * v[3][0] +
v[1][3] * v[2][0] * v[3][1] -
v[1][0] * v[2][3] * v[3][1] -
v[1][1] * v[2][0] * v[3][3] +
v[1][0] * v[2][1] * v[3][3];
m.values[2][1] = v[0][3] * v[2][1] * v[3][0] -
v[0][1] * v[2][3] * v[3][0] -
v[0][3] * v[2][0] * v[3][1] +
v[0][0] * v[2][3] * v[3][1] +
v[0][1] * v[2][0] * v[3][3] -
v[0][0] * v[2][1] * v[3][3];
m.values[2][2] = v[0][1] * v[1][3] * v[3][0] -
v[0][3] * v[1][1] * v[3][0] +
v[0][3] * v[1][0] * v[3][1] -
v[0][0] * v[1][3] * v[3][1] -
v[0][1] * v[1][0] * v[3][3] +
v[0][0] * v[1][1] * v[3][3];
m.values[2][3] = v[0][3] * v[1][1] * v[2][0] -
v[0][1] * v[1][3] * v[2][0] -
v[0][3] * v[1][0] * v[2][1] +
v[0][0] * v[1][3] * v[2][1] +
v[0][1] * v[1][0] * v[2][3] -
v[0][0] * v[1][1] * v[2][3];
m.values[3][0] = v[1][2] * v[2][1] * v[3][0] -
v[1][1] * v[2][2] * v[3][0] -
v[1][2] * v[2][0] * v[3][1] +
v[1][0] * v[2][2] * v[3][1] +
v[1][1] * v[2][0] * v[3][2] -
v[1][0] * v[2][1] * v[3][2];
m.values[3][1] = v[0][1] * v[2][2] * v[3][0] -
v[0][2] * v[2][1] * v[3][0] +
v[0][2] * v[2][0] * v[3][1] -
v[0][0] * v[2][2] * v[3][1] -
v[0][1] * v[2][0] * v[3][2] +
v[0][0] * v[2][1] * v[3][2];
m.values[3][2] = v[0][2] * v[1][1] * v[3][0] -
v[0][1] * v[1][2] * v[3][0] -
v[0][2] * v[1][0] * v[3][1] +
v[0][0] * v[1][2] * v[3][1] +
v[0][1] * v[1][0] * v[3][2] -
v[0][0] * v[1][1] * v[3][2];
m.values[3][3] = v[0][1] * v[1][2] * v[2][0] -
v[0][2] * v[1][1] * v[2][0] +
v[0][2] * v[1][0] * v[2][1] -
v[0][0] * v[1][2] * v[2][1] -
v[0][1] * v[1][0] * v[2][2] +
v[0][0] * v[1][1] * v[2][2];
m /= d;
return m;
2011-05-23 17:18:52 +10:00
}
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
template <typename T>
matrix<T>&
matrix<T>::invert (void) {
2014-08-19 20:45:28 +10:00
auto m = *this;
m.invert ();
*this = m;
return *this;
}
//-----------------------------------------------------------------------------
template <typename T>
matrix<T>
matrix<T>::inverse_affine (void) const {
return matrix<T>(*this).invert_affine ();
}
//-----------------------------------------------------------------------------
template <typename T>
matrix<T>&
matrix<T>::invert_affine (void) {
2012-05-11 12:34:21 +10:00
CHECK_HARD (is_affine ());
// inv ([ M b ] == [ inv(M) -inv(M).b ]
2014-08-19 20:45:28 +10:00
// [ 0 1 ]) [ 0 1 ]
// Invert the 3x3 M
2014-07-07 15:17:45 +10:00
T A = (values[1][1] * values[2][2] - values[1][2] * values[2][1]);
T B = (values[1][2] * values[2][0] - values[1][0] * values[2][2]);
T C = (values[1][0] * values[2][1] - values[1][1] * values[2][0]);
2014-07-07 15:17:45 +10:00
T D = (values[0][2] * values[2][1] - values[0][1] * values[2][2]);
T E = (values[0][0] * values[2][2] - values[0][2] * values[2][0]);
T F = (values[2][0] * values[0][1] - values[0][0] * values[2][1]);
2014-07-07 15:17:45 +10:00
T G = (values[0][1] * values[1][2] - values[0][2] * values[1][1]);
T H = (values[0][2] * values[1][0] - values[0][0] * values[1][2]);
T K = (values[0][0] * values[1][1] - values[0][1] * values[1][0]);
2014-08-19 20:45:28 +10:00
T d = values[0][0] * A + values[0][1] * B + values[0][2] * C;
CHECK_NEQ (d, 0.0);
2014-08-19 20:45:28 +10:00
values[0][0] = A / d;
values[0][1] = D / d;
values[0][2] = G / d;
values[1][0] = B / d;
values[1][1] = E / d;
values[1][2] = H / d;
values[2][0] = C / d;
values[2][1] = F / d;
values[2][2] = K / d;
// Multiply the b
2014-07-07 15:17:45 +10:00
T b0 = - values[0][0] * values[0][3] - values[0][1] * values[1][3] - values[0][2] * values[2][3];
T b1 = - values[1][0] * values[0][3] - values[1][1] * values[1][3] - values[1][2] * values[2][3];
T b2 = - values[2][0] * values[0][3] - values[2][1] * values[1][3] - values[2][2] * values[2][3];
values[0][3] = b0;
values[1][3] = b1;
values[2][3] = b2;
2011-05-23 17:18:52 +10:00
return *this;
}
2014-08-19 20:45:28 +10:00
//-----------------------------------------------------------------------------
template <typename T>
T
matrix<T>::det (void) const {
return values[0][3] * values[1][2] * values[2][1] * values[3][0] -
values[0][2] * values[1][3] * values[2][1] * values[3][0] -
values[0][3] * values[1][1] * values[2][2] * values[3][0] +
values[0][1] * values[1][3] * values[2][2] * values[3][0] +
values[0][2] * values[1][1] * values[2][3] * values[3][0] -
values[0][1] * values[1][2] * values[2][3] * values[3][0] -
values[0][3] * values[1][2] * values[2][0] * values[3][1] +
values[0][2] * values[1][3] * values[2][0] * values[3][1] +
values[0][3] * values[1][0] * values[2][2] * values[3][1] -
values[0][0] * values[1][3] * values[2][2] * values[3][1] -
values[0][2] * values[1][0] * values[2][3] * values[3][1] +
values[0][0] * values[1][2] * values[2][3] * values[3][1] +
values[0][3] * values[1][1] * values[2][0] * values[3][2] -
values[0][1] * values[1][3] * values[2][0] * values[3][2] -
values[0][3] * values[1][0] * values[2][1] * values[3][2] +
values[0][0] * values[1][3] * values[2][1] * values[3][2] +
values[0][1] * values[1][0] * values[2][3] * values[3][2] -
values[0][0] * values[1][1] * values[2][3] * values[3][2] -
values[0][2] * values[1][1] * values[2][0] * values[3][3] +
values[0][1] * values[1][2] * values[2][0] * values[3][3] +
values[0][2] * values[1][0] * values[2][1] * values[3][3] -
values[0][0] * values[1][2] * values[2][1] * values[3][3] -
values[0][1] * values[1][0] * values[2][2] * values[3][3] +
values[0][0] * values[1][1] * values[2][2] * values[3][3];
}
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
template <typename T>
matrix<T>
matrix<T>::operator* (const matrix<T> &rhs) const {
matrix<T> m;
memset (m.values, 0, sizeof (m.values));
2011-05-23 17:18:52 +10:00
for (unsigned i = 0; i < 4; ++i)
for (unsigned j = 0; j < 4; ++j)
for (unsigned k = 0; k < 4; ++k)
m.values[i][j] += values[i][k] * rhs.values[k][j];
2011-05-23 17:18:52 +10:00
return m;
2011-05-23 17:18:52 +10:00
}
2014-08-19 20:46:00 +10:00
//-----------------------------------------------------------------------------
template <typename T>
vector<4>
matrix<T>::operator* (const vector<4> &rhs) const {
return vector<4> {
values[0][0] * rhs.x + values[0][1] * rhs.y + values[0][2] * rhs.z + values[0][3] * rhs.w,
values[1][0] * rhs.x + values[1][1] * rhs.y + values[1][2] * rhs.z + values[1][3] * rhs.w,
values[2][0] * rhs.x + values[2][1] * rhs.y + values[2][2] * rhs.z + values[2][3] * rhs.w,
values[3][0] * rhs.x + values[3][1] * rhs.y + values[3][2] * rhs.z + values[3][3] * rhs.w
};
}
//-----------------------------------------------------------------------------
template <typename T>
matrix<T>
matrix<T>::operator/ (T s) const {
matrix<T> m;
for (size_t r = 0; r < m.rows; ++r)
for (size_t c = 0; c < m.cols; ++c)
m.values[r][c] = values[r][c] / s;
return m;
}
//-----------------------------------------------------------------------------
template <typename T>
matrix<T>&
matrix<T>::operator/= (T s) {
for (size_t r = 0; r < rows; ++r)
for (size_t c = 0; c < cols; ++c)
values[r][c] /= s;
return *this;
}
//-----------------------------------------------------------------------------
template <typename T>
bool
matrix<T>::operator== (const matrix<T> &rhs) const {
for (size_t r = 0; r < rows; ++r)
for (size_t c = 0; c < cols; ++c)
if (!almost_equal (rhs.values[r][c], values[r][c]))
return false;
return true;
}
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
template <typename T>
util::point<3>
2014-07-07 15:17:45 +10:00
matrix<T>::to_local (const util::point<3> &p) const {
2012-05-11 12:34:21 +10:00
CHECK_SOFT (is_affine ());
2011-05-23 17:18:52 +10:00
return { p.x * values[0][0] +
p.y * values[0][1] +
p.z * values[0][2] + values[0][3],
p.x * values[1][0] +
p.y * values[1][1] +
p.z * values[1][2] + values[1][3],
p.x * values[2][0] +
p.y * values[2][1] +
p.z * values[2][2] + values[2][3] };
2011-05-23 17:18:52 +10:00
}
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
template <typename T>
util::point<3>
2014-07-07 15:17:45 +10:00
matrix<T>::to_global (const util::point<3> &p) const {
2011-10-24 10:26:17 +11:00
return inverse ().to_local (p);
2011-05-23 17:18:52 +10:00
}
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
template <typename T>
2011-05-23 17:18:52 +10:00
bool
2014-07-07 15:17:45 +10:00
matrix<T>::is_affine (void) const {
2014-08-19 20:45:28 +10:00
return exactly_equal (values[3][0], T {0}) &&
exactly_equal (values[3][1], T {0}) &&
exactly_equal (values[3][2], T {0}) &&
exactly_equal (values[3][3], T {1});
2011-05-23 17:18:52 +10:00
}
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
template <typename T>
const matrix<T>
matrix<T>::IDENTITY = { { { 1, 0, 0, 0 },
{ 0, 1, 0, 0 },
{ 0, 0, 1, 0 },
{ 0, 0, 0, 1 } } };
template <typename T>
const matrix<T>
matrix<T>::ZEROES = { { { 0, 0, 0, 0 },
{ 0, 0, 0, 0 },
{ 0, 0, 0, 0 },
{ 0, 0, 0, 0 } } };
2011-05-23 17:18:52 +10:00
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
namespace util {
template struct matrix<float>;
template struct matrix<double>;
}
2011-05-23 17:18:52 +10:00
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
namespace util {
template <typename T>
std::ostream&
operator<< (std::ostream &os, const matrix<T> &m) {
os << "{ {" << m.values[0][0] << ", "
<< m.values[0][1] << ", "
<< m.values[0][2] << ", "
<< m.values[0][3] << "}, "
<< "{" << m.values[1][0] << ", "
<< m.values[1][1] << ", "
<< m.values[1][2] << ", "
<< m.values[1][3] << "}, "
<< "{" << m.values[2][0] << ", "
<< m.values[2][1] << ", "
<< m.values[2][2] << ", "
<< m.values[2][3] << "}, "
<< "{" << m.values[3][0] << ", "
<< m.values[3][1] << ", "
<< m.values[3][2] << ", "
<< m.values[3][3] << "} }";
return os;
}
2011-05-23 17:18:52 +10:00
}
template std::ostream& util::operator<< (std::ostream&, const matrix<float>&);
template std::ostream& util::operator<< (std::ostream&, const matrix<double>&);