Make 3d matrix/vectors and general matrix/vectors

This commit is contained in:
Danny Robson 2011-10-18 21:45:55 +11:00
parent b71049df85
commit 2aee108e79
11 changed files with 1154 additions and 709 deletions

View File

@ -1,4 +1,4 @@
AUTOMAKE_OPTIONS = dist-bzip2 dist-xz foreign AUTOMAKE_OPTIONS = dist-bzip2 dist-xz foreign subdir-objects
ACLOCAL_AMFLAGS = -I m4 ACLOCAL_AMFLAGS = -I m4
AM_CXXFLAGS = $(BOOST_CPPFLAGS) $(COMMON_CXXFLAGS) AM_CXXFLAGS = $(BOOST_CPPFLAGS) $(COMMON_CXXFLAGS)
@ -23,6 +23,8 @@ UTIL_INCLUDE = \
ip.hpp \ ip.hpp \
json.hpp \ json.hpp \
maths.hpp \ maths.hpp \
maths/matrix.hpp \
maths/vector.hpp \
matrix.hpp \ matrix.hpp \
nocopy.hpp \ nocopy.hpp \
point.hpp \ point.hpp \
@ -31,12 +33,11 @@ UTIL_INCLUDE = \
range.hpp \ range.hpp \
region.hpp \ region.hpp \
signal.hpp \ signal.hpp \
si.hpp \
stream.hpp \ stream.hpp \
string.hpp \ string.hpp \
si.hpp \
time.hpp \ time.hpp \
types.hpp \ types.hpp \
vector.hpp \
version.hpp version.hpp
UTIL_FILES = \ UTIL_FILES = \
@ -52,16 +53,18 @@ UTIL_FILES = \
ip.cpp \ ip.cpp \
json.cpp \ json.cpp \
maths.cpp \ maths.cpp \
maths/matrix.cpp \
maths/vector.cpp \
matrix.cpp \ matrix.cpp \
point.cpp \ point.cpp \
pool.cpp \ pool.cpp \
random.cpp \ random.cpp \
range.cpp \ range.cpp \
region.cpp \ region.cpp \
si.cpp \
signal.cpp \ signal.cpp \
stream.cpp \ stream.cpp \
string.cpp \ string.cpp \
si.cpp \
time.cpp \ time.cpp \
types.cpp \ types.cpp \
vector.cpp \ vector.cpp \

532
maths/matrix.cpp Normal file
View 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
View 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
View 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
View 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

View File

