common: add 4x4 (from mesa) and 2x2 matrix inverse

bsputil/CMakeLists.txt

@@ -7,6 +7,7 @@ set(BSPUTIL_SOURCES
 	${CMAKE_SOURCE_DIR}/common/bspfile.cc
 	${CMAKE_SOURCE_DIR}/common/bsputils.cc
 	${CMAKE_SOURCE_DIR}/common/mathlib.cc
+	${CMAKE_SOURCE_DIR}/common/qvec.cc
 	${CMAKE_SOURCE_DIR}/common/polylib.cc
 	${CMAKE_SOURCE_DIR}/common/log.cc
 	${CMAKE_SOURCE_DIR}/common/threads.cc

common/qvec.cc

@@ -0,0 +1,150 @@
+/*  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>
+
+/*
+ * 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)
+{
+    double flat_in[16];
+    double flat_out[16];
+    
+    for (int i=0; i<4; i++)
+        for (int j=0; j<4; j++)
+            flat_in[4*i+j] = input[i][j];
+    
+    *ok = gluInvertMatrixd(flat_in, flat_out);
+    if (!*ok)
+        return qmat4x4d();
+    
+    qmat4x4d result;
+    for (int i=0; i<4; i++)
+        for (int j=0; j<4; j++)
+            result[i][j] = flat_out[4*i+j];
+    return result;
+}
+
+qmat4x4f qv::invert(const qmat4x4f &input, bool *ok)
+{
+    return qmat4x4f(qv::invert(qmat4x4d(input), ok));
+}
+
+qmat2x2f qv::invert(const qmat2x2f &m, bool *ok)
+{
+    float a = m[0][0];
+    float b = m[1][0];
+    float c = m[0][1];
+    float d = m[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);
+}

include/common/qvec.hh

@@ -267,6 +267,15 @@ public:
             this->m_cols[i] = other.m_cols[i];
     }
 
+    /**
+     * Casting from another matrix type of the same size
+     */
+    template <class T2>
+    qmat(const qmat<M, N, T2> &other) {
+        for (int i=0; i<N; i++)
+            m_cols[i] = qvec<M, T>(other[i]);
+    }
+
     // initializer list
     qmat(std::initializer_list<T> list) {
         assert(list.size() == M*N);
@@ -324,6 +333,16 @@ public:
         }
         return res;
     }
+    
+    // multiplication by a scalar
+    
+    qmat<M,N,T> operator*(const T scalar) const {
+        qmat<M,N,T> res;
+        for (int j=0; j<N; j++) {
+            res[j] = this->m_cols[j] * scalar;
+        }
+        return res;
+    }
 };
 
 using qmat2x2f = qmat<2, 2, float>;
@@ -351,4 +370,11 @@ using qmat4x2d = qmat<4, 2, double>;
 using qmat4x3d = qmat<4, 3, double>;
 using qmat4x4d = qmat<4, 4, double>;
 
+namespace qv {
+    qmat4x4f invert(const qmat4x4f &input, bool *ok);
+    qmat4x4d invert(const qmat4x4d &input, bool *ok);
+    
+    qmat2x2f invert(const qmat2x2f &input, bool *ok);
+};
+
 #endif /* __COMMON_QVEC_HH__ */

light/CMakeLists.txt

@@ -23,6 +23,7 @@ set(LIGHT_SOURCES
 	${CMAKE_SOURCE_DIR}/common/bspfile.cc	
 	${CMAKE_SOURCE_DIR}/common/cmdlib.cc
 	${CMAKE_SOURCE_DIR}/common/mathlib.cc
+	${CMAKE_SOURCE_DIR}/common/qvec.cc
 	${CMAKE_SOURCE_DIR}/common/mesh.cc
 	${CMAKE_SOURCE_DIR}/common/log.cc
 	${CMAKE_SOURCE_DIR}/common/threads.cc

vis/CMakeLists.txt

@@ -12,6 +12,7 @@ set(VIS_SOURCES
 	state.cc
 	${CMAKE_SOURCE_DIR}/common/cmdlib.cc
 	${CMAKE_SOURCE_DIR}/common/mathlib.cc
+	${CMAKE_SOURCE_DIR}/common/qvec.cc
 	${CMAKE_SOURCE_DIR}/common/polylib.cc
 	${CMAKE_SOURCE_DIR}/common/bsputils.cc
 	${CMAKE_SOURCE_DIR}/common/bspfile.cc