diff --git a/Makefile.am b/Makefile.am index be218ece..62c87679 100644 --- a/Makefile.am +++ b/Makefile.am @@ -1,4 +1,4 @@ -AUTOMAKE_OPTIONS = dist-bzip2 dist-xz foreign +AUTOMAKE_OPTIONS = dist-bzip2 dist-xz foreign subdir-objects ACLOCAL_AMFLAGS = -I m4 AM_CXXFLAGS = $(BOOST_CPPFLAGS) $(COMMON_CXXFLAGS) @@ -23,6 +23,8 @@ UTIL_INCLUDE = \ ip.hpp \ json.hpp \ maths.hpp \ + maths/matrix.hpp \ + maths/vector.hpp \ matrix.hpp \ nocopy.hpp \ point.hpp \ @@ -31,12 +33,11 @@ UTIL_INCLUDE = \ range.hpp \ region.hpp \ signal.hpp \ + si.hpp \ stream.hpp \ string.hpp \ - si.hpp \ time.hpp \ types.hpp \ - vector.hpp \ version.hpp UTIL_FILES = \ @@ -52,19 +53,21 @@ UTIL_FILES = \ ip.cpp \ json.cpp \ maths.cpp \ + maths/matrix.cpp \ + maths/vector.cpp \ matrix.cpp \ point.cpp \ pool.cpp \ random.cpp \ range.cpp \ region.cpp \ + si.cpp \ signal.cpp \ stream.cpp \ string.cpp \ - si.cpp \ time.cpp \ types.cpp \ - vector.cpp \ + vector.cpp \ version.cpp diff --git a/maths/matrix.cpp b/maths/matrix.cpp new file mode 100644 index 00000000..4d8bd0b4 --- /dev/null +++ b/maths/matrix.cpp @@ -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 . + * + * Copyright 2010 Danny Robson + */ + +#include "matrix.hpp" + +#include "debug.hpp" +#include "range.hpp" +#include "maths.hpp" + +#include + +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 &_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 &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 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 (); } + diff --git a/maths/matrix.hpp b/maths/matrix.hpp new file mode 100644 index 00000000..d211e6e7 --- /dev/null +++ b/maths/matrix.hpp @@ -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 . + * + * Copyright 2010 Danny Robson + */ + +#ifndef __UTIL_MATHS_MATRIX_HPP +#define __UTIL_MATHS_MATRIX_HPP + +#include "vector.hpp" + +#include +#include +#include +#include +#include + +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 &_data); + matrix (const std::initializer_list &_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 diff --git a/maths/vector.cpp b/maths/vector.cpp new file mode 100644 index 00000000..b9bb5164 --- /dev/null +++ b/maths/vector.cpp @@ -0,0 +1,80 @@ +#include "vector.hpp" + +#include "debug.hpp" + +#include + + +using namespace maths; + + +/* Constructors */ + +vector::vector (const std::initializer_list &_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] }); +} diff --git a/maths/vector.hpp b/maths/vector.hpp new file mode 100644 index 00000000..5f483c59 --- /dev/null +++ b/maths/vector.hpp @@ -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 . + * + * Copyright 2011 Danny Robson + */ + +#ifndef __UTIL_MATHS_VECTOR_HPP +#define __UTIL_MATHS_VECTOR_HPP + +#include +#include + +namespace maths { + class vector { + protected: + std::vector m_data; + + public: + vector (const std::initializer_list &_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 diff --git a/matrix.cpp b/matrix.cpp index 4d8bd0b4..f6e49aa7 100644 --- a/matrix.cpp +++ b/matrix.cpp @@ -1,532 +1,157 @@ -/* - * This file is part of libgim. - * - * libgim is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * libgim is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS - * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more - * details. - * - * You should have received a copy of the GNU General Public License - * along with libgim. If not, see . - * - * Copyright 2010 Danny Robson - */ + /**************************************************************************** + __ .__ .__ .__ .___ + ___________ ___.__. _______/ |______ | | | | |__| ______ ____ __| _/ + _/ ___\_ __ < | |/ ___/\ __\__ \ | | | | | |/ ___// __ \ / __ | + \ \___| | \/\___ |\___ \ | | / __ \| |_| |_| |\___ \\ ___// /_/ | + \___ >__| / ____/____ > |__| (____ /____/____/__/____ >\___ >____ | + \/ \/ \/ \/ \/ \/ \/ + + Copyright: + Danny Robson, 2011 + *****************************************************************************/ #include "matrix.hpp" #include "debug.hpp" -#include "range.hpp" -#include "maths.hpp" -#include +#include 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 &_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 &rhs): - m_rows (rhs.size ()), - m_columns (rhs.begin()->size ()), - m_data (new double[m_rows * m_columns]) -{ - double *row_cursor = m_data; - - for (auto i = rhs.begin (); i != rhs.end (); ++i) { - check (i->size () == m_columns); - - std::copy (i->data (), i->data () + i->size (), row_cursor); - row_cursor += m_columns; - } -} - - -matrix::matrix (const matrix &rhs): - m_rows (rhs.m_rows), - m_columns (rhs.m_columns) { - m_data = new double [m_rows * m_columns]; - std::copy (rhs.m_data, rhs.m_data + m_rows * m_columns, m_data); -} - - -matrix::matrix (matrix &&rhs): - m_rows (rhs.m_rows), - m_columns (rhs.m_columns), - m_data (rhs.m_data) { - rhs.m_data = NULL; -} - - -matrix::~matrix() - { delete [] m_data; } void -matrix::sanity (void) const { - check (m_rows > 0); - check (m_columns > 0); - check (m_data != NULL); +matrix::scale (double x, double y, double z) { + check_hard (is_affine ()); + values[0][0] *= x; + values[1][1] *= y; + values[2][2] *= z; } -const double * -matrix::operator [] (unsigned int row) const { - check_hard (row < m_rows); - return m_data + row * m_columns; -} - - -double * -matrix::operator [] (unsigned int row) { - check_hard (row < m_rows); - return m_data + row * m_columns; -} - - -const double * -matrix::data (void) const - { return m_data; } - - -matrix& -matrix::operator =(const matrix& rhs) { - if (size () != rhs.size ()) { - delete [] m_data; - m_data = new double [m_rows * m_columns]; - } - - m_rows = rhs.m_rows; - m_columns = rhs.m_columns; - std::copy (rhs.m_data, rhs.m_data + m_rows * m_columns, m_data); - - return *this; -} - - -matrix -matrix::operator * (double scalar) const { - matrix val (*this); - - for (unsigned int i = 0; i < m_rows; ++i) - for (unsigned int j = 0; j < m_columns; ++j) - val[i][j] *= scalar; - - return val; -} - - -matrix& -matrix::operator *=(double scalar) { - for (unsigned int i = 0; i < m_rows; ++i) - for (unsigned int j = 0; j < m_columns; ++j) - (*this)[i][j] *= scalar; - - return *this; -} - - -matrix& -matrix::operator /= (double scalar) - { return (*this) *= (1.0 / scalar); } - - -matrix -matrix::operator + (double scalar) const { - matrix val (*this); - for (unsigned int i = 0; i < m_rows; ++i) - for (unsigned int j = 0; j < m_columns; ++j) - val[i][j] += scalar; - - return val; -} - - -matrix& -matrix::operator +=(double scalar) { - for (unsigned int i = 0; i < m_rows; ++i) - for (unsigned int j = 0; j < m_columns; ++j) - (*this)[i][j] += scalar; - - return *this; -} - - -matrix -matrix::operator * (const matrix& rhs) const { - if (m_columns != rhs.rows ()) - throw std::invalid_argument ("matrices size mismatch in multiplication"); - - matrix val (matrix::zeroes (m_rows, rhs.columns ())); - - for (unsigned int i = 0; i < m_rows; ++i) - for (unsigned int j = 0; j < rhs.columns (); ++j) - for (unsigned int k = 0; k < m_columns; ++k) - val[i][j] += (*this)[i][k] * rhs[k][j]; - - return val; -} - - -matrix& -matrix::operator *=(const matrix& rhs) - { return *this = *this * rhs; } - - -bool -matrix::operator ==(const matrix& rhs) const { - if (rhs.rows () != rows () || - rhs.columns () != columns ()) - return false; - - return std::equal (m_data, m_data + size (), rhs.data ()); -} - - -//matrix transpose (void) const { ; } - - -size_t -matrix::rows (void) const - { return m_rows; } - - -size_t -matrix::columns (void) const - { return m_columns; } - - -size_t -matrix::size (void) const - { return rows () * columns (); } - - -bool -matrix::is_square (void) const - { return m_rows == m_columns; } - - -bool -matrix::is_magic (void) const { - if (!is_square ()) - return false; - - unsigned int expected = m_rows * (m_rows * m_rows + 1) / 2; - range numbers (1, m_rows * m_rows); - - for (unsigned int i = 0; i < m_rows; ++i) { - unsigned int sum1 = 0, sum2 = 0; - - for (unsigned int j = 0; j < m_columns; ++j) { - if (!numbers.contains ((*this)[i][j]) || - !numbers.contains ((*this)[j][i])) - return false; - - sum1 += (*this)[i][j]; - sum2 += (*this)[j][i]; - } - - if (sum1 != expected || sum2 != expected) - return false; - } - - return true; -} - - -bool -matrix::is_homogeneous (void) const { - if (m_rows != m_columns) - return false; - - // Check the final row is all zeroes - for (unsigned int i = 0; i < m_columns - 1; ++i) { - if (!almost_equal ((*this)[m_rows - 1][i], 0.)) - return false; - } - - // Except for the last element, which has to be one - return almost_equal ((*this)[m_rows - 1][m_columns - 1], 1.); -} - - -double -matrix::determinant (void) const { - if (m_rows != m_columns) - not_implemented (); - - switch (m_rows) { - case 2: return determinant2x2 (); - case 3: return determinant3x3 (); - case 4: return determinant4x4 (); - } - - not_implemented (); -} - - -// With matrix A = [ a, b ] -// [ c, d ] -// -// det (A) = ad - bc - -double -matrix::determinant2x2 (void) const { - check_eq (m_rows, 2); - check_eq (m_columns, 2); - - return (*this)[0][0] * (*this)[1][1] - - (*this)[0][1] * (*this)[1][0]; -} - - - -// [ a, b, c ] -// Given matrix A = [ d, e, f ] -// [ g, h, i ] -// -// det (A) = aei + bfg + cdh - afg - bdi - ceg -// det (A) = a(ei - fg) + b(fg - di) + c(dh - eg) -double -matrix::determinant3x3 (void) const { - check_eq (m_rows, 3); - check_eq (m_columns, 3); - - return (*this)[0][0] * (*this)[1][1] * (*this)[2][2] + // aei - (*this)[0][1] * (*this)[1][2] * (*this)[2][0] + // bfg - (*this)[0][2] * (*this)[1][0] * (*this)[2][1] - // cdh - (*this)[0][0] * (*this)[1][2] * (*this)[2][1] - // afh - (*this)[0][1] * (*this)[1][0] * (*this)[2][2] - // bdi - (*this)[0][2] * (*this)[1][1] * (*this)[2][0]; // ceg -} - - -// From libMathematics, http://www.geometrictools.com/ -double -matrix::determinant4x4 (void) const { - check_eq (m_rows, 4); - check_eq (m_columns, 4); - - double a0 = m_data[ 0] * m_data[ 5] - m_data[ 1] * m_data[ 4], - a1 = m_data[ 0] * m_data[ 6] - m_data[ 2] * m_data[ 4], - a2 = m_data[ 0] * m_data[ 7] - m_data[ 3] * m_data[ 4], - a3 = m_data[ 1] * m_data[ 6] - m_data[ 2] * m_data[ 5], - a4 = m_data[ 1] * m_data[ 7] - m_data[ 3] * m_data[ 5], - a5 = m_data[ 2] * m_data[ 7] - m_data[ 3] * m_data[ 6], - b0 = m_data[ 8] * m_data[13] - m_data[ 9] * m_data[12], - b1 = m_data[ 8] * m_data[14] - m_data[10] * m_data[12], - b2 = m_data[ 8] * m_data[15] - m_data[11] * m_data[12], - b3 = m_data[ 9] * m_data[14] - m_data[10] * m_data[13], - b4 = m_data[ 9] * m_data[15] - m_data[11] * m_data[13], - b5 = m_data[10] * m_data[15] - m_data[11] * m_data[14]; - - return a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0; +void +matrix::translate (double x, double y, double z) { + check_hard (is_affine ()); + values[0][3] += x; + values[1][3] += y; + values[2][3] += z; } matrix matrix::inverse (void) const { - if (m_rows != m_columns) - not_implemented (); + return matrix(*this).invert (); +} - switch (m_rows) { - case 2: return inverse2x2 (); - case 3: return inverse3x3 (); - case 4: return inverse4x4 (); - } - not_implemented (); +matrix& +matrix::invert (void) { + check_hard (is_affine ()); + + // inv ([ M b ] == [ inv(M) -inv(M).b ] + // [ 0 1 ]) [ 0 1 + + // Invert the 3x3 M + double A = (values[1][1] * values[2][2] - values[1][2] * values[2][1]); + double B = (values[1][2] * values[2][0] - values[1][0] * values[2][2]); + double C = (values[1][0] * values[2][1] - values[1][1] * values[2][0]); + + double D = (values[0][2] * values[2][1] - values[0][1] * values[2][2]); + double E = (values[0][0] * values[2][2] - values[0][2] * values[2][0]); + double F = (values[2][0] * values[0][1] - values[0][0] * values[2][1]); + + double G = (values[0][1] * values[1][2] - values[0][2] * values[1][1]); + double H = (values[0][2] * values[1][0] - values[0][0] * values[1][2]); + double K = (values[0][0] * values[1][1] - values[0][1] * values[1][0]); + + double det = values[0][0] * A + values[0][1] * B + values[0][2] * C; + check_neq (det, 0.0); + + values[0][0] = A / det; + values[0][1] = D / det; + values[0][2] = G / det; + values[1][0] = B / det; + values[1][1] = E / det; + values[1][2] = H / det; + values[2][0] = C / det; + values[2][1] = F / det; + values[2][2] = K / det; + + // Multiply the b + double b0 = - values[0][0] * values[0][3] - values[0][1] * values[1][3] - values[0][2] * values[2][3]; + double b1 = - values[1][0] * values[0][3] - values[1][1] * values[1][3] - values[1][2] * values[2][3]; + double b2 = - values[2][0] * values[0][3] - values[2][1] * values[1][3] - values[2][2] * values[2][3]; + + values[0][3] = b0; + values[1][3] = b1; + values[2][3] = b2; + + return *this; } matrix -matrix::inverse2x2 (void) const { - check (m_rows == 2); - check (m_columns == 2); +matrix::operator* (const matrix &rhs) const { + matrix m; + memset (m.values, 0, sizeof (m.values)); - double det = determinant2x2 (); - if (almost_equal (det, 0.)) - throw not_invertible (); - return matrix (2, 2, { (*this)[1][1], -(*this)[0][1], - -(*this)[1][0], (*this)[0][0] }) /= det; -} + for (unsigned i = 0; i < 4; ++i) + for (unsigned j = 0; j < 4; ++j) + for (unsigned k = 0; k < 4; ++k) + m.values[i][j] += values[i][k] * rhs.values[k][j]; - -// [ a, b, c ] -// Given matrix A = [ d, e, f ] -// [ g, h, i ] -// -matrix -matrix::inverse3x3 (void) const { - check (m_rows == 3); - check (m_columns == 3); - - double det = determinant3x3(); - if (almost_equal (det, 0.)) - throw not_invertible (); - - matrix val (m_rows, m_columns, { - (*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1], // ei - fh - (*this)[0][2] * (*this)[2][1] - (*this)[0][1] * (*this)[2][2], // ch - bi - (*this)[0][1] * (*this)[1][2] - (*this)[0][2] * (*this)[1][1], // bf - ce - (*this)[1][2] * (*this)[2][0] - (*this)[1][0] * (*this)[2][2], // fg - di - (*this)[0][0] * (*this)[2][2] - (*this)[0][2] * (*this)[2][0], // ai - cg - (*this)[0][2] * (*this)[1][0] - (*this)[0][0] * (*this)[1][2], // cd - af - (*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0], // dh - eg - (*this)[0][1] * (*this)[2][0] - (*this)[0][0] * (*this)[2][1], // bg - ah - (*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0] // ae - bd - }); - - return val /= det; - - //matrix val ({ vector::cross ((*this)[1], (*this)[2], 3), - // vector::cross ((*this)[2], (*this)[0], 3), - // vector::cross ((*this)[0], (*this)[1], 3) }); - //return val /= determinant3x3 (); -} - - -matrix -matrix::inverse4x4 (void) const { - double a0 = m_data[ 0] * m_data[ 5] - m_data[ 1] * m_data[ 4], - a1 = m_data[ 0] * m_data[ 6] - m_data[ 2] * m_data[ 4], - a2 = m_data[ 0] * m_data[ 7] - m_data[ 3] * m_data[ 4], - a3 = m_data[ 1] * m_data[ 6] - m_data[ 2] * m_data[ 5], - a4 = m_data[ 1] * m_data[ 7] - m_data[ 3] * m_data[ 5], - a5 = m_data[ 2] * m_data[ 7] - m_data[ 3] * m_data[ 6], - b0 = m_data[ 8] * m_data[13] - m_data[ 9] * m_data[12], - b1 = m_data[ 8] * m_data[14] - m_data[10] * m_data[12], - b2 = m_data[ 8] * m_data[15] - m_data[11] * m_data[12], - b3 = m_data[ 9] * m_data[14] - m_data[10] * m_data[13], - b4 = m_data[ 9] * m_data[15] - m_data[11] * m_data[13], - b5 = m_data[10] * m_data[15] - m_data[11] * m_data[14]; - - double det = a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0; - if (almost_equal (det, 0.)) - throw not_invertible (); - - return matrix (4, 4, { - + m_data[ 5] * b5 - m_data[ 6] * b4 + m_data[ 7] * b3, - - m_data[ 1] * b5 + m_data[ 2] * b4 - m_data[ 3] * b3, - + m_data[13] * a5 - m_data[14] * a4 + m_data[15] * a3, - - m_data[ 9] * a5 + m_data[10] * a4 - m_data[11] * a3, - - m_data[ 4] * b5 + m_data[ 6] * b2 - m_data[ 7] * b1, - + m_data[ 0] * b5 - m_data[ 2] * b2 + m_data[ 3] * b1, - - m_data[12] * a5 + m_data[14] * a2 - m_data[15] * a1, - + m_data[ 8] * a5 - m_data[10] * a2 + m_data[11] * a1, - + m_data[ 4] * b4 - m_data[ 5] * b2 + m_data[ 7] * b0, - - m_data[ 0] * b4 + m_data[ 1] * b2 - m_data[ 3] * b0, - + m_data[12] * a4 - m_data[13] * a2 + m_data[15] * a0, - - m_data[ 8] * a4 + m_data[ 9] * a2 - m_data[11] * a0, - - m_data[ 4] * b3 + m_data[ 5] * b1 - m_data[ 6] * b0, - + m_data[ 0] * b3 - m_data[ 1] * b1 + m_data[ 2] * b0, - - m_data[12] * a3 + m_data[13] * a1 - m_data[14] * a0, - + m_data[ 8] * a3 - m_data[ 9] * a1 + m_data[10] * a0 - }) /= det; -} - - - -matrix -matrix::zeroes (size_t diag) - { return zeroes (diag, diag); } - - -matrix -matrix::zeroes (size_t rows, size_t columns) { - matrix m (rows, columns); - std::fill (m.m_data, m.m_data + m.size (), 0.0); return m; } -matrix -matrix::identity (size_t diag) { - matrix val (zeroes (diag)); - for (unsigned int i = 0; i < diag; ++i) - val[i][i] = 1.0; - return val; +util::point +matrix::to_local (const util::point &p) const { + check_soft (is_affine ()); + + return { p.x * values[0][0] + p.y * values[0][1] + p.z * values[0][2] + values[0][3], + p.x * values[1][0] + p.y * values[1][1] + p.z * values[1][2] + values[1][3], + p.x * values[2][0] + p.y * values[2][1] + p.z * values[2][2] + values[2][3] }; } -matrix -matrix::magic (size_t n) { - check_hard (n > 2); - - if (n % 2 == 1) - return magic_odd (n); - - if (n % 4 == 0) - return magic_even_single (n); - - return magic_even_double (n); +util::point +matrix::to_global (const util::point &p) const { + matrix m = *this; + m.invert (); + return m.to_local (p); } -// Use the 'siamese' method. Start from the top centre, progress up-left one. -// If filled then drop down one row instead. Wrap around indexing. -matrix -matrix::magic_odd (size_t n) { - check_hard (n > 2); - check_hard (n % 2 == 1); - matrix val (zeroes (n)); - for (unsigned int i = 1, x = n / 2, y = 0; i <= n * n; ++i) { - val[y][x] = i; - - unsigned int x1 = (x + 1) % n, - y1 = (y + n - 1) % n; - - if (!almost_equal (val[y1][x1], 0)) { - x1 = x; - y1 = (y + 1) % n; - } - - x = x1; - y = y1; - } - - return val; +bool +matrix::is_affine (void) const { + return exact_equal (values[3][0], 0.0) && + exact_equal (values[3][1], 0.0) && + exact_equal (values[3][2], 0.0) && + exact_equal (values[3][3], 1.0); } -matrix -matrix::magic_even_single (size_t) - { not_implemented (); } +const matrix +matrix::IDENTITY = { { { 1.0, 0.0, 0.0, 0.0 }, + { 0.0, 1.0, 0.0, 0.0 }, + { 0.0, 0.0, 1.0, 0.0 }, + { 0.0, 0.0, 0.0, 1.0 } } }; -matrix -matrix::magic_even_double (size_t) - { not_implemented (); } +const matrix +matrix::ZEROES = { { { 0.0, 0.0, 0.0, 0.0 }, + { 0.0, 0.0, 0.0, 0.0 }, + { 0.0, 0.0, 0.0, 0.0 }, + { 0.0, 0.0, 0.0, 0.0 } } }; + + +std::ostream& +operator<< (std::ostream &os, const matrix &m) { + os << "{ {" << m.values[0][0] << ", " << m.values[0][1] << ", " << m.values[0][2] << ", " << m.values[0][3] << "}, " + << "{" << m.values[1][0] << ", " << m.values[1][1] << ", " << m.values[1][2] << ", " << m.values[1][3] << "}, " + << "{" << m.values[2][0] << ", " << m.values[2][1] << ", " << m.values[2][2] << ", " << m.values[2][3] << "}, " + << "{" << m.values[3][0] << ", " << m.values[3][1] << ", " << m.values[3][2] << ", " << m.values[3][3] << "} }"; + + return os; +} diff --git a/matrix.hpp b/matrix.hpp index fa0f8a16..8b7ee993 100644 --- a/matrix.hpp +++ b/matrix.hpp @@ -1,119 +1,44 @@ -/* - * This file is part of libgim. - * - * libgim is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * libgim is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS - * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more - * details. - * - * You should have received a copy of the GNU General Public License - * along with libgim. If not, see . - * - * Copyright 2010 Danny Robson - */ + /**************************************************************************** + __ .__ .__ .__ .___ + ___________ ___.__. _______/ |______ | | | | |__| ______ ____ __| _/ + _/ ___\_ __ < | |/ ___/\ __\__ \ | | | | | |/ ___// __ \ / __ | + \ \___| | \/\___ |\___ \ | | / __ \| |_| |_| |\___ \\ ___// /_/ | + \___ >__| / ____/____ > |__| (____ /____/____/__/____ >\___ >____ | + \/ \/ \/ \/ \/ \/ \/ + + Copyright: + Danny Robson, 2011 + *****************************************************************************/ #ifndef __UTIL_MATRIX_HPP #define __UTIL_MATRIX_HPP -#include "vector.hpp" +#include "point.hpp" -#include -#include -#include -#include #include -namespace maths { - class matrix { - protected: - size_t m_rows, - m_columns; - double *restrict m_data; +namespace util { + struct matrix { + double values[4][4]; - public: - matrix (size_t _rows, size_t _columns); + void scale (double x, double y, double z); + void translate (double x, double y, double z); - matrix (size_t _rows, - size_t _columns, - const std::initializer_list &_data); - matrix (const std::initializer_list &_data); + matrix inverse (void) const; + matrix& invert (void); - matrix (const matrix &rhs); - matrix (matrix &&rhs); + matrix operator* (const matrix&) const; - ~matrix(); + point to_local (const point &p) const; + point to_global (const point &p) const; - void sanity (void) const; + bool is_affine (void) const; - const double * operator [] (unsigned int row) const; - double * operator [] (unsigned int row); - - const double * data (void) const; - - matrix& operator =(const matrix &rhs); - matrix operator * (double scalar) const; - matrix& operator *=(double scalar); - matrix operator * (const matrix &rhs) const; - matrix& operator *=(const matrix &rhs); - matrix& operator /=(double scalar); - matrix operator + (double scalar) const; - matrix& operator +=(double scalar); - matrix& operator -=(double scalar); - bool operator ==(const matrix &rhs) const; - - //matrix transpose (void) const { ; } - - size_t rows (void) const; - size_t columns (void) const; - size_t size (void) const; - - /// Checks if this is a sqaure matrix, with a zero final column - /// and row (excepting the final diagonal entry). - bool is_homogeneous (void) const; - bool is_square (void) const; - bool is_magic (void) const; - public: - double determinant (void) const; - matrix inverse (void) const; - - protected: - double determinant2x2 (void) const; - double determinant3x3 (void) const; - double determinant4x4 (void) const; - - matrix inverse2x2 (void) const; - matrix inverse3x3 (void) const; - matrix inverse4x4 (void) const; - - public: - static matrix zeroes (size_t n); - static matrix zeroes (size_t rows, size_t columns); - static matrix identity (size_t n); - - /// Generate a magic square of order 'n' - static matrix magic (size_t n); - - protected: - /// Generate a magic square with 'n' odd - static matrix magic_odd (size_t n); - /// Generate a magic square with 'n' divisible by 2, and not 4 - static matrix magic_even_single (size_t n); - /// Generate a magic square with 'n' divisible by 4, and not 2 - static matrix magic_even_double (size_t n); - }; - - class not_invertible : public std::runtime_error { - public: - not_invertible (): - std::runtime_error ("not_invertible") - { ; } + static const matrix IDENTITY; + static const matrix ZEROES; }; } +std::ostream& operator<< (std::ostream&, const util::matrix&); #endif diff --git a/point.cpp b/point.cpp index b302ab96..5f6b0a9b 100644 --- a/point.cpp +++ b/point.cpp @@ -19,21 +19,14 @@ #include "point.hpp" +#include "debug.hpp" + #include using namespace std; using namespace util; -// Unused components are zeroed so that higher dimensional operations can -// operate without fear of triggering NaNs or other such complications. -point::point (double _x, double _y, double _z): - x (_x), - y (_y), - z (_z) -{ ; } - - double point::distance (const point &other) const { return sqrt ((x - other.x) * (x - other.x) + @@ -51,17 +44,49 @@ point::manhattan (const point &other) const { point& -point::operator+= (const point &rhs) { - x += rhs.x; - y += rhs.y; +point::operator*= (double f) { + x *= f; + y *= f; + z *= f; return *this; } point -point::operator- (const point &rhs) const - { return point (x - rhs.x, y - rhs.y); } +point::operator* (double f) const { + return { x * f, y * f, z * f }; +} + + +point +point::operator+ (const vector &rhs) const { + return { x + rhs.x, y + rhs.y, z + rhs.z }; +} + + +point& +point::operator+= (const vector &rhs) { + x += rhs.x; + y += rhs.y; + z += rhs.z; + + return *this; +} + + +util::vector +point::operator- (const point &rhs) const { + return { x - rhs.x, y - rhs.y, z - rhs.z }; +} + + +void +point::sanity (void) const { + check_soft (!std::isnan (x)); + check_soft (!std::isnan (y)); + check_soft (!std::isnan (z)); +} std::ostream& diff --git a/point.hpp b/point.hpp index 04ef61e7..f888b79c 100644 --- a/point.hpp +++ b/point.hpp @@ -22,18 +22,24 @@ #include +#include "vector.hpp" + namespace util { /// A three dimensional position in space. struct point { double x, y, z; - point (double x, double y, double z = 0.0); - double distance (const point &) const; double manhattan (const point &) const; - point& operator+= (const point&); - point operator- (const point&) const; + point& operator*= (double); + point operator* (double) const; + + point operator+ (const vector&) const; + point& operator+= (const vector&); + vector operator- (const point&) const; + + void sanity (void) const; }; } diff --git a/vector.cpp b/vector.cpp index b9bb5164..ee3903aa 100644 --- a/vector.cpp +++ b/vector.cpp @@ -1,80 +1,153 @@ +/* + * This file is part of libgim. + * + * libgim is free software: you can redistribute it and/or modify it under the + * terms of the GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) any later + * version. + * + * libgim is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS + * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more + * details. + * + * You should have received a copy of the GNU General Public License + * along with libgim. If not, see . + * + * Copyright 2011 Danny Robson + */ + #include "vector.hpp" #include "debug.hpp" +#include "maths.hpp" +#include +#include +#include #include -using namespace maths; +using namespace util; - -/* Constructors */ - -vector::vector (const std::initializer_list &_data): - m_data (_data) -{ ; } - - -vector::vector (unsigned int _size): - m_data (_size) -{ ; } - - -vector::vector (const double *restrict _data, - unsigned int _size): - m_data (_size) -{ std::copy (_data, _data + _size, m_data.begin ()); } - - -vector::vector (const vector &rhs): - m_data (rhs.m_data) -{ ; } - - -vector::vector (const vector &&rhs): - m_data (std::move (rhs.m_data)) -{ ; } - - -vector::~vector (void) -{ ; } - - -/* element accessors */ - -const double* -vector::data (void) const - { return &m_data[0]; } - - -double & -vector::operator[] (unsigned int offset) - { return m_data[offset]; } - - -const double& -vector::operator[] (unsigned int offset) const - { return m_data[offset]; } - - -unsigned int -vector::size (void) const - { return m_data.size (); } - - -/* dot and cross products */ - -double vector::dot (const double *restrict A, - const double *restrict B, - unsigned int size) -{ return std::inner_product(A, A + size, B, 0.0); } - - -vector vector::cross (const double *restrict A, - const double *restrict B, - unsigned int size) { - check_hard (size == 3); - return vector ({ A[1] * B[2] - A[2] * B[1], - A[2] * B[0] - A[0] * B[2], - A[0] * B[1] - A[1] * B[0] }); +util::vector +util::vector::operator* (double rhs) const { + return { rhs * x, rhs * y, rhs * z }; } + + +vector& +vector::operator*= (double rhs) { + x *= rhs; + y *= rhs; + z *= rhs; + + return *this; +} + + +vector +vector::operator+ (const vector &rhs) const { + return { x + rhs.x, y + rhs.y, z + rhs.z }; +} + + +vector +vector::operator- (const vector &rhs) const + { return { x - rhs.x, y - rhs.y, z - rhs.z }; } + + +vector& +vector::operator-= (const vector &rhs) { + x -= rhs.x; + y -= rhs.y; + z -= rhs.z; + + return *this; +} + + +vector& +vector::operator+= (const vector &rhs) { + x += rhs.x; + y += rhs.y; + z += rhs.z; + + return *this; +} + + +vector& +vector::operator= (const vector &rhs) { + x = rhs.x; + y = rhs.y; + z = rhs.z; + + return *this; +} + + +double +vector::magnitude (void) const { + return sqrt (x * x + y * y + z * z); +} + + +double +vector::magnitude2 (void) const { + return x * x + y * y + z * z; +} + + +double +vector::dot (const vector &rhs) const { + return x * rhs.x + y * rhs.y + z * rhs.z; +} + + +vector +vector::cross (const vector &rhs) const { + return { y * rhs.z - z * rhs.y, + z * rhs.x - x * rhs.z, + x * rhs.y - y * rhs.x }; +} + + +vector& +vector::normalise (void) { + double mag = magnitude (); + + x /= mag; + y /= mag; + z /= mag; + + return *this; +} + + +vector +vector::normalised (void) const { + double mag = magnitude (); + return { x / mag, y / mag, z / mag }; +} + + +void +vector::sanity (void) const { + check_soft (!std::isnan (x)); + check_soft (!std::isnan (y)); + check_soft (!std::isnan (z)); +} + + +util::vector +operator* (double a, const util::vector &b) + { return b * a; } + + +std::ostream& +operator<< (std::ostream &os, const util::vector &v) { + os << "vec(" << v.x << ", " << v.y << ", " << v.z << ")"; + return os; +} + diff --git a/vector.hpp b/vector.hpp index 48d1a253..8cdd7bc3 100644 --- a/vector.hpp +++ b/vector.hpp @@ -20,42 +20,38 @@ #ifndef __UTIL_VECTOR_HPP #define __UTIL_VECTOR_HPP -#include -#include +#include -namespace maths { - class vector { - protected: - std::vector m_data; +namespace util { + struct vector { + double x, y, z; - public: - vector (const std::initializer_list &_data); - explicit - vector (unsigned int _size); - vector (const double *restrict _data, - unsigned int _size); + vector operator* (double) const; + vector& operator*=(double); - vector (const vector &rhs); - vector (const vector &&rhs); + vector operator+ (const vector&) const; + vector& operator+=(const vector&); - ~vector (void); + vector operator- (const vector&) const; + vector& operator-=(const vector&); - const double* data (void) const; - double& operator[] (unsigned int); - const double& operator[] (unsigned int) const; + vector& operator =(const vector &); - unsigned int size (void) const; + double magnitude (void) const; + double magnitude2 (void) const; + double dot (const vector&) const; + vector cross (const vector&) const; - static double dot (const double *restrict A, - const double *restrict B, - unsigned int size); + vector& normalise (void); + vector normalised (void) const; - static vector cross (const double *restrict A, - const double *restrict B, - unsigned int size); + void sanity (void) const; }; } +util::vector operator* (double, const util::vector&); +std::ostream& operator<< (std::ostream&, const util::vector&); #endif +