181 lines
6.8 KiB
C++
181 lines
6.8 KiB
C++
/* Copyright (C) 2017 Eric Wasylishen
|
|
|
|
This program 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 2 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program 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 this program; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
|
|
See file, 'COPYING', for details.
|
|
*/
|
|
|
|
#include <common/qvec.hh>
|
|
|
|
#include <cmath> // for NAN
|
|
#include <fmt/core.h>
|
|
|
|
namespace qv
|
|
{
|
|
// explicit specialization, for reducing compile times
|
|
template<>
|
|
[[nodiscard]] std::string to_string(const qvec<double, 3> &v1)
|
|
{
|
|
return fmt::format("{}", v1);
|
|
}
|
|
|
|
// explicit specialization, for reducing compile times
|
|
template<>
|
|
[[nodiscard]] std::string to_string(const qvec<int, 3> &v1)
|
|
{
|
|
return fmt::format("{}", v1);
|
|
}
|
|
}
|
|
|
|
/*
|
|
* SGI FREE SOFTWARE LICENSE B (Version 2.0, Sept. 18, 2008)
|
|
* Copyright (C) 1991-2000 Silicon Graphics, Inc. All Rights Reserved.
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a
|
|
* copy of this software and associated documentation files (the "Software"),
|
|
* to deal in the Software without restriction, including without limitation
|
|
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
|
|
* and/or sell copies of the Software, and to permit persons to whom the
|
|
* Software is furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice including the dates of first publication and
|
|
* either this permission notice or a reference to
|
|
* http://oss.sgi.com/projects/FreeB/
|
|
* shall be included in all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
|
|
* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
|
|
* SILICON GRAPHICS, INC. BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
|
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
|
|
* OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*
|
|
* Except as contained in this notice, the name of Silicon Graphics, Inc.
|
|
* shall not be used in advertising or otherwise to promote the sale, use or
|
|
* other dealings in this Software without prior written authorization from
|
|
* Silicon Graphics, Inc.
|
|
*/
|
|
/*
|
|
** Invert 4x4 matrix.
|
|
** Contributed by David Moore (See Mesa bug #6748)
|
|
*/
|
|
static bool gluInvertMatrixd(const std::array<double, 16> &m, std::array<double, 16> &invOut)
|
|
{
|
|
double inv[16], det;
|
|
int i;
|
|
|
|
inv[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] +
|
|
m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
|
|
inv[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] -
|
|
m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
|
|
inv[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] +
|
|
m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
|
|
inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] -
|
|
m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
|
|
inv[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] -
|
|
m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
|
|
inv[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] +
|
|
m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
|
|
inv[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] -
|
|
m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
|
|
inv[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] +
|
|
m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
|
|
inv[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] +
|
|
m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
|
|
inv[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] -
|
|
m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
|
|
inv[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] +
|
|
m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
|
|
inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] -
|
|
m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
|
|
inv[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] -
|
|
m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
|
|
inv[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] +
|
|
m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
|
|
inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] -
|
|
m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
|
|
inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] -
|
|
m[8] * m[2] * m[5];
|
|
|
|
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
|
|
if (det == 0)
|
|
return false;
|
|
|
|
det = 1.0 / det;
|
|
|
|
for (i = 0; i < 16; i++)
|
|
invOut[i] = inv[i] * det;
|
|
|
|
return true;
|
|
}
|
|
|
|
qmat4x4d qv::inverse(const qmat4x4d &input)
|
|
{
|
|
qmat4x4d res;
|
|
bool ok = gluInvertMatrixd(input.m_values, res.m_values);
|
|
if (!ok) {
|
|
res = qmat4x4d(NAN);
|
|
}
|
|
return res;
|
|
}
|
|
|
|
qmat4x4f qv::inverse(const qmat4x4f &input)
|
|
{
|
|
return qmat4x4f(qv::inverse(qmat4x4d(input)));
|
|
}
|
|
|
|
qmat3x3f qv::inverse(const qmat3x3f &m)
|
|
{
|
|
qmat4x4d temp {};
|
|
for (int r = 0; r < 3; ++r) {
|
|
for (int c = 0; c < 3; ++c) {
|
|
temp.at(r,c) = m.at(r,c);
|
|
}
|
|
}
|
|
|
|
temp = qv::inverse(temp);
|
|
|
|
// return the upper-left 3x3 matrix
|
|
qmat3x3f result;
|
|
for (int r = 0; r < 3; ++r) {
|
|
for (int c = 0; c < 3; ++c) {
|
|
result.at(r,c) = temp.at(r,c);
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
qmat2x2f qv::inverse(const qmat2x2f &m)
|
|
{
|
|
// http://www.mathwords.com/i/inverse_of_a_matrix.htm
|
|
float a = m.at(0, 0);
|
|
float b = m.at(0, 1);
|
|
float c = m.at(1, 0);
|
|
float d = m.at(1, 1);
|
|
|
|
float det = a * d - b * c;
|
|
if (det == 0) {
|
|
return qmat2x2f(NAN);
|
|
}
|
|
|
|
qmat2x2f result{
|
|
d, -c, // col 0
|
|
-b, a // col 1
|
|
};
|
|
|
|
return result * (1.0f / det);
|
|
}
|