Danny Robson
1fc1dd4134
Contains is more descriptive (of the actual implementation) as it implies the entire argument is within the range. Includes should also be provided at some point.
533 lines
14 KiB
C++
533 lines
14 KiB
C++
/*
|
|
* 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.
|
|
*
|
|
* 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.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with libgim. If not, see <http://www.gnu.org/licenses/>.
|
|
*
|
|
* Copyright 2010 Danny Robson <danny@blubinc.net>
|
|
*/
|
|
|
|
#include "matrix.hpp"
|
|
|
|
#include "debug.hpp"
|
|
#include "range.hpp"
|
|
#include "maths.hpp"
|
|
|
|
#include <algorithm>
|
|
|
|
using namespace maths;
|
|
|
|
|
|
matrix::matrix (size_t _rows, size_t _columns):
|
|
m_rows (_rows),
|
|
m_columns (_columns),
|
|
m_data (NULL) {
|
|
if (m_rows <= 0 || m_columns <= 0)
|
|
throw std::runtime_error ("rows and columns must be positive");
|
|
|
|
m_data = new double[size ()];
|
|
}
|
|
|
|
|
|
matrix::matrix (size_t _rows,
|
|
size_t _columns,
|
|
const std::initializer_list <double> &_data):
|
|
m_rows (_rows),
|
|
m_columns (_columns),
|
|
m_data (NULL)
|
|
{
|
|
if (m_rows <= 0 || m_columns <= 0)
|
|
throw std::runtime_error ("rows and columns must be positive");
|
|
if (size () != _data.size ())
|
|
throw std::runtime_error ("element and initializer size differs");
|
|
check_hard (m_rows * m_columns == _data.size());
|
|
|
|
m_data = new double[size ()];
|
|
std::copy (_data.begin (), _data.end (), m_data);
|
|
}
|
|
|
|
|
|
matrix::matrix (const std::initializer_list <vector> &rhs):
|
|
m_rows (rhs.size ()),
|
|
m_columns (rhs.begin()->size ()),
|
|
m_data (new double[m_rows * m_columns])
|
|
{
|
|
double *row_cursor = m_data;
|
|
|
|
for (auto i = rhs.begin (); i != rhs.end (); ++i) {
|
|
check (i->size () == m_columns);
|
|
|
|
std::copy (i->data (), i->data () + i->size (), row_cursor);
|
|
row_cursor += m_columns;
|
|
}
|
|
}
|
|
|
|
|
|
matrix::matrix (const matrix &rhs):
|
|
m_rows (rhs.m_rows),
|
|
m_columns (rhs.m_columns) {
|
|
m_data = new double [m_rows * m_columns];
|
|
std::copy (rhs.m_data, rhs.m_data + m_rows * m_columns, m_data);
|
|
}
|
|
|
|
|
|
matrix::matrix (matrix &&rhs):
|
|
m_rows (rhs.m_rows),
|
|
m_columns (rhs.m_columns),
|
|
m_data (rhs.m_data) {
|
|
rhs.m_data = NULL;
|
|
}
|
|
|
|
|
|
matrix::~matrix()
|
|
{ delete [] m_data; }
|
|
|
|
|
|
void
|
|
matrix::sanity (void) const {
|
|
check (m_rows > 0);
|
|
check (m_columns > 0);
|
|
check (m_data != NULL);
|
|
}
|
|
|
|
|
|
const double *
|
|
matrix::operator [] (unsigned int row) const {
|
|
check_hard (row < m_rows);
|
|
return m_data + row * m_columns;
|
|
}
|
|
|
|
|
|
double *
|
|
matrix::operator [] (unsigned int row) {
|
|
check_hard (row < m_rows);
|
|
return m_data + row * m_columns;
|
|
}
|
|
|
|
|
|
const double *
|
|
matrix::data (void) const
|
|
{ return m_data; }
|
|
|
|
|
|
matrix&
|
|
matrix::operator =(const matrix& rhs) {
|
|
if (size () != rhs.size ()) {
|
|
delete [] m_data;
|
|
m_data = new double [m_rows * m_columns];
|
|
}
|
|
|
|
m_rows = rhs.m_rows;
|
|
m_columns = rhs.m_columns;
|
|
std::copy (rhs.m_data, rhs.m_data + m_rows * m_columns, m_data);
|
|
|
|
return *this;
|
|
}
|
|
|
|
|
|
matrix
|
|
matrix::operator * (double scalar) const {
|
|
matrix val (*this);
|
|
|
|
for (unsigned int i = 0; i < m_rows; ++i)
|
|
for (unsigned int j = 0; j < m_columns; ++j)
|
|
val[i][j] *= scalar;
|
|
|
|
return val;
|
|
}
|
|
|
|
|
|
matrix&
|
|
matrix::operator *=(double scalar) {
|
|
for (unsigned int i = 0; i < m_rows; ++i)
|
|
for (unsigned int j = 0; j < m_columns; ++j)
|
|
(*this)[i][j] *= scalar;
|
|
|
|
return *this;
|
|
}
|
|
|
|
|
|
matrix&
|
|
matrix::operator /= (double scalar)
|
|
{ return (*this) *= (1.0 / scalar); }
|
|
|
|
|
|
matrix
|
|
matrix::operator + (double scalar) const {
|
|
matrix val (*this);
|
|
for (unsigned int i = 0; i < m_rows; ++i)
|
|
for (unsigned int j = 0; j < m_columns; ++j)
|
|
val[i][j] += scalar;
|
|
|
|
return val;
|
|
}
|
|
|
|
|
|
matrix&
|
|
matrix::operator +=(double scalar) {
|
|
for (unsigned int i = 0; i < m_rows; ++i)
|
|
for (unsigned int j = 0; j < m_columns; ++j)
|
|
(*this)[i][j] += scalar;
|
|
|
|
return *this;
|
|
}
|
|
|
|
|
|
matrix
|
|
matrix::operator * (const matrix& rhs) const {
|
|
if (m_columns != rhs.rows ())
|
|
throw std::invalid_argument ("matrices size mismatch in multiplication");
|
|
|
|
matrix val (matrix::zeroes (m_rows, rhs.columns ()));
|
|
|
|
for (unsigned int i = 0; i < m_rows; ++i)
|
|
for (unsigned int j = 0; j < rhs.columns (); ++j)
|
|
for (unsigned int k = 0; k < m_columns; ++k)
|
|
val[i][j] += (*this)[i][k] * rhs[k][j];
|
|
|
|
return val;
|
|
}
|
|
|
|
|
|
matrix&
|
|
matrix::operator *=(const matrix& rhs)
|
|
{ return *this = *this * rhs; }
|
|
|
|
|
|
bool
|
|
matrix::operator ==(const matrix& rhs) const {
|
|
if (rhs.rows () != rows () ||
|
|
rhs.columns () != columns ())
|
|
return false;
|
|
|
|
return std::equal (m_data, m_data + size (), rhs.data ());
|
|
}
|
|
|
|
|
|
//matrix transpose (void) const { ; }
|
|
|
|
|
|
size_t
|
|
matrix::rows (void) const
|
|
{ return m_rows; }
|
|
|
|
|
|
size_t
|
|
matrix::columns (void) const
|
|
{ return m_columns; }
|
|
|
|
|
|
size_t
|
|
matrix::size (void) const
|
|
{ return rows () * columns (); }
|
|
|
|
|
|
bool
|
|
matrix::is_square (void) const
|
|
{ return m_rows == m_columns; }
|
|
|
|
|
|
bool
|
|
matrix::is_magic (void) const {
|
|
if (!is_square ())
|
|
return false;
|
|
|
|
unsigned int expected = m_rows * (m_rows * m_rows + 1) / 2;
|
|
range<double> numbers (1, m_rows * m_rows);
|
|
|
|
for (unsigned int i = 0; i < m_rows; ++i) {
|
|
unsigned int sum1 = 0, sum2 = 0;
|
|
|
|
for (unsigned int j = 0; j < m_columns; ++j) {
|
|
if (!numbers.contains ((*this)[i][j]) ||
|
|
!numbers.contains ((*this)[j][i]))
|
|
return false;
|
|
|
|
sum1 += (*this)[i][j];
|
|
sum2 += (*this)[j][i];
|
|
}
|
|
|
|
if (sum1 != expected || sum2 != expected)
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool
|
|
matrix::is_homogeneous (void) const {
|
|
if (m_rows != m_columns)
|
|
return false;
|
|
|
|
// Check the final row is all zeroes
|
|
for (unsigned int i = 0; i < m_columns - 1; ++i) {
|
|
if (!almost_equal ((*this)[m_rows - 1][i], 0.))
|
|
return false;
|
|
}
|
|
|
|
// Except for the last element, which has to be one
|
|
return almost_equal ((*this)[m_rows - 1][m_columns - 1], 1.);
|
|
}
|
|
|
|
|
|
double
|
|
matrix::determinant (void) const {
|
|
if (m_rows != m_columns)
|
|
not_implemented ();
|
|
|
|
switch (m_rows) {
|
|
case 2: return determinant2x2 ();
|
|
case 3: return determinant3x3 ();
|
|
case 4: return determinant4x4 ();
|
|
}
|
|
|
|
not_implemented ();
|
|
}
|
|
|
|
|
|
// With matrix A = [ a, b ]
|
|
// [ c, d ]
|
|
//
|
|
// det (A) = ad - bc
|
|
|
|
double
|
|
matrix::determinant2x2 (void) const {
|
|
check_eq (m_rows, 2);
|
|
check_eq (m_columns, 2);
|
|
|
|
return (*this)[0][0] * (*this)[1][1] -
|
|
(*this)[0][1] * (*this)[1][0];
|
|
}
|
|
|
|
|
|
|
|
// [ a, b, c ]
|
|
// Given matrix A = [ d, e, f ]
|
|
// [ g, h, i ]
|
|
//
|
|
// det (A) = aei + bfg + cdh - afg - bdi - ceg
|
|
// det (A) = a(ei - fg) + b(fg - di) + c(dh - eg)
|
|
double
|
|
matrix::determinant3x3 (void) const {
|
|
check_eq (m_rows, 3);
|
|
check_eq (m_columns, 3);
|
|
|
|
return (*this)[0][0] * (*this)[1][1] * (*this)[2][2] + // aei
|
|
(*this)[0][1] * (*this)[1][2] * (*this)[2][0] + // bfg
|
|
(*this)[0][2] * (*this)[1][0] * (*this)[2][1] - // cdh
|
|
(*this)[0][0] * (*this)[1][2] * (*this)[2][1] - // afh
|
|
(*this)[0][1] * (*this)[1][0] * (*this)[2][2] - // bdi
|
|
(*this)[0][2] * (*this)[1][1] * (*this)[2][0]; // ceg
|
|
}
|
|
|
|
|
|
// From libMathematics, http://www.geometrictools.com/
|
|
double
|
|
matrix::determinant4x4 (void) const {
|
|
check_eq (m_rows, 4);
|
|
check_eq (m_columns, 4);
|
|
|
|
double a0 = m_data[ 0] * m_data[ 5] - m_data[ 1] * m_data[ 4],
|
|
a1 = m_data[ 0] * m_data[ 6] - m_data[ 2] * m_data[ 4],
|
|
a2 = m_data[ 0] * m_data[ 7] - m_data[ 3] * m_data[ 4],
|
|
a3 = m_data[ 1] * m_data[ 6] - m_data[ 2] * m_data[ 5],
|
|
a4 = m_data[ 1] * m_data[ 7] - m_data[ 3] * m_data[ 5],
|
|
a5 = m_data[ 2] * m_data[ 7] - m_data[ 3] * m_data[ 6],
|
|
b0 = m_data[ 8] * m_data[13] - m_data[ 9] * m_data[12],
|
|
b1 = m_data[ 8] * m_data[14] - m_data[10] * m_data[12],
|
|
b2 = m_data[ 8] * m_data[15] - m_data[11] * m_data[12],
|
|
b3 = m_data[ 9] * m_data[14] - m_data[10] * m_data[13],
|
|
b4 = m_data[ 9] * m_data[15] - m_data[11] * m_data[13],
|
|
b5 = m_data[10] * m_data[15] - m_data[11] * m_data[14];
|
|
|
|
return a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0;
|
|
}
|
|
|
|
|
|
matrix
|
|
matrix::inverse (void) const {
|
|
if (m_rows != m_columns)
|
|
not_implemented ();
|
|
|
|
switch (m_rows) {
|
|
case 2: return inverse2x2 ();
|
|
case 3: return inverse3x3 ();
|
|
case 4: return inverse4x4 ();
|
|
}
|
|
|
|
not_implemented ();
|
|
}
|
|
|
|
|
|
matrix
|
|
matrix::inverse2x2 (void) const {
|
|
check (m_rows == 2);
|
|
check (m_columns == 2);
|
|
|
|
double det = determinant2x2 ();
|
|
if (almost_equal (det, 0.))
|
|
throw not_invertible ();
|
|
return matrix (2, 2, { (*this)[1][1], -(*this)[0][1],
|
|
-(*this)[1][0], (*this)[0][0] }) /= det;
|
|
}
|
|
|
|
|
|
// [ a, b, c ]
|
|
// Given matrix A = [ d, e, f ]
|
|
// [ g, h, i ]
|
|
//
|
|
matrix
|
|
matrix::inverse3x3 (void) const {
|
|
check (m_rows == 3);
|
|
check (m_columns == 3);
|
|
|
|
double det = determinant3x3();
|
|
if (almost_equal (det, 0.))
|
|
throw not_invertible ();
|
|
|
|
matrix val (m_rows, m_columns, {
|
|
(*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1], // ei - fh
|
|
(*this)[0][2] * (*this)[2][1] - (*this)[0][1] * (*this)[2][2], // ch - bi
|
|
(*this)[0][1] * (*this)[1][2] - (*this)[0][2] * (*this)[1][1], // bf - ce
|
|
(*this)[1][2] * (*this)[2][0] - (*this)[1][0] * (*this)[2][2], // fg - di
|
|
(*this)[0][0] * (*this)[2][2] - (*this)[0][2] * (*this)[2][0], // ai - cg
|
|
(*this)[0][2] * (*this)[1][0] - (*this)[0][0] * (*this)[1][2], // cd - af
|
|
(*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0], // dh - eg
|
|
(*this)[0][1] * (*this)[2][0] - (*this)[0][0] * (*this)[2][1], // bg - ah
|
|
(*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0] // ae - bd
|
|
});
|
|
|
|
return val /= det;
|
|
|
|
//matrix val ({ vector::cross ((*this)[1], (*this)[2], 3),
|
|
// vector::cross ((*this)[2], (*this)[0], 3),
|
|
// vector::cross ((*this)[0], (*this)[1], 3) });
|
|
//return val /= determinant3x3 ();
|
|
}
|
|
|
|
|
|
matrix
|
|
matrix::inverse4x4 (void) const {
|
|
double a0 = m_data[ 0] * m_data[ 5] - m_data[ 1] * m_data[ 4],
|
|
a1 = m_data[ 0] * m_data[ 6] - m_data[ 2] * m_data[ 4],
|
|
a2 = m_data[ 0] * m_data[ 7] - m_data[ 3] * m_data[ 4],
|
|
a3 = m_data[ 1] * m_data[ 6] - m_data[ 2] * m_data[ 5],
|
|
a4 = m_data[ 1] * m_data[ 7] - m_data[ 3] * m_data[ 5],
|
|
a5 = m_data[ 2] * m_data[ 7] - m_data[ 3] * m_data[ 6],
|
|
b0 = m_data[ 8] * m_data[13] - m_data[ 9] * m_data[12],
|
|
b1 = m_data[ 8] * m_data[14] - m_data[10] * m_data[12],
|
|
b2 = m_data[ 8] * m_data[15] - m_data[11] * m_data[12],
|
|
b3 = m_data[ 9] * m_data[14] - m_data[10] * m_data[13],
|
|
b4 = m_data[ 9] * m_data[15] - m_data[11] * m_data[13],
|
|
b5 = m_data[10] * m_data[15] - m_data[11] * m_data[14];
|
|
|
|
double det = a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0;
|
|
if (almost_equal (det, 0.))
|
|
throw not_invertible ();
|
|
|
|
return matrix (4, 4, {
|
|
+ m_data[ 5] * b5 - m_data[ 6] * b4 + m_data[ 7] * b3,
|
|
- m_data[ 1] * b5 + m_data[ 2] * b4 - m_data[ 3] * b3,
|
|
+ m_data[13] * a5 - m_data[14] * a4 + m_data[15] * a3,
|
|
- m_data[ 9] * a5 + m_data[10] * a4 - m_data[11] * a3,
|
|
- m_data[ 4] * b5 + m_data[ 6] * b2 - m_data[ 7] * b1,
|
|
+ m_data[ 0] * b5 - m_data[ 2] * b2 + m_data[ 3] * b1,
|
|
- m_data[12] * a5 + m_data[14] * a2 - m_data[15] * a1,
|
|
+ m_data[ 8] * a5 - m_data[10] * a2 + m_data[11] * a1,
|
|
+ m_data[ 4] * b4 - m_data[ 5] * b2 + m_data[ 7] * b0,
|
|
- m_data[ 0] * b4 + m_data[ 1] * b2 - m_data[ 3] * b0,
|
|
+ m_data[12] * a4 - m_data[13] * a2 + m_data[15] * a0,
|
|
- m_data[ 8] * a4 + m_data[ 9] * a2 - m_data[11] * a0,
|
|
- m_data[ 4] * b3 + m_data[ 5] * b1 - m_data[ 6] * b0,
|
|
+ m_data[ 0] * b3 - m_data[ 1] * b1 + m_data[ 2] * b0,
|
|
- m_data[12] * a3 + m_data[13] * a1 - m_data[14] * a0,
|
|
+ m_data[ 8] * a3 - m_data[ 9] * a1 + m_data[10] * a0
|
|
}) /= det;
|
|
}
|
|
|
|
|
|
|
|
matrix
|
|
matrix::zeroes (size_t diag)
|
|
{ return zeroes (diag, diag); }
|
|
|
|
|
|
matrix
|
|
matrix::zeroes (size_t rows, size_t columns) {
|
|
matrix m (rows, columns);
|
|
std::fill (m.m_data, m.m_data + m.size (), 0.0);
|
|
return m;
|
|
}
|
|
|
|
|
|
matrix
|
|
matrix::identity (size_t diag) {
|
|
matrix val (zeroes (diag));
|
|
for (unsigned int i = 0; i < diag; ++i)
|
|
val[i][i] = 1.0;
|
|
return val;
|
|
}
|
|
|
|
|
|
matrix
|
|
matrix::magic (size_t n) {
|
|
check_hard (n > 2);
|
|
|
|
if (n % 2 == 1)
|
|
return magic_odd (n);
|
|
|
|
if (n % 4 == 0)
|
|
return magic_even_single (n);
|
|
|
|
return magic_even_double (n);
|
|
}
|
|
|
|
|
|
// Use the 'siamese' method. Start from the top centre, progress up-left one.
|
|
// If filled then drop down one row instead. Wrap around indexing.
|
|
matrix
|
|
matrix::magic_odd (size_t n) {
|
|
check_hard (n > 2);
|
|
check_hard (n % 2 == 1);
|
|
|
|
matrix val (zeroes (n));
|
|
for (unsigned int i = 1, x = n / 2, y = 0; i <= n * n; ++i) {
|
|
val[y][x] = i;
|
|
|
|
unsigned int x1 = (x + 1) % n,
|
|
y1 = (y + n - 1) % n;
|
|
|
|
if (!almost_equal (val[y1][x1], 0)) {
|
|
x1 = x;
|
|
y1 = (y + 1) % n;
|
|
}
|
|
|
|
x = x1;
|
|
y = y1;
|
|
}
|
|
|
|
return val;
|
|
}
|
|
|
|
|
|
matrix
|
|
matrix::magic_even_single (size_t)
|
|
{ not_implemented (); }
|
|
|
|
|
|
matrix
|
|
matrix::magic_even_double (size_t)
|
|
{ not_implemented (); }
|
|
|