libcruft-util/vector.cpp

500 lines
14 KiB
C++
Raw Normal View History

/*
* 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.
2012-05-30 20:20:19 +10:00
*
* 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.
2012-05-30 20:20:19 +10:00
*
* You should have received a copy of the GNU General Public License
* along with libgim. If not, see <http://www.gnu.org/licenses/>.
*
2012-04-23 13:06:41 +10:00
* Copyright 2011 Danny Robson <danny@nerdcruft.net>
*/
2011-05-23 17:18:52 +10:00
#include "vector.hpp"
#include "debug.hpp"
#include "maths.hpp"
#include "random.hpp"
2011-05-23 17:18:52 +10:00
#include <algorithm>
#include <cmath>
#include <limits>
2011-05-23 17:18:52 +10:00
#include <numeric>
2012-06-13 15:50:17 +10:00
#if defined(COMPILER_GCC)
2012-05-26 18:02:11 +10:00
#pragma GCC optimize("-O3")
2012-06-13 15:50:17 +10:00
#endif
2011-05-23 17:18:52 +10:00
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
using namespace util;
2011-05-23 17:18:52 +10:00
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>
util::vector<S,T>::operator* (T rhs) const {
util::vector<S,T> out;
for (size_t i = 0; i < S; ++i)
out.data[i] = this->data[i] * rhs;
return out;
}
2011-05-23 17:18:52 +10:00
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>&
util::vector<S,T>::operator*= (T rhs) {
for (auto &i: this->data)
i *= rhs;
2011-05-23 17:18:52 +10:00
return *this;
}
2011-05-23 17:18:52 +10:00
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>
util::vector<S,T>::operator* (const vector<S,T> &rhs) const {
util::vector<S,T> out;
for (size_t i = 0; i < S; ++i)
out.data[i] = this->data[i] * rhs.data[i];
return out;
2011-10-24 15:17:15 +11:00
}
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>&
util::vector<S,T>::operator*= (const vector<S,T> &rhs) {
for (size_t i = 0; i < S; ++i)
this->data[i] *= rhs.data[i];
2011-10-24 15:17:15 +11:00
return *this;
}
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>
util::vector<S,T>::operator/ (T rhs) const {
util::vector<S,T> out;
2014-08-18 22:14:31 +10:00
2012-05-23 20:41:48 +10:00
for (size_t i = 0; i < S; ++i)
out.data[i] = this->data[i] / rhs;
return out;
}
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>&
util::vector<S,T>::operator/= (T rhs) {
2012-05-23 20:41:48 +10:00
for (size_t i = 0; i < S; ++i)
this->data[i] /= rhs;
return *this;
}
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>
util::vector<S,T>::operator+ (const util::vector<S,T> &rhs) const {
util::vector<S,T> out;
for (size_t i = 0; i < S; ++i)
out.data[i] = this->data[i] + rhs.data[i];
return out;
}
2011-05-23 17:18:52 +10:00
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>
util::vector<S,T>::operator+ (T rhs) const {
util::vector<S,T> out;
2012-05-26 18:02:11 +10:00
for (size_t i = 0; i < S; ++i)
out.data[i] = this->data[i] + rhs;
return out;
}
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>&
util::vector<S,T>::operator+= (const util::vector<S,T> &rhs) {
2012-05-26 18:02:11 +10:00
for (size_t i = 0; i < S; ++i)
this->data[i] += rhs.data[i];
return *this;
}
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>&
util::vector<S,T>::operator+= (T rhs) {
2012-05-26 18:02:11 +10:00
for (size_t i = 0; i < S; ++i)
this->data[i] += rhs;
return *this;
}
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>
util::vector<S,T>::operator- (void) const {
util::vector<S,T> out;
for (size_t i = 0; i < S; ++i)
out.data[i] = -this->data[i];
return out;
2011-10-24 15:17:15 +11:00
}
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>
util::vector<S,T>::operator- (const util::vector<S,T> &rhs) const {
util::vector<S,T> out;
for (size_t i = 0; i < S; ++i)
out.data[i] = this->data[i] - rhs.data[i];
return out;
}
2011-05-23 17:18:52 +10:00
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>&
util::vector<S,T>::operator-= (const util::vector<S,T> &rhs) {
for (size_t i = 0; i < S; ++i)
this->data[i] -= rhs.data[i];
2011-05-23 17:18:52 +10:00
return *this;
}
2011-05-23 17:18:52 +10:00
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>
util::vector<S,T>::operator- (T rhs) const {
util::vector<S,T> out;
for (size_t i = 0; i < S; ++i)
out.data[i] = this->data[i] - rhs;
return out;
}
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>&
util::vector<S,T>::operator-= (T rhs) {
for (size_t i = 0; i < S; ++i)
this->data[i] -= rhs;
return *this;
}
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>&
util::vector<S,T>::operator= (const util::vector<S,T> &rhs) {
2015-02-19 13:26:27 +11:00
std::copy (std::begin (rhs.data), std::end (rhs.data), std::begin (this->data));
2011-05-23 17:18:52 +10:00
return *this;
}
2011-05-23 17:18:52 +10:00
2015-02-19 14:20:01 +11:00
///////////////////////////////////////////////////////////////////////////////
template <size_t S, typename T>
2011-10-29 15:07:34 +11:00
bool
util::vector<S,T>::operator== (const util::vector<S,T> &rhs) const {
for (size_t i = 0; i < S; ++i)
if (!almost_equal (this->data[i], rhs.data[i]))
return false;
2011-10-29 15:07:34 +11:00
return true;
}
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
T
util::vector<S,T>::magnitude (void) const {
// TODO: this should not truncate for integral types
return static_cast<T> (std::sqrt (magnitude2 ()));
}
2011-05-23 17:18:52 +10:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
T
util::vector<S,T>::magnitude2 (void) const {
T total { 0 };
for (size_t i = 0; i < S; ++i)
total += pow2 (this->data[i]);
return total;
}
2011-05-23 17:18:52 +10:00
2015-01-28 15:00:20 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
T
util::vector<S,T>::difference (const util::vector<S,T> &rhs) const {
// TODO: change the signature to ensure it does not truncate
return static_cast<T> (std::sqrt (difference2 (rhs)));
}
//-----------------------------------------------------------------------------
template <size_t S, typename T>
T
util::vector<S,T>::difference2 (const util::vector<S,T> &rhs) const {
T sum {0};
for (size_t i = 0; i < S; ++i)
sum += pow2 (this->data[i] - rhs.data[i]);
return sum;
}
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>&
util::vector<S,T>::normalise (void) {
T mag = magnitude ();
2011-05-23 17:18:52 +10:00
for (size_t i = 0; i < S; ++i)
this->data[i] /= mag;
2011-05-23 17:18:52 +10:00
return *this;
2011-05-23 17:18:52 +10:00
}
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
util::vector<S,T>
util::vector<S,T>::normalised (void) const {
T mag = magnitude ();
util::vector<S,T> out;
2014-08-18 22:14:31 +10:00
for (size_t i = 0; i < S; ++i)
out.data[i] = this->data[i] / mag;
return out;
}
//-----------------------------------------------------------------------------
template <typename T>
util::vector<2,T>
util::polar_to_cartesian (const util::vector<2,T> &v) {
return util::vector<2,T> {
v.r * std::cos (v.t),
v.r * std::sin (v.t)
};
}
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
T
util::vector<S,T>::dot (const util::vector<S,T> &rhs) const {
T total { 0 };
2012-05-26 18:02:11 +10:00
for (size_t i = 0; i < S; ++i)
total += this->data[i] * rhs.data[i];
return total;
}
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <typename T>
util::vector<3,T>
util::cross (const util::vector<3,T> &a,
const util::vector<3,T> &b)
{
return util::vector<3,T> {
a.y * b.z - a.z * b.y,
a.z * b.x - a.x * b.z,
a.x * b.y - a.y * b.x
};
2012-05-26 18:02:11 +10:00
}
template util::vector3f util::cross(const util::vector3f&, const util::vector3f&);
template util::vector3d util::cross(const util::vector3d&, const util::vector3d&);
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
template <typename T>
util::vector<3,T>
util::spherical_to_cartesian (const util::vector<3,T> &s) {
return util::vector<3,T> {
2011-10-29 23:14:07 +11:00
s.x * sin (s.y) * cos (s.z),
s.x * sin (s.y) * sin (s.z),
s.x * cos (s.y),
};
}
2015-02-19 14:20:01 +11:00
//-----------------------------------------------------------------------------
template <typename T>
util::vector<3,T>
util::cartesian_to_spherical (const util::vector<3,T> &c) {
T mag = c.magnitude ();
2011-10-29 23:14:07 +11:00
return util::vector<3,T> {
2011-10-29 23:14:07 +11:00
mag,
acos (c.z / mag),
atan2 (c.y, c.x)
};
}
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
2012-05-03 15:56:56 +10:00
bool
util::vector<S,T>::is_zero (void) const {
2015-02-19 13:26:27 +11:00
return std::all_of (std::begin (this->data),
std::end (this->data),
[] (T i) { return almost_zero (i); });
2012-05-03 15:56:56 +10:00
}
2015-01-15 14:01:51 +11:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
const util::vector<S,T>
util::vector<S,T>::ZERO (T{0});
//-----------------------------------------------------------------------------
template <size_t S, typename T>
void
util::vector<S,T>::sanity (void) const {
2015-02-19 13:26:27 +11:00
CHECK (std::all_of (std::begin (this->data),
std::end (this->data),
[] (T i) { return !std::isnan (i); }));
}
2015-02-19 13:26:27 +11:00
///////////////////////////////////////////////////////////////////////////////
template <size_t S, typename T, typename U>
util::vector<S,T>
util::operator* (U a, const util::vector<S,T> &b)
{
return b * T(a);
}
//-----------------------------------------------------------------------------
template <size_t S, typename T, typename U>
util::vector<S,T>
util::operator+ (U a, const util::vector<S,T> &b)
{
return b + T(a);
}
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
template <size_t S, typename T, typename U>
util::vector<S,T>
util::operator- (U a, const util::vector<S,T> &b)
{
return a + (-b);
}
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
2015-02-19 13:27:06 +11:00
template <size_t S, typename T>
T
util::dot (vector<S,T> a, vector<S,T> b)
{
return std::inner_product (std::begin (a),
std::end (a),
std::begin (b),
T{0});
}
///////////////////////////////////////////////////////////////////////////////
// ostream
template <size_t S, typename T>
std::ostream&
util::operator<< (std::ostream &os, const util::vector<S,T> &v) {
os << "vec" << S << "(" << v.data[0];
2012-06-14 18:32:17 +10:00
for (size_t i = 1; i < S; ++i)
os << ", " << v.data[i];
os << ")";
return os;
}
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
template <size_t S, typename T>
const json::tree::node&
util::operator>> (const json::tree::node &node, util::vector<S,T> &v) {
const json::tree::array &array = node.as_array ();
2014-08-18 22:14:31 +10:00
if (array.size () != S)
throw std::runtime_error ("Invalid dimensionality for json-to-vector");
// XXX: This used to be a std::transform but gcc 4.9.0 hit an internal
// compiler error at this point in release mode, so we dumb it down a
// little.
for (size_t i = 0; i < array.size (); ++i)
v.data[i] = static_cast<T> (array[i].as_number ().native ());
return node;
}
//-----------------------------------------------------------------------------
#define INSTANTIATE_S_T(S,T) \
template struct util::vector<S,T>; \
template util::vector<S,T> util::operator* (int, const util::vector<S,T>&); \
template util::vector<S,T> util::operator* (unsigned, const util::vector<S,T>&); \
template util::vector<S,T> util::operator* (float, const util::vector<S,T>&); \
template util::vector<S,T> util::operator+ (T, const util::vector<S,T>&); \
template util::vector<S,T> util::operator- (T, const util::vector<S,T>&); \
2015-02-19 13:27:06 +11:00
template T util::dot (util::vector<S,T>, util::vector<S,T>); \
template std::ostream& util::operator<< (std::ostream&, const util::vector<S,T> &v);\
template const json::tree::node& util::operator>> (const json::tree::node&, util::vector<S,T>&);
#define INSTANTIATE(T) \
INSTANTIATE_S_T(1,T) \
INSTANTIATE_S_T(2,T) \
INSTANTIATE_S_T(3,T) \
INSTANTIATE_S_T(4,T)
INSTANTIATE(uint32_t)
INSTANTIATE(int32_t)
INSTANTIATE(uint64_t)
INSTANTIATE(int64_t)
INSTANTIATE(float)
INSTANTIATE(double)
2012-05-26 18:02:11 +10:00
//-----------------------------------------------------------------------------
namespace util {
template <> vector<1,float> random (void) { util::vector<1,float> out; randomise (out.data); return out; }
template <> vector<2,float> random (void) { util::vector<2,float> out; randomise (out.data); return out; }
template <> vector<3,float> random (void) { util::vector<3,float> out; randomise (out.data); return out; }
template <> vector<4,float> random (void) { util::vector<4,float> out; randomise (out.data); return out; }
template <> vector<1,double> random (void) { util::vector<1,double> out; randomise (out.data); return out; }
template <> vector<2,double> random (void) { util::vector<2,double> out; randomise (out.data); return out; }
template <> vector<3,double> random (void) { util::vector<3,double> out; randomise (out.data); return out; }
template <> vector<4,double> random (void) { util::vector<4,double> out; randomise (out.data); return out; }
}