Make 3d matrix/vectors and general matrix/vectors
This commit is contained in:
parent
b71049df85
commit
2aee108e79
13
Makefile.am
13
Makefile.am
@ -1,4 +1,4 @@
|
||||
AUTOMAKE_OPTIONS = dist-bzip2 dist-xz foreign
|
||||
AUTOMAKE_OPTIONS = dist-bzip2 dist-xz foreign subdir-objects
|
||||
ACLOCAL_AMFLAGS = -I m4
|
||||
|
||||
AM_CXXFLAGS = $(BOOST_CPPFLAGS) $(COMMON_CXXFLAGS)
|
||||
@ -23,6 +23,8 @@ UTIL_INCLUDE = \
|
||||
ip.hpp \
|
||||
json.hpp \
|
||||
maths.hpp \
|
||||
maths/matrix.hpp \
|
||||
maths/vector.hpp \
|
||||
matrix.hpp \
|
||||
nocopy.hpp \
|
||||
point.hpp \
|
||||
@ -31,12 +33,11 @@ UTIL_INCLUDE = \
|
||||
range.hpp \
|
||||
region.hpp \
|
||||
signal.hpp \
|
||||
si.hpp \
|
||||
stream.hpp \
|
||||
string.hpp \
|
||||
si.hpp \
|
||||
time.hpp \
|
||||
types.hpp \
|
||||
vector.hpp \
|
||||
version.hpp
|
||||
|
||||
UTIL_FILES = \
|
||||
@ -52,19 +53,21 @@ UTIL_FILES = \
|
||||
ip.cpp \
|
||||
json.cpp \
|
||||
maths.cpp \
|
||||
maths/matrix.cpp \
|
||||
maths/vector.cpp \
|
||||
matrix.cpp \
|
||||
point.cpp \
|
||||
pool.cpp \
|
||||
random.cpp \
|
||||
range.cpp \
|
||||
region.cpp \
|
||||
si.cpp \
|
||||
signal.cpp \
|
||||
stream.cpp \
|
||||
string.cpp \
|
||||
si.cpp \
|
||||
time.cpp \
|
||||
types.cpp \
|
||||
vector.cpp \
|
||||
vector.cpp \
|
||||
version.cpp
|
||||
|
||||
|
||||
|
||||
532
maths/matrix.cpp
Normal file
532
maths/matrix.cpp
Normal file
@ -0,0 +1,532 @@
|
||||
/*
|
||||
* 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 util;
|
||||
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 (); }
|
||||
|
||||
119
maths/matrix.hpp
Normal file
119
maths/matrix.hpp
Normal file
@ -0,0 +1,119 @@
|
||||
/*
|
||||
* 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>
|
||||
*/
|
||||
|
||||
#ifndef __UTIL_MATHS_MATRIX_HPP
|
||||
#define __UTIL_MATHS_MATRIX_HPP
|
||||
|
||||
#include "vector.hpp"
|
||||
|
||||
#include <assert.h>
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
#include <initializer_list>
|
||||
#include <iostream>
|
||||
|
||||
namespace maths {
|
||||
class matrix {
|
||||
protected:
|
||||
size_t m_rows,
|
||||
m_columns;
|
||||
double *restrict m_data;
|
||||
|
||||
public:
|
||||
matrix (size_t _rows, size_t _columns);
|
||||
|
||||
matrix (size_t _rows,
|
||||
size_t _columns,
|
||||
const std::initializer_list <double> &_data);
|
||||
matrix (const std::initializer_list <vector> &_data);
|
||||
|
||||
matrix (const matrix &rhs);
|
||||
matrix (matrix &&rhs);
|
||||
|
||||
~matrix();
|
||||
|
||||
void sanity (void) const;
|
||||
|
||||
const double * operator [] (unsigned int row) const;
|
||||
double * operator [] (unsigned int row);
|
||||
|
||||
const double * data (void) const;
|
||||
|
||||
matrix& operator =(const matrix &rhs);
|
||||
matrix operator * (double scalar) const;
|
||||
matrix& operator *=(double scalar);
|
||||
matrix operator * (const matrix &rhs) const;
|
||||
matrix& operator *=(const matrix &rhs);
|
||||
matrix& operator /=(double scalar);
|
||||
matrix operator + (double scalar) const;
|
||||
matrix& operator +=(double scalar);
|
||||
matrix& operator -=(double scalar);
|
||||
bool operator ==(const matrix &rhs) const;
|
||||
|
||||
//matrix transpose (void) const { ; }
|
||||
|
||||
size_t rows (void) const;
|
||||
size_t columns (void) const;
|
||||
size_t size (void) const;
|
||||
|
||||
/// Checks if this is a sqaure matrix, with a zero final column
|
||||
/// and row (excepting the final diagonal entry).
|
||||
bool is_homogeneous (void) const;
|
||||
bool is_square (void) const;
|
||||
bool is_magic (void) const;
|
||||
public:
|
||||
double determinant (void) const;
|
||||
matrix inverse (void) const;
|
||||
|
||||
protected:
|
||||
double determinant2x2 (void) const;
|
||||
double determinant3x3 (void) const;
|
||||
double determinant4x4 (void) const;
|
||||
|
||||
matrix inverse2x2 (void) const;
|
||||
matrix inverse3x3 (void) const;
|
||||
matrix inverse4x4 (void) const;
|
||||
|
||||
public:
|
||||
static matrix zeroes (size_t n);
|
||||
static matrix zeroes (size_t rows, size_t columns);
|
||||
static matrix identity (size_t n);
|
||||
|
||||
/// Generate a magic square of order 'n'
|
||||
static matrix magic (size_t n);
|
||||
|
||||
protected:
|
||||
/// Generate a magic square with 'n' odd
|
||||
static matrix magic_odd (size_t n);
|
||||
/// Generate a magic square with 'n' divisible by 2, and not 4
|
||||
static matrix magic_even_single (size_t n);
|
||||
/// Generate a magic square with 'n' divisible by 4, and not 2
|
||||
static matrix magic_even_double (size_t n);
|
||||
};
|
||||
|
||||
class not_invertible : public std::runtime_error {
|
||||
public:
|
||||
not_invertible ():
|
||||
std::runtime_error ("not_invertible")
|
||||
{ ; }
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
80
maths/vector.cpp
Normal file
80
maths/vector.cpp
Normal file
@ -0,0 +1,80 @@
|
||||
#include "vector.hpp"
|
||||
|
||||
#include "debug.hpp"
|
||||
|
||||
#include <numeric>
|
||||
|
||||
|
||||
using namespace maths;
|
||||
|
||||
|
||||
/* Constructors */
|
||||
|
||||
vector::vector (const std::initializer_list<double> &_data):
|
||||
m_data (_data)
|
||||
{ ; }
|
||||
|
||||
|
||||
vector::vector (unsigned int _size):
|
||||
m_data (_size)
|
||||
{ ; }
|
||||
|
||||
|
||||
vector::vector (const double *restrict _data,
|
||||
unsigned int _size):
|
||||
m_data (_size)
|
||||
{ std::copy (_data, _data + _size, m_data.begin ()); }
|
||||
|
||||
|
||||
vector::vector (const vector &rhs):
|
||||
m_data (rhs.m_data)
|
||||
{ ; }
|
||||
|
||||
|
||||
vector::vector (const vector &&rhs):
|
||||
m_data (std::move (rhs.m_data))
|
||||
{ ; }
|
||||
|
||||
|
||||
vector::~vector (void)
|
||||
{ ; }
|
||||
|
||||
|
||||
/* element accessors */
|
||||
|
||||
const double*
|
||||
vector::data (void) const
|
||||
{ return &m_data[0]; }
|
||||
|
||||
|
||||
double &
|
||||
vector::operator[] (unsigned int offset)
|
||||
{ return m_data[offset]; }
|
||||
|
||||
|
||||
const double&
|
||||
vector::operator[] (unsigned int offset) const
|
||||
{ return m_data[offset]; }
|
||||
|
||||
|
||||
unsigned int
|
||||
vector::size (void) const
|
||||
{ return m_data.size (); }
|
||||
|
||||
|
||||
/* dot and cross products */
|
||||
|
||||
double vector::dot (const double *restrict A,
|
||||
const double *restrict B,
|
||||
unsigned int size)
|
||||
{ return std::inner_product(A, A + size, B, 0.0); }
|
||||
|
||||
|
||||
vector vector::cross (const double *restrict A,
|
||||
const double *restrict B,
|
||||
unsigned int size) {
|
||||
check_hard (size == 3);
|
||||
return vector ({ A[1] * B[2] - A[2] * B[1],
|
||||
A[2] * B[0] - A[0] * B[2],
|
||||
A[0] * B[1] - A[1] * B[0] });
|
||||
}
|
||||
61
maths/vector.hpp
Normal file
61
maths/vector.hpp
Normal file
@ -0,0 +1,61 @@
|
||||
/*
|
||||
* 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 2011 Danny Robson <danny@blubinc.net>
|
||||
*/
|
||||
|
||||
#ifndef __UTIL_MATHS_VECTOR_HPP
|
||||
#define __UTIL_MATHS_VECTOR_HPP
|
||||
|
||||
#include <vector>
|
||||
#include <initializer_list>
|
||||
|
||||
namespace maths {
|
||||
class vector {
|
||||
protected:
|
||||
std::vector<double> m_data;
|
||||
|
||||
public:
|
||||
vector (const std::initializer_list<double> &_data);
|
||||
explicit
|
||||
vector (unsigned int _size);
|
||||
vector (const double *restrict _data,
|
||||
unsigned int _size);
|
||||
|
||||
vector (const vector &rhs);
|
||||
vector (const vector &&rhs);
|
||||
|
||||
~vector (void);
|
||||
|
||||
const double* data (void) const;
|
||||
double& operator[] (unsigned int);
|
||||
const double& operator[] (unsigned int) const;
|
||||
|
||||
unsigned int size (void) const;
|
||||
|
||||
|
||||
static double dot (const double *restrict A,
|
||||
const double *restrict B,
|
||||
unsigned int size);
|
||||
|
||||
static vector cross (const double *restrict A,
|
||||
const double *restrict B,
|
||||
unsigned int size);
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
603
matrix.cpp
603
matrix.cpp
@ -1,532 +1,157 @@
|
||||
/*
|
||||
* 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>
|
||||
*/
|
||||
/****************************************************************************
|
||||
__ .__ .__ .__ .___
|
||||
___________ ___.__. _______/ |______ | | | | |__| ______ ____ __| _/
|
||||
_/ ___\_ __ < | |/ ___/\ __\__ \ | | | | | |/ ___// __ \ / __ |
|
||||
\ \___| | \/\___ |\___ \ | | / __ \| |_| |_| |\___ \\ ___// /_/ |
|
||||
\___ >__| / ____/____ > |__| (____ /____/____/__/____ >\___ >____ |
|
||||
\/ \/ \/ \/ \/ \/ \/
|
||||
|
||||
Copyright:
|
||||
Danny Robson, 2011
|
||||
*****************************************************************************/
|
||||
|
||||
#include "matrix.hpp"
|
||||
|
||||
#include "debug.hpp"
|
||||
#include "range.hpp"
|
||||
#include "maths.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
|
||||
using namespace util;
|
||||
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);
|
||||
matrix::scale (double x, double y, double z) {
|
||||
check_hard (is_affine ());
|
||||
values[0][0] *= x;
|
||||
values[1][1] *= y;
|
||||
values[2][2] *= z;
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
void
|
||||
matrix::translate (double x, double y, double z) {
|
||||
check_hard (is_affine ());
|
||||
values[0][3] += x;
|
||||
values[1][3] += y;
|
||||
values[2][3] += z;
|
||||
}
|
||||
|
||||
|
||||
matrix
|
||||
matrix::inverse (void) const {
|
||||
if (m_rows != m_columns)
|
||||
not_implemented ();
|
||||
return matrix(*this).invert ();
|
||||
}
|
||||
|
||||
switch (m_rows) {
|
||||
case 2: return inverse2x2 ();
|
||||
case 3: return inverse3x3 ();
|
||||
case 4: return inverse4x4 ();
|
||||
}
|
||||
|
||||
not_implemented ();
|
||||
matrix&
|
||||
matrix::invert (void) {
|
||||
check_hard (is_affine ());
|
||||
|
||||
// inv ([ M b ] == [ inv(M) -inv(M).b ]
|
||||
// [ 0 1 ]) [ 0 1
|
||||
|
||||
// Invert the 3x3 M
|
||||
double A = (values[1][1] * values[2][2] - values[1][2] * values[2][1]);
|
||||
double B = (values[1][2] * values[2][0] - values[1][0] * values[2][2]);
|
||||
double C = (values[1][0] * values[2][1] - values[1][1] * values[2][0]);
|
||||
|
||||
double D = (values[0][2] * values[2][1] - values[0][1] * values[2][2]);
|
||||
double E = (values[0][0] * values[2][2] - values[0][2] * values[2][0]);
|
||||
double F = (values[2][0] * values[0][1] - values[0][0] * values[2][1]);
|
||||
|
||||
double G = (values[0][1] * values[1][2] - values[0][2] * values[1][1]);
|
||||
double H = (values[0][2] * values[1][0] - values[0][0] * values[1][2]);
|
||||
double K = (values[0][0] * values[1][1] - values[0][1] * values[1][0]);
|
||||
|
||||
double det = values[0][0] * A + values[0][1] * B + values[0][2] * C;
|
||||
check_neq (det, 0.0);
|
||||
|
||||
values[0][0] = A / det;
|
||||
values[0][1] = D / det;
|
||||
values[0][2] = G / det;
|
||||
values[1][0] = B / det;
|
||||
values[1][1] = E / det;
|
||||
values[1][2] = H / det;
|
||||
values[2][0] = C / det;
|
||||
values[2][1] = F / det;
|
||||
values[2][2] = K / det;
|
||||
|
||||
// Multiply the b
|
||||
double b0 = - values[0][0] * values[0][3] - values[0][1] * values[1][3] - values[0][2] * values[2][3];
|
||||
double b1 = - values[1][0] * values[0][3] - values[1][1] * values[1][3] - values[1][2] * values[2][3];
|
||||
double 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;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
matrix
|
||||
matrix::inverse2x2 (void) const {
|
||||
check (m_rows == 2);
|
||||
check (m_columns == 2);
|
||||
matrix::operator* (const matrix &rhs) const {
|
||||
matrix m;
|
||||
memset (m.values, 0, sizeof (m.values));
|
||||
|
||||
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;
|
||||
}
|
||||
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];
|
||||
|
||||
|
||||
// [ 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;
|
||||
util::point
|
||||
matrix::to_local (const util::point &p) const {
|
||||
check_soft (is_affine ());
|
||||
|
||||
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] };
|
||||
}
|
||||
|
||||
|
||||
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);
|
||||
util::point
|
||||
matrix::to_global (const util::point &p) const {
|
||||
matrix m = *this;
|
||||
m.invert ();
|
||||
return m.to_local (p);
|
||||
}
|
||||
|
||||
|
||||
// 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;
|
||||
bool
|
||||
matrix::is_affine (void) const {
|
||||
return exact_equal (values[3][0], 0.0) &&
|
||||
exact_equal (values[3][1], 0.0) &&
|
||||
exact_equal (values[3][2], 0.0) &&
|
||||
exact_equal (values[3][3], 1.0);
|
||||
}
|
||||
|
||||
|
||||
matrix
|
||||
matrix::magic_even_single (size_t)
|
||||
{ not_implemented (); }
|
||||
const matrix
|
||||
matrix::IDENTITY = { { { 1.0, 0.0, 0.0, 0.0 },
|
||||
{ 0.0, 1.0, 0.0, 0.0 },
|
||||
{ 0.0, 0.0, 1.0, 0.0 },
|
||||
{ 0.0, 0.0, 0.0, 1.0 } } };
|
||||
|
||||
|
||||
matrix
|
||||
matrix::magic_even_double (size_t)
|
||||
{ not_implemented (); }
|
||||
const matrix
|
||||
matrix::ZEROES = { { { 0.0, 0.0, 0.0, 0.0 },
|
||||
{ 0.0, 0.0, 0.0, 0.0 },
|
||||
{ 0.0, 0.0, 0.0, 0.0 },
|
||||
{ 0.0, 0.0, 0.0, 0.0 } } };
|
||||
|
||||
|
||||
std::ostream&
|
||||
operator<< (std::ostream &os, const matrix &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;
|
||||
}
|
||||
|
||||
|
||||
127
matrix.hpp
127
matrix.hpp
@ -1,119 +1,44 @@
|
||||
/*
|
||||
* 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>
|
||||
*/
|
||||
/****************************************************************************
|
||||
__ .__ .__ .__ .___
|
||||
___________ ___.__. _______/ |______ | | | | |__| ______ ____ __| _/
|
||||
_/ ___\_ __ < | |/ ___/\ __\__ \ | | | | | |/ ___// __ \ / __ |
|
||||
\ \___| | \/\___ |\___ \ | | / __ \| |_| |_| |\___ \\ ___// /_/ |
|
||||
\___ >__| / ____/____ > |__| (____ /____/____/__/____ >\___ >____ |
|
||||
\/ \/ \/ \/ \/ \/ \/
|
||||
|
||||
Copyright:
|
||||
Danny Robson, 2011
|
||||
*****************************************************************************/
|
||||
|
||||
#ifndef __UTIL_MATRIX_HPP
|
||||
#define __UTIL_MATRIX_HPP
|
||||
|
||||
#include "vector.hpp"
|
||||
#include "point.hpp"
|
||||
|
||||
#include <assert.h>
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
#include <initializer_list>
|
||||
#include <iostream>
|
||||
|
||||
namespace maths {
|
||||
class matrix {
|
||||
protected:
|
||||
size_t m_rows,
|
||||
m_columns;
|
||||
double *restrict m_data;
|
||||
namespace util {
|
||||
struct matrix {
|
||||
double values[4][4];
|
||||
|
||||
public:
|
||||
matrix (size_t _rows, size_t _columns);
|
||||
void scale (double x, double y, double z);
|
||||
void translate (double x, double y, double z);
|
||||
|
||||
matrix (size_t _rows,
|
||||
size_t _columns,
|
||||
const std::initializer_list <double> &_data);
|
||||
matrix (const std::initializer_list <vector> &_data);
|
||||
matrix inverse (void) const;
|
||||
matrix& invert (void);
|
||||
|
||||
matrix (const matrix &rhs);
|
||||
matrix (matrix &&rhs);
|
||||
matrix operator* (const matrix&) const;
|
||||
|
||||
~matrix();
|
||||
point to_local (const point &p) const;
|
||||
point to_global (const point &p) const;
|
||||
|
||||
void sanity (void) const;
|
||||
bool is_affine (void) const;
|
||||
|
||||
const double * operator [] (unsigned int row) const;
|
||||
double * operator [] (unsigned int row);
|
||||
|
||||
const double * data (void) const;
|
||||
|
||||
matrix& operator =(const matrix &rhs);
|
||||
matrix operator * (double scalar) const;
|
||||
matrix& operator *=(double scalar);
|
||||
matrix operator * (const matrix &rhs) const;
|
||||
matrix& operator *=(const matrix &rhs);
|
||||
matrix& operator /=(double scalar);
|
||||
matrix operator + (double scalar) const;
|
||||
matrix& operator +=(double scalar);
|
||||
matrix& operator -=(double scalar);
|
||||
bool operator ==(const matrix &rhs) const;
|
||||
|
||||
//matrix transpose (void) const { ; }
|
||||
|
||||
size_t rows (void) const;
|
||||
size_t columns (void) const;
|
||||
size_t size (void) const;
|
||||
|
||||
/// Checks if this is a sqaure matrix, with a zero final column
|
||||
/// and row (excepting the final diagonal entry).
|
||||
bool is_homogeneous (void) const;
|
||||
bool is_square (void) const;
|
||||
bool is_magic (void) const;
|
||||
public:
|
||||
double determinant (void) const;
|
||||
matrix inverse (void) const;
|
||||
|
||||
protected:
|
||||
double determinant2x2 (void) const;
|
||||
double determinant3x3 (void) const;
|
||||
double determinant4x4 (void) const;
|
||||
|
||||
matrix inverse2x2 (void) const;
|
||||
matrix inverse3x3 (void) const;
|
||||
matrix inverse4x4 (void) const;
|
||||
|
||||
public:
|
||||
static matrix zeroes (size_t n);
|
||||
static matrix zeroes (size_t rows, size_t columns);
|
||||
static matrix identity (size_t n);
|
||||
|
||||
/// Generate a magic square of order 'n'
|
||||
static matrix magic (size_t n);
|
||||
|
||||
protected:
|
||||
/// Generate a magic square with 'n' odd
|
||||
static matrix magic_odd (size_t n);
|
||||
/// Generate a magic square with 'n' divisible by 2, and not 4
|
||||
static matrix magic_even_single (size_t n);
|
||||
/// Generate a magic square with 'n' divisible by 4, and not 2
|
||||
static matrix magic_even_double (size_t n);
|
||||
};
|
||||
|
||||
class not_invertible : public std::runtime_error {
|
||||
public:
|
||||
not_invertible ():
|
||||
std::runtime_error ("not_invertible")
|
||||
{ ; }
|
||||
static const matrix IDENTITY;
|
||||
static const matrix ZEROES;
|
||||
};
|
||||
}
|
||||
|
||||
std::ostream& operator<< (std::ostream&, const util::matrix&);
|
||||
|
||||
#endif
|
||||
|
||||
53
point.cpp
53
point.cpp
@ -19,21 +19,14 @@
|
||||
|
||||
#include "point.hpp"
|
||||
|
||||
#include "debug.hpp"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
using namespace util;
|
||||
|
||||
|
||||
// Unused components are zeroed so that higher dimensional operations can
|
||||
// operate without fear of triggering NaNs or other such complications.
|
||||
point::point (double _x, double _y, double _z):
|
||||
x (_x),
|
||||
y (_y),
|
||||
z (_z)
|
||||
{ ; }
|
||||
|
||||
|
||||
double
|
||||
point::distance (const point &other) const {
|
||||
return sqrt ((x - other.x) * (x - other.x) +
|
||||
@ -51,17 +44,49 @@ point::manhattan (const point &other) const {
|
||||
|
||||
|
||||
point&
|
||||
point::operator+= (const point &rhs) {
|
||||
x += rhs.x;
|
||||
y += rhs.y;
|
||||
point::operator*= (double f) {
|
||||
x *= f;
|
||||
y *= f;
|
||||
z *= f;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
point
|
||||
point::operator- (const point &rhs) const
|
||||
{ return point (x - rhs.x, y - rhs.y); }
|
||||
point::operator* (double f) const {
|
||||
return { x * f, y * f, z * f };
|
||||
}
|
||||
|
||||
|
||||
point
|
||||
point::operator+ (const vector &rhs) const {
|
||||
return { x + rhs.x, y + rhs.y, z + rhs.z };
|
||||
}
|
||||
|
||||
|
||||
point&
|
||||
point::operator+= (const vector &rhs) {
|
||||
x += rhs.x;
|
||||
y += rhs.y;
|
||||
z += rhs.z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
util::vector
|
||||
point::operator- (const point &rhs) const {
|
||||
return { x - rhs.x, y - rhs.y, z - rhs.z };
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
point::sanity (void) const {
|
||||
check_soft (!std::isnan (x));
|
||||
check_soft (!std::isnan (y));
|
||||
check_soft (!std::isnan (z));
|
||||
}
|
||||
|
||||
|
||||
std::ostream&
|
||||
|
||||
14
point.hpp
14
point.hpp
@ -22,18 +22,24 @@
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include "vector.hpp"
|
||||
|
||||
namespace util {
|
||||
/// A three dimensional position in space.
|
||||
struct point {
|
||||
double x, y, z;
|
||||
|
||||
point (double x, double y, double z = 0.0);
|
||||
|
||||
double distance (const point &) const;
|
||||
double manhattan (const point &) const;
|
||||
|
||||
point& operator+= (const point&);
|
||||
point operator- (const point&) const;
|
||||
point& operator*= (double);
|
||||
point operator* (double) const;
|
||||
|
||||
point operator+ (const vector&) const;
|
||||
point& operator+= (const vector&);
|
||||
vector operator- (const point&) const;
|
||||
|
||||
void sanity (void) const;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
215
vector.cpp
215
vector.cpp
@ -1,80 +1,153 @@
|
||||
/*
|
||||
* 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 2011 Danny Robson <danny@blubinc.net>
|
||||
*/
|
||||
|
||||
#include "vector.hpp"
|
||||
|
||||
#include "debug.hpp"
|
||||
#include "maths.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <numeric>
|
||||
|
||||
|
||||
using namespace maths;
|
||||
using namespace util;
|
||||
|
||||
|
||||
/* Constructors */
|
||||
|
||||
vector::vector (const std::initializer_list<double> &_data):
|
||||
m_data (_data)
|
||||
{ ; }
|
||||
|
||||
|
||||
vector::vector (unsigned int _size):
|
||||
m_data (_size)
|
||||
{ ; }
|
||||
|
||||
|
||||
vector::vector (const double *restrict _data,
|
||||
unsigned int _size):
|
||||
m_data (_size)
|
||||
{ std::copy (_data, _data + _size, m_data.begin ()); }
|
||||
|
||||
|
||||
vector::vector (const vector &rhs):
|
||||
m_data (rhs.m_data)
|
||||
{ ; }
|
||||
|
||||
|
||||
vector::vector (const vector &&rhs):
|
||||
m_data (std::move (rhs.m_data))
|
||||
{ ; }
|
||||
|
||||
|
||||
vector::~vector (void)
|
||||
{ ; }
|
||||
|
||||
|
||||
/* element accessors */
|
||||
|
||||
const double*
|
||||
vector::data (void) const
|
||||
{ return &m_data[0]; }
|
||||
|
||||
|
||||
double &
|
||||
vector::operator[] (unsigned int offset)
|
||||
{ return m_data[offset]; }
|
||||
|
||||
|
||||
const double&
|
||||
vector::operator[] (unsigned int offset) const
|
||||
{ return m_data[offset]; }
|
||||
|
||||
|
||||
unsigned int
|
||||
vector::size (void) const
|
||||
{ return m_data.size (); }
|
||||
|
||||
|
||||
/* dot and cross products */
|
||||
|
||||
double vector::dot (const double *restrict A,
|
||||
const double *restrict B,
|
||||
unsigned int size)
|
||||
{ return std::inner_product(A, A + size, B, 0.0); }
|
||||
|
||||
|
||||
vector vector::cross (const double *restrict A,
|
||||
const double *restrict B,
|
||||
unsigned int size) {
|
||||
check_hard (size == 3);
|
||||
return vector ({ A[1] * B[2] - A[2] * B[1],
|
||||
A[2] * B[0] - A[0] * B[2],
|
||||
A[0] * B[1] - A[1] * B[0] });
|
||||
util::vector
|
||||
util::vector::operator* (double rhs) const {
|
||||
return { rhs * x, rhs * y, rhs * z };
|
||||
}
|
||||
|
||||
|
||||
vector&
|
||||
vector::operator*= (double rhs) {
|
||||
x *= rhs;
|
||||
y *= rhs;
|
||||
z *= rhs;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
vector
|
||||
vector::operator+ (const vector &rhs) const {
|
||||
return { x + rhs.x, y + rhs.y, z + rhs.z };
|
||||
}
|
||||
|
||||
|
||||
vector
|
||||
vector::operator- (const vector &rhs) const
|
||||
{ return { x - rhs.x, y - rhs.y, z - rhs.z }; }
|
||||
|
||||
|
||||
vector&
|
||||
vector::operator-= (const vector &rhs) {
|
||||
x -= rhs.x;
|
||||
y -= rhs.y;
|
||||
z -= rhs.z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
vector&
|
||||
vector::operator+= (const vector &rhs) {
|
||||
x += rhs.x;
|
||||
y += rhs.y;
|
||||
z += rhs.z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
vector&
|
||||
vector::operator= (const vector &rhs) {
|
||||
x = rhs.x;
|
||||
y = rhs.y;
|
||||
z = rhs.z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
double
|
||||
vector::magnitude (void) const {
|
||||
return sqrt (x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
|
||||
double
|
||||
vector::magnitude2 (void) const {
|
||||
return x * x + y * y + z * z;
|
||||
}
|
||||
|
||||
|
||||
double
|
||||
vector::dot (const vector &rhs) const {
|
||||
return x * rhs.x + y * rhs.y + z * rhs.z;
|
||||
}
|
||||
|
||||
|
||||
vector
|
||||
vector::cross (const vector &rhs) const {
|
||||
return { y * rhs.z - z * rhs.y,
|
||||
z * rhs.x - x * rhs.z,
|
||||
x * rhs.y - y * rhs.x };
|
||||
}
|
||||
|
||||
|
||||
vector&
|
||||
vector::normalise (void) {
|
||||
double mag = magnitude ();
|
||||
|
||||
x /= mag;
|
||||
y /= mag;
|
||||
z /= mag;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
vector
|
||||
vector::normalised (void) const {
|
||||
double mag = magnitude ();
|
||||
return { x / mag, y / mag, z / mag };
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
vector::sanity (void) const {
|
||||
check_soft (!std::isnan (x));
|
||||
check_soft (!std::isnan (y));
|
||||
check_soft (!std::isnan (z));
|
||||
}
|
||||
|
||||
|
||||
util::vector
|
||||
operator* (double a, const util::vector &b)
|
||||
{ return b * a; }
|
||||
|
||||
|
||||
std::ostream&
|
||||
operator<< (std::ostream &os, const util::vector &v) {
|
||||
os << "vec(" << v.x << ", " << v.y << ", " << v.z << ")";
|
||||
return os;
|
||||
}
|
||||
|
||||
|
||||
46
vector.hpp
46
vector.hpp
@ -20,42 +20,38 @@
|
||||
#ifndef __UTIL_VECTOR_HPP
|
||||
#define __UTIL_VECTOR_HPP
|
||||
|
||||
#include <vector>
|
||||
#include <initializer_list>
|
||||
#include <iostream>
|
||||
|
||||
namespace maths {
|
||||
class vector {
|
||||
protected:
|
||||
std::vector<double> m_data;
|
||||
namespace util {
|
||||
struct vector {
|
||||
double x, y, z;
|
||||
|
||||
public:
|
||||
vector (const std::initializer_list<double> &_data);
|
||||
explicit
|
||||
vector (unsigned int _size);
|
||||
vector (const double *restrict _data,
|
||||
unsigned int _size);
|
||||
vector operator* (double) const;
|
||||
vector& operator*=(double);
|
||||
|
||||
vector (const vector &rhs);
|
||||
vector (const vector &&rhs);
|
||||
vector operator+ (const vector&) const;
|
||||
vector& operator+=(const vector&);
|
||||
|
||||
~vector (void);
|
||||
vector operator- (const vector&) const;
|
||||
vector& operator-=(const vector&);
|
||||
|
||||
const double* data (void) const;
|
||||
double& operator[] (unsigned int);
|
||||
const double& operator[] (unsigned int) const;
|
||||
vector& operator =(const vector &);
|
||||
|
||||
unsigned int size (void) const;
|
||||
double magnitude (void) const;
|
||||
double magnitude2 (void) const;
|
||||
|
||||
double dot (const vector&) const;
|
||||
vector cross (const vector&) const;
|
||||
|
||||
static double dot (const double *restrict A,
|
||||
const double *restrict B,
|
||||
unsigned int size);
|
||||
vector& normalise (void);
|
||||
vector normalised (void) const;
|
||||
|
||||
static vector cross (const double *restrict A,
|
||||
const double *restrict B,
|
||||
unsigned int size);
|
||||
void sanity (void) const;
|
||||
};
|
||||
}
|
||||
|
||||
util::vector operator* (double, const util::vector&);
|
||||
|
||||
std::ostream& operator<< (std::ostream&, const util::vector&);
|
||||
#endif
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user