nerdcruft/libcruft-util
1
0
Fork 0
Code Issues Releases Activity

Make 3d matrix/vectors and general matrix/vectors

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Makefile.am
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

579
matrix.cpp
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;
}

125
matrix.hpp
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

53
point.cpp
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&

14
point.hpp
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;
}; };
} }

215
vector.cpp
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;
}

46
vector.hpp
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