148 lines
5.7 KiB
C++
148 lines
5.7 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 <sstream>
|
|
|
|
/*
|
|
* 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 double m[16], double invOut[16])
|
|
{
|
|
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::invert(const qmat4x4d &input, bool *ok)
|
|
{
|
|
qmat4x4d res;
|
|
*ok = gluInvertMatrixd(input.m_values, res.m_values);
|
|
return res;
|
|
}
|
|
|
|
qmat4x4f qv::invert(const qmat4x4f &input, bool *ok)
|
|
{
|
|
return qmat4x4f(qv::invert(qmat4x4d(input), ok));
|
|
}
|
|
|
|
qmat2x2f qv::invert(const qmat2x2f &m, bool *ok)
|
|
{
|
|
// 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) {
|
|
*ok = false;
|
|
return qmat2x2f();
|
|
}
|
|
|
|
*ok = true;
|
|
qmat2x2f result {
|
|
d, -c, // col 0
|
|
-b, a // col 1
|
|
};
|
|
|
|
return result * (1.0f/det);
|
|
}
|
|
|
|
std::string qv::to_string(const qvec<3,float> &v1)
|
|
{
|
|
std::stringstream ss;
|
|
ss << v1[0] << " " << v1[1] << " " << v1[2];
|
|
return ss.str();
|
|
}
|