libcruft-util/matrix.cpp

360 lines
9.8 KiB
C++
Raw Normal View History

/*
2015-04-13 18:05:28 +10:00
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
2015-04-13 18:05:28 +10:00
* http://www.apache.org/licenses/LICENSE-2.0
2014-08-18 22:14:31 +10:00
*
2015-04-13 18:05:28 +10:00
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
2015-10-30 23:40:13 +11:00
* Copyright 2011-2015 Danny Robson <danny@nerdcruft.net>
*/
2011-05-23 17:18:52 +10:00
#include "matrix.hpp"
#include "debug.hpp"
#include "iterator.hpp"
#include "point.hpp"
2011-05-23 17:18:52 +10:00
#include <cstring>
2014-12-15 13:44:33 +11:00
#include <cmath>
2011-05-23 17:18:52 +10:00
2017-02-21 21:19:28 +11:00
using util::matrix;
2011-05-23 17:18:52 +10:00
///////////////////////////////////////////////////////////////////////////////
2014-08-19 20:45:28 +10:00
//-----------------------------------------------------------------------------
//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;
//}
2014-08-19 20:45:28 +10:00
//-----------------------------------------------------------------------------
template <std::size_t Rows, std::size_t Cols, typename T>
T
2017-02-21 21:19:28 +11:00
util::matrix<Rows,Cols,T>::determinant (void) const
{
return util::determinant (*this);
2011-05-23 17:18:52 +10:00
}
2014-08-19 20:45:28 +10:00
//-----------------------------------------------------------------------------
template <std::size_t Rows, std::size_t Cols, typename T>
2017-02-21 21:19:28 +11:00
util::matrix<Rows,Cols,T>
util::matrix<Rows,Cols,T>::inverse (void) const
{
return util::inverse (*this);
2014-08-19 20:45:28 +10:00
}
2016-09-14 17:46:03 +10:00
///////////////////////////////////////////////////////////////////////////////
template <std::size_t Rows, std::size_t Cols, typename T>
2017-02-21 21:19:28 +11:00
matrix<Cols,Rows,T>
util::transposed (const matrix<Rows,Cols,T> &m)
2016-09-14 17:46:03 +10:00
{
2017-02-21 21:19:28 +11:00
util::matrix<Cols,Rows,T> res;
2016-09-14 17:46:03 +10:00
for (std::size_t y = 0; y < Rows; ++y)
for (std::size_t x = 0; x < Cols; ++x)
2016-09-14 17:46:03 +10:00
res[y][x] = m[x][y];
return res;
}
//-----------------------------------------------------------------------------
2017-02-21 21:19:28 +11:00
template util::matrix3f util::transposed (const matrix3f&);
template util::matrix4f util::transposed (const matrix4f&);
///////////////////////////////////////////////////////////////////////////////
template <std::size_t Rows, std::size_t Cols, typename T>
2011-05-23 17:18:52 +10:00
bool
2017-02-21 21:19:28 +11:00
matrix<Rows,Cols,T>::is_affine (void) const
{
2017-02-21 21:19:28 +11:00
if (Rows != Cols)
return false;
for (std::size_t i = 0; i < Rows - 1; ++i)
2017-02-21 21:19:28 +11:00
if (!exactly_zero (values[Rows-1][i]))
return false;
return equal (values[Rows-1][Rows-1], T{1});
2011-05-23 17:18:52 +10:00
}
2014-12-15 13:44:33 +11:00
//-----------------------------------------------------------------------------
template <typename T>
2017-02-21 21:19:28 +11:00
util::matrix4<T>
util::ortho (T l, T r, // left, right
T b, T t, // bottom, top
T n, T f) // near, far
2014-12-15 13:44:33 +11:00
{
CHECK_GT (f, n);
2014-12-15 13:44:33 +11:00
T tx = - (r + l) / (r - l);
T ty = - (t + b) / (t - b);
T tz = - (f + n) / (f - n);
2014-12-15 13:44:33 +11:00
T rl = 2 / (r - l);
T tb = 2 / (t - b);
T fn = -2 / (f - n);
2014-12-15 13:44:33 +11:00
return {{
2014-12-15 13:44:33 +11:00
{ rl, 0, 0, tx },
{ 0, tb, 0, ty },
{ 0, 0, fn, tz },
{ 0, 0, 0, 1 },
}};
2014-12-15 13:44:33 +11:00
}
2018-03-06 13:53:57 +11:00
template util::matrix4f util::ortho (float, float, float, float, float, float);
2014-12-15 13:44:33 +11:00
//-----------------------------------------------------------------------------
template <typename T>
2017-02-21 21:19:28 +11:00
util::matrix4<T>
util::ortho2D (T left, T right,
T bottom, T top)
2014-12-15 13:44:33 +11:00
{
return ortho (left, right, bottom, top, T{-1}, T{1});
2014-12-15 13:44:33 +11:00
}
template util::matrix4f util::ortho2D (float, float, float, float);
2014-12-15 13:44:33 +11:00
//-----------------------------------------------------------------------------
template <typename T>
2017-02-21 21:19:28 +11:00
util::matrix4<T>
util::perspective (T fov, T aspect, range<T> Z)
2014-12-15 13:44:33 +11:00
{
CHECK_LIMIT (fov, 0, 2 * util::pi<T>);
CHECK_GE (Z.lo, 0);
CHECK_GE (Z.hi, 0);
2014-12-15 13:44:33 +11:00
T f = cos (T{0.5} * fov) / sin (T{0.5} * fov);
2016-10-11 20:56:41 +11:00
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);
2014-12-15 13:44:33 +11:00
return { {
2016-10-11 20:56:41 +11:00
{ x, 0, 0, 0 },
{ 0, y, 0, 0 },
2014-12-15 13:44:33 +11:00
{ 0, 0, z1, z2 },
{ 0, 0, -1, 0 }
} };
}
template util::matrix4f util::perspective<float> (float, float, range<float>);
2014-12-15 13:44:33 +11:00
//-----------------------------------------------------------------------------
// 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>
2017-02-21 21:19:28 +11:00
util::matrix4<T>
util::look_at (const util::point<3,T> eye,
const util::point<3,T> centre,
const util::vector<3,T> up)
2014-12-15 13:44:33 +11:00
{
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 util::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 * util::translation (0-eye);
2014-12-15 13:44:33 +11:00
}
template util::matrix4f util::look_at (util::point3f, util::point3f, util::vector3f);
//-----------------------------------------------------------------------------
//template <typename T>
//util::matrix4<T>
//util::translation (util::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 util::matrix4f util::translation (util::vector3f);
2015-01-19 19:12:44 +11:00
//-----------------------------------------------------------------------------
#if 0
template <typename T>
2017-02-21 21:19:28 +11:00
util::matrix4<T>
util::scale (T mag)
2015-01-19 19:12:44 +11:00
{
return scale (vector<3,T> (mag));
}
template util::matrix4f util::scale(float);
#endif
//-----------------------------------------------------------------------------
//template <typename T>
//util::matrix4<T>
//util::scale (util::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 util::matrix4f util::scale(util::vector3f);
2014-12-30 01:32:02 +11:00
//-----------------------------------------------------------------------------
template <typename T>
2017-02-21 21:19:28 +11:00
util::matrix4<T>
util::rotation (T angle, util::vector<3,T> about)
2014-12-30 01:32:02 +11:00
{
CHECK (is_normalised (about));
2014-12-30 01:32:02 +11:00
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 util::matrix4f util::rotation (float, util::vector3f);
2011-05-23 17:18:52 +10:00
2014-07-07 15:17:45 +10:00
//-----------------------------------------------------------------------------
2017-02-21 21:19:28 +11:00
template struct util::matrix<2,2,float>;
template struct util::matrix<2,2,double>;
2017-02-21 21:19:28 +11:00
template struct util::matrix<3,3,float>;
template struct util::matrix<3,3,double>;
2017-02-21 21:19:28 +11:00
template struct util::matrix<4,4,float>;
template struct util::matrix<4,4,double>;
2011-05-23 17:18:52 +10:00
2016-09-14 17:53:34 +10:00
///////////////////////////////////////////////////////////////////////////////
// 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>
2016-09-14 17:53:34 +10:00
util::vector<3,T>
2017-02-21 21:19:28 +11:00
util::to_euler (const matrix<Rows,Cols,T> &m)
2016-09-14 17:53:34 +10:00
{
2017-02-21 21:19:28 +11:00
static_assert (Rows == Cols && (Rows == 3 || Rows == 4),
"only defined for 3d affine transforms");
2016-09-14 17:53:34 +10:00
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 };
}
//-----------------------------------------------------------------------------
2017-02-21 21:19:28 +11:00
template util::vector<3,float> util::to_euler (const matrix<3,3,float>&);
template util::vector<3,float> util::to_euler (const matrix<4,4,float>&);
2016-09-14 17:53:34 +10:00