@ -1,183 +1,89 @@
/* /****************************************************************************
* 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 Copyright:
* WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS Danny Robson, 2011
* 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 "matrix.hpp"
#include "debug.hpp" #include "debug.hpp"
#include "range.hpp"
#include "maths.hpp"
#include <algorithm> #include <cstring>
using namespace util; 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 void
matrix::sanity (void) const { matrix::scale (double x, double y, double z) {
check (m_rows > 0); check_hard (is_affine ());
check (m_columns > 0); values[0][0] *= x;
check (m_data != NULL); values[1][1] *= y;
values[2][2] *= z;
} }
const double * void
matrix::operator [] (unsigned int row) const { matrix::translate (double x, double y, double z) {
check_hard (row < m_rows); check_hard (is_affine ());
return m_data + row * m_columns; values[0][3] += x;
} values[1][3] += y;
values[2][3] += z;
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
matrix::operator * (double scalar) const { matrix::inverse (void) const {
matrix val (*this); return matrix(*this).invert ();
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&
matrix::operator *=(double scalar) { matrix::invert (void) {
for (unsigned int i = 0; i < m_rows; ++i) check_hard (is_affine ());
for (unsigned int j = 0; j < m_columns; ++j)
(*this)[i][j] *= scalar;
return *this; // 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]);
matrix& double D = (values[0][2] * values[2][1] - values[0][1] * values[2][2]);
matrix::operator /= (double scalar) double E = (values[0][0] * values[2][2] - values[0][2] * values[2][0]);
{ return (*this) *= (1.0 / scalar); } 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]);
matrix double det = values[0][0] * A + values[0][1] * B + values[0][2] * C;
matrix::operator + (double scalar) const { check_neq (det, 0.0);
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; 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];
matrix& values[0][3] = b0;
matrix::operator +=(double scalar) { values[1][3] = b1;
for (unsigned int i = 0; i < m_rows; ++i) values[2][3] = b2;
for (unsigned int j = 0; j < m_columns; ++j)
(*this)[i][j] += scalar;
return *this; return *this;
} }
@ -185,348 +91,67 @@ matrix::operator +=(double scalar) {
matrix matrix
matrix::operator* (const matrix &rhs) const { matrix::operator* (const matrix &rhs) const {
if (m_columns != rhs.rows ()) matrix m;
throw std::invalid_argument ("matrices size mismatch in multiplication"); memset (m.values, 0, sizeof (m.values));
matrix val (matrix::zeroes (m_rows, rhs.columns ())); 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];
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; return m;
} }
matrix util::point
matrix::identity (size_t diag) { matrix::to_local (const util::point &p) const {
matrix val (zeroes (diag)); check_soft (is_affine ());
for (unsigned int i = 0; i < diag; ++i)
val[i][i] = 1.0; return { p.x * values[0][0] + p.y * values[0][1] + p.z * values[0][2] + values[0][3],
return val; 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 util::point
matrix::magic (size_t n) { matrix::to_global (const util::point &p) const {
check_hard (n > 2); matrix m = *this;
m.invert ();
if (n % 2 == 1) return m.to_local (p);
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)); bool
for (unsigned int i = 1, x = n / 2, y = 0; i <= n * n; ++i) { matrix::is_affine (void) const {
val[y][x] = i; return exact_equal (values[3][0], 0.0) &&
exact_equal (values[3][1], 0.0) &&
unsigned int x1 = (x + 1) % n, exact_equal (values[3][2], 0.0) &&
y1 = (y + n - 1) % n; exact_equal (values[3][3], 1.0);
if (!almost_equal (val[y1][x1], 0)) {
x1 = x;
y1 = (y + 1) % n;
}
x = x1;
y = y1;
}
return val;
} }
matrix const matrix
matrix::magic_even_single (size_t) matrix::IDENTITY = { { { 1.0, 0.0, 0.0, 0.0 },
{ not_implemented (); } { 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 const matrix
matrix::magic_even_double (size_t) matrix::ZEROES = { { { 0.0, 0.0, 0.0, 0.0 },
{ not_implemented (); } { 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;
}

View File

@ -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 Copyright:
* WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS Danny Robson, 2011
* 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_MATRIX_HPP #ifndef __UTIL_MATRIX_HPP
#define __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> #include <iostream>
namespace maths { namespace util {
class matrix { struct matrix {
protected: double values[4][4];
size_t m_rows,
m_columns;
double *restrict m_data;
public: void scale (double x, double y, double z);
matrix (size_t _rows, size_t _columns); 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 (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; matrix inverse (void) const;
matrix& invert (void);
protected: matrix operator* (const matrix&) const;
double determinant2x2 (void) const;
double determinant3x3 (void) const;
double determinant4x4 (void) const;
matrix inverse2x2 (void) const; point to_local (const point &p) const;
matrix inverse3x3 (void) const; point to_global (const point &p) const;
matrix inverse4x4 (void) const;
public: bool is_affine (void) const;
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 const matrix IDENTITY;
static matrix magic (size_t n); static const matrix ZEROES;
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")
{ ; }
}; };
} }
std::ostream& operator<< (std::ostream&, const util::matrix&);
#endif #endif

View File

@ -19,21 +19,14 @@
#include "point.hpp" #include "point.hpp"
#include "debug.hpp"
#include <cmath> #include <cmath>
using namespace std; using namespace std;
using namespace util; 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 double
point::distance (const point &other) const { point::distance (const point &other) const {
return sqrt ((x - other.x) * (x - other.x) + return sqrt ((x - other.x) * (x - other.x) +
@ -51,17 +44,49 @@ point::manhattan (const point &other) const {
point& point&
point::operator+= (const point &rhs) { point::operator*= (double f) {
x += rhs.x; x *= f;
y += rhs.y; y *= f;
z *= f;
return *this; return *this;
} }
point point
point::operator- (const point &rhs) const point::operator* (double f) const {
{ return point (x - rhs.x, y - rhs.y); } 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& std::ostream&

View File

@ -22,18 +22,24 @@
#include <iostream> #include <iostream>
#include "vector.hpp"
namespace util { namespace util {
/// A three dimensional position in space. /// A three dimensional position in space.
struct point { struct point {
double x, y, z; double x, y, z;
point (double x, double y, double z = 0.0);
double distance (const point &) const; double distance (const point &) const;
double manhattan (const point &) const; double manhattan (const point &) const;
point& operator+= (const point&); point& operator*= (double);
point operator- (const point&) const; point operator* (double) const;
point operator+ (const vector&) const;
point& operator+= (const vector&);
vector operator- (const point&) const;
void sanity (void) const;
}; };
} }

View File

@ -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 "vector.hpp"
#include "debug.hpp" #include "debug.hpp"
#include "maths.hpp"
#include <algorithm>
#include <cmath>
#include <limits>
#include <numeric> #include <numeric>
using namespace maths; using namespace util;
util::vector
/* Constructors */ util::vector::operator* (double rhs) const {
return { rhs * x, rhs * y, rhs * z };
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] });
} }
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;
}

View File

@ -20,42 +20,38 @@
#ifndef __UTIL_VECTOR_HPP #ifndef __UTIL_VECTOR_HPP
#define __UTIL_VECTOR_HPP #define __UTIL_VECTOR_HPP
#include <vector> #include <iostream>
#include <initializer_list>
namespace maths { namespace util {
class vector { struct vector {
protected: double x, y, z;
std::vector<double> m_data;
public: vector operator* (double) const;
vector (const std::initializer_list<double> &_data); vector& operator*=(double);
explicit
vector (unsigned int _size);
vector (const double *restrict _data,
unsigned int _size);
vector (const vector &rhs); vector operator+ (const vector&) const;
vector (const vector &&rhs); vector& operator+=(const vector&);
~vector (void); vector operator- (const vector&) const;
vector& operator-=(const vector&);
const double* data (void) const; vector& operator =(const vector &);
double& operator[] (unsigned int);
const double& operator[] (unsigned int) const;
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, vector& normalise (void);
const double *restrict B, vector normalised (void) const;
unsigned int size);
static vector cross (const double *restrict A, void sanity (void) const;
const double *restrict B,
unsigned int size);
}; };
} }
util::vector operator* (double, const util::vector&);
std::ostream& operator<< (std::ostream&, const util::vector&);
#endif #endif