Danny Robson
fdaa5e1392
LIMIT hid an off-by-one bug when tests used end iterators. We rename the assertion to uncover all uses of the flawed implementation, and split it into an identical assertion, and one intended to protect against iterator ends.
352 lines
9.5 KiB
C++
352 lines
9.5 KiB
C++
/*
|
|
* This Source Code Form is subject to the terms of the Mozilla Public
|
|
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
|
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
|
*
|
|
* Copyright 2011-2015 Danny Robson <danny@nerdcruft.net>
|
|
*/
|
|
|
|
#include "matrix.hpp"
|
|
|
|
#include "debug/assert.hpp"
|
|
#include "iterator/infix.hpp"
|
|
#include "point.hpp"
|
|
|
|
#include <cstring>
|
|
#include <cmath>
|
|
|
|
using cruft::matrix;
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
//-----------------------------------------------------------------------------
|
|
//template <std::size_t S, typename T>
|
|
//matrix<S,T>&
|
|
//matrix<S,T>::invert_affine (void)
|
|
//{
|
|
// CHECK (is_affine ());
|
|
//
|
|
// // inv ([ M b ] == [ inv(M) -inv(M).b ]
|
|
// // [ 0 1 ]) [ 0 1 ]
|
|
//
|
|
// // Invert the 3x3 M
|
|
// T A = (values[1][1] * values[2][2] - values[1][2] * values[2][1]);
|
|
// T B = (values[1][2] * values[2][0] - values[1][0] * values[2][2]);
|
|
// T C = (values[1][0] * values[2][1] - values[1][1] * values[2][0]);
|
|
//
|
|
// T D = (values[0][2] * values[2][1] - values[0][1] * values[2][2]);
|
|
// T E = (values[0][0] * values[2][2] - values[0][2] * values[2][0]);
|
|
// T F = (values[2][0] * values[0][1] - values[0][0] * values[2][1]);
|
|
//
|
|
// T G = (values[0][1] * values[1][2] - values[0][2] * values[1][1]);
|
|
// T H = (values[0][2] * values[1][0] - values[0][0] * values[1][2]);
|
|
// T K = (values[0][0] * values[1][1] - values[0][1] * values[1][0]);
|
|
//
|
|
// T d = values[0][0] * A + values[0][1] * B + values[0][2] * C;
|
|
// CHECK_NEQ (d, 0.0);
|
|
//
|
|
// values[0][0] = A / d;
|
|
// values[0][1] = D / d;
|
|
// values[0][2] = G / d;
|
|
// values[1][0] = B / d;
|
|
// values[1][1] = E / d;
|
|
// values[1][2] = H / d;
|
|
// values[2][0] = C / d;
|
|
// values[2][1] = F / d;
|
|
// values[2][2] = K / d;
|
|
//
|
|
// // Multiply the b
|
|
// T b0 = - values[0][0] * values[0][3] - values[0][1] * values[1][3] - values[0][2] * values[2][3];
|
|
// T b1 = - values[1][0] * values[0][3] - values[1][1] * values[1][3] - values[1][2] * values[2][3];
|
|
// T 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;
|
|
//}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
T
|
|
cruft::matrix<Rows,Cols,T>::determinant (void) const
|
|
{
|
|
return cruft::determinant (*this);
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
cruft::matrix<Rows,Cols,T>
|
|
cruft::matrix<Rows,Cols,T>::inverse (void) const
|
|
{
|
|
return cruft::inverse (*this);
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
matrix<Cols,Rows,T>
|
|
cruft::transposed (const matrix<Rows,Cols,T> &m)
|
|
{
|
|
cruft::matrix<Cols,Rows,T> res;
|
|
|
|
for (std::size_t y = 0; y < Rows; ++y)
|
|
for (std::size_t x = 0; x < Cols; ++x)
|
|
res[y][x] = m[x][y];
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template cruft::matrix3f cruft::transposed (const matrix3f&);
|
|
template cruft::matrix4f cruft::transposed (const matrix4f&);
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
bool
|
|
matrix<Rows,Cols,T>::is_affine (void) const
|
|
{
|
|
if (Rows != Cols)
|
|
return false;
|
|
|
|
for (std::size_t i = 0; i < Rows - 1; ++i)
|
|
if (!exactly_zero (values[Rows-1][i]))
|
|
return false;
|
|
|
|
return equal (values[Rows-1][Rows-1], T{1});
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template <typename T>
|
|
cruft::matrix4<T>
|
|
cruft::ortho (T l, T r, // left, right
|
|
T b, T t, // bottom, top
|
|
T n, T f) // near, far
|
|
{
|
|
CHECK_GT (f, n);
|
|
|
|
T tx = - (r + l) / (r - l);
|
|
T ty = - (t + b) / (t - b);
|
|
T tz = - (f + n) / (f - n);
|
|
|
|
T rl = 2 / (r - l);
|
|
T tb = 2 / (t - b);
|
|
T fn = -2 / (f - n);
|
|
|
|
return {{
|
|
{ rl, 0, 0, tx },
|
|
{ 0, tb, 0, ty },
|
|
{ 0, 0, fn, tz },
|
|
{ 0, 0, 0, 1 },
|
|
}};
|
|
}
|
|
|
|
template cruft::matrix4f cruft::ortho (float, float, float, float, float, float);
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template <typename T>
|
|
cruft::matrix4<T>
|
|
cruft::ortho2D (T left, T right,
|
|
T bottom, T top)
|
|
{
|
|
return ortho (left, right, bottom, top, T{-1}, T{1});
|
|
}
|
|
|
|
template cruft::matrix4f cruft::ortho2D (float, float, float, float);
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template <typename T>
|
|
cruft::matrix4<T>
|
|
cruft::perspective (T fov, T aspect, range<T> Z)
|
|
{
|
|
CHECK_INCLUSIVE (fov, 0, 2 * cruft::pi<T>);
|
|
CHECK_GE (Z.lo, 0);
|
|
CHECK_GE (Z.hi, 0);
|
|
|
|
T f = cos (T{0.5} * fov) / sin (T{0.5} * fov);
|
|
|
|
T x = f / aspect;
|
|
T y = f;
|
|
T z1 = (Z.hi + Z.lo) / (Z.lo - Z.hi);
|
|
T z2 = (2 * Z.hi * Z.lo) / (Z.lo - Z.hi);
|
|
|
|
return { {
|
|
{ x, 0, 0, 0 },
|
|
{ 0, y, 0, 0 },
|
|
{ 0, 0, z1, z2 },
|
|
{ 0, 0, -1, 0 }
|
|
} };
|
|
}
|
|
|
|
template cruft::matrix4f cruft::perspective<float> (float, float, range<float>);
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Emulates gluLookAt
|
|
//
|
|
// Translates the viewpoint to the origin, then rotates the world to point
|
|
// along eye to centre (negative-Z).
|
|
// Implemented for right handed world coordinates.
|
|
//
|
|
// Assumes 'up' is normalised.
|
|
template <typename T>
|
|
cruft::matrix4<T>
|
|
cruft::look_at (const cruft::point<3,T> eye,
|
|
const cruft::point<3,T> centre,
|
|
const cruft::vector<3,T> up)
|
|
{
|
|
CHECK (is_normalised (up));
|
|
|
|
const auto forward = normalised (centre - eye);
|
|
const auto side = normalised (cross (forward, up));
|
|
const auto newup = cross (side, forward);
|
|
|
|
const auto &f = forward, &s = side, &u = newup;
|
|
|
|
const cruft::matrix4<T> rot {{
|
|
{ s.x, s.y, s.z, 0 },
|
|
{ u.x, u.y, u.z, 0 },
|
|
{-f.x,-f.y,-f.z, 0 },
|
|
{ 0, 0, 0, 1 },
|
|
}};
|
|
|
|
return rot * cruft::translation (0-eye);
|
|
}
|
|
|
|
template cruft::matrix4f cruft::look_at (cruft::point3f, cruft::point3f, cruft::vector3f);
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//template <typename T>
|
|
//cruft::matrix4<T>
|
|
//cruft::translation (cruft::vector<3,T> v)
|
|
//{
|
|
// return { {
|
|
// { 1.f, 0.f, 0.f, v.x },
|
|
// { 0.f, 1.f, 0.f, v.y },
|
|
// { 0.f, 0.f, 1.f, v.z },
|
|
// { 0.f, 0.f, 0.f, 1.f },
|
|
// } };
|
|
//}
|
|
//
|
|
//
|
|
//template cruft::matrix4f cruft::translation (cruft::vector3f);
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
#if 0
|
|
template <typename T>
|
|
cruft::matrix4<T>
|
|
cruft::scale (T mag)
|
|
{
|
|
return scale (vector<3,T> (mag));
|
|
}
|
|
|
|
template cruft::matrix4f cruft::scale(float);
|
|
#endif
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//template <typename T>
|
|
//cruft::matrix4<T>
|
|
//cruft::scale (cruft::vector<3,T> v)
|
|
//{
|
|
// return { {
|
|
// { v.x, 0.f, 0.f, 0.f },
|
|
// { 0.f, v.y, 0.f, 0.f },
|
|
// { 0.f, 0.f, v.z, 0.f },
|
|
// { 0.f, 0.f, 0.f, 1.f }
|
|
// } };
|
|
//}
|
|
|
|
//template cruft::matrix4f cruft::scale(cruft::vector3f);
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template <typename T>
|
|
cruft::matrix4<T>
|
|
cruft::rotation (T angle, cruft::vector<3,T> about)
|
|
{
|
|
CHECK (is_normalised (about));
|
|
|
|
T c = std::cos (angle);
|
|
T s = std::sin (angle);
|
|
T x = about.x,
|
|
y = about.y,
|
|
z = about.z;
|
|
|
|
return { {
|
|
{ x * x * (1 - c) + c,
|
|
x * y * (1 - c) - z * s,
|
|
x * z * (1 - c) + y * s,
|
|
0
|
|
},
|
|
|
|
{ y * x * (1 - c) + z * s,
|
|
y * y * (1 - c) + c,
|
|
y * z * (1 - c) - x * s,
|
|
0
|
|
},
|
|
|
|
{ z * x * (1 - c) - y * s,
|
|
z * y * (1 - c) + x * s,
|
|
z * z * (1 - c) + c,
|
|
0
|
|
},
|
|
|
|
{ 0, 0, 0, 1 }
|
|
} };
|
|
}
|
|
|
|
template cruft::matrix4f cruft::rotation (float, cruft::vector3f);
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template struct cruft::matrix<2,2,float>;
|
|
template struct cruft::matrix<2,2,double>;
|
|
|
|
template struct cruft::matrix<3,3,float>;
|
|
template struct cruft::matrix<3,3,double>;
|
|
|
|
template struct cruft::matrix<4,4,float>;
|
|
template struct cruft::matrix<4,4,double>;
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
// Uses the algorithm from:
|
|
// "Extracting Euler Angles from a Rotation Matrix" by
|
|
// Mike Day, Insomniac Games.
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
cruft::vector<3,T>
|
|
cruft::to_euler (const matrix<Rows,Cols,T> &m)
|
|
{
|
|
static_assert (Rows == Cols && (Rows == 3 || Rows == 4),
|
|
"only defined for 3d affine transforms");
|
|
|
|
const auto theta0 = std::atan2 (m[2][1], m[2][2]);
|
|
|
|
const auto c1 = std::hypot (m[0][0], m[1][0]);
|
|
const auto theta1 = std::atan2 (-m[2][0], c1);
|
|
|
|
const auto s0 = std::sin(theta0);
|
|
const auto c0 = std::cos(theta0);
|
|
const auto theta2 = std::atan2(
|
|
s0 * m[0][2] - c0 * m[0][1],
|
|
c0 * m[1][1] - s0 * m[1][2]
|
|
);
|
|
|
|
return { theta0, theta1, theta2 };
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template cruft::vector<3,float> cruft::to_euler (const matrix<3,3,float>&);
|
|
template cruft::vector<3,float> cruft::to_euler (const matrix<4,4,float>&);
|
|
|