| 1 |
/*************************************************************************** |
| 2 |
* Copyright (C) 2005-2019 by the FIFE team * |
| 3 |
* http://www.fifengine.net * |
| 4 |
* This file is part of FIFE. * |
| 5 |
* * |
| 6 |
* FIFE is free software; you can redistribute it and/or * |
| 7 |
* modify it under the terms of the GNU Lesser General Public * |
| 8 |
* License as published by the Free Software Foundation; either * |
| 9 |
* version 2.1 of the License, or (at your option) any later version. * |
| 10 |
* * |
| 11 |
* This library is distributed in the hope that it will be useful, * |
| 12 |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
| 13 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * |
| 14 |
* Lesser General Public License for more details. * |
| 15 |
* * |
| 16 |
* You should have received a copy of the GNU Lesser General Public * |
| 17 |
* License along with this library; if not, write to the * |
| 18 |
* Free Software Foundation, Inc., * |
| 19 |
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA * |
| 20 |
***************************************************************************/ |
| 21 |
/*************************************************************************** |
| 22 |
* Includes some heavy copying from mathgl-pp project * |
| 23 |
* (http://sourceforge.net/projects/mathgl-pp/) * |
| 24 |
***************************************************************************/ |
| 25 |
|
| 26 |
#ifndef FIFE_UTIL_MATRIX_H |
| 27 |
#define FIFE_UTIL_MATRIX_H |
| 28 |
|
| 29 |
// Standard C++ library includes |
| 30 |
#include <cassert> |
| 31 |
#include <iostream> |
| 32 |
|
| 33 |
// Platform specific includes |
| 34 |
|
| 35 |
// 3rd party library includes |
| 36 |
|
| 37 |
// FIFE includes |
| 38 |
// These includes are split up in two parts, separated by one empty line |
| 39 |
// First block: files included from the FIFE root src directory |
| 40 |
// Second block: files included from the same folder |
| 41 |
#include "util/base/fife_stdint.h" |
| 42 |
#include "util/structures/point.h" |
| 43 |
|
| 44 |
#include "fife_math.h" |
| 45 |
|
| 46 |
namespace FIFE { |
| 47 |
|
| 48 |
|
| 49 |
/** Minimal matrix class to assist in view 3d calculations |
| 50 |
*/ |
| 51 |
template <typename T> |
| 52 |
class Matrix { |
| 53 |
public: |
| 54 |
Matrix<T>() {} |
| 55 |
template <typename U> friend class Matrix; |
| 56 |
template <typename U> Matrix<T>(const Matrix<U>& mat) { |
| 57 |
memmove(m, mat.m, 16*sizeof(T)); |
| 58 |
} |
| 59 |
~Matrix() {} |
| 60 |
|
| 61 |
/** Adjoint method inverse, constant time inversion implementation |
| 62 |
*/ |
| 63 |
Matrix inverse() const { |
| 64 |
Matrix ret(adjoint()); |
| 65 |
|
| 66 |
T determinant = m0*ret[0] + m1*ret[4] + m2*ret[8] + m3*ret[12]; |
| 67 |
assert(determinant!=0 && "Singular matrix has no inverse"); |
| 68 |
|
| 69 |
ret/=determinant; |
| 70 |
return ret; |
| 71 |
} |
| 72 |
|
| 73 |
/** Divide this matrix by a scalar |
| 74 |
*/ |
| 75 |
inline Matrix& operator/= (T val) { |
| 76 |
for (unsigned i = 0; i < 16; ++i) |
| 77 |
m[i] /= val; |
| 78 |
return *this; |
| 79 |
} |
| 80 |
|
| 81 |
/** Get the adjoint matrix |
| 82 |
*/ |
| 83 |
Matrix adjoint() const { |
| 84 |
Matrix ret; |
| 85 |
|
| 86 |
ret[0] = cofactorm0(); |
| 87 |
ret[1] = -cofactorm4(); |
| 88 |
ret[2] = cofactorm8(); |
| 89 |
ret[3] = -cofactorm12(); |
| 90 |
|
| 91 |
ret[4] = -cofactorm1(); |
| 92 |
ret[5] = cofactorm5(); |
| 93 |
ret[6] = -cofactorm9(); |
| 94 |
ret[7] = cofactorm13(); |
| 95 |
|
| 96 |
ret[8] = cofactorm2(); |
| 97 |
ret[9] = -cofactorm6(); |
| 98 |
ret[10] = cofactorm10(); |
| 99 |
ret[11] = -cofactorm14(); |
| 100 |
|
| 101 |
ret[12] = -cofactorm3(); |
| 102 |
ret[13] = cofactorm7(); |
| 103 |
ret[14] = -cofactorm11(); |
| 104 |
ret[15] = cofactorm15(); |
| 105 |
|
| 106 |
return ret; |
| 107 |
} |
| 108 |
|
| 109 |
|
| 110 |
/** Make this a rotation matrix |
| 111 |
*/ |
| 112 |
|
| 113 |
inline Matrix& loadRotate(T angle, T x, T y, T z) { |
| 114 |
T magSqr = x*x + y*y + z*z; |
| 115 |
if (magSqr != 1.0) { |
| 116 |
T mag = Math<T>::Sqrt(magSqr); |
| 117 |
x/=mag; |
| 118 |
y/=mag; |
| 119 |
z/=mag; |
| 120 |
} |
| 121 |
T c = Math<T>::Cos(angle*Math<T>::pi()/180); |
| 122 |
T s = Math<T>::Sin(angle*Math<T>::pi()/180); |
| 123 |
m0 = x*x*(1-c)+c; |
| 124 |
m1 = y*x*(1-c)+z*s; |
| 125 |
m2 = z*x*(1-c)-y*s; |
| 126 |
m3 = 0; |
| 127 |
|
| 128 |
m4 = x*y*(1-c)-z*s; |
| 129 |
m5 = y*y*(1-c)+c; |
| 130 |
m6 = z*y*(1-c)+x*s; |
| 131 |
m7 = 0; |
| 132 |
|
| 133 |
m8 = x*z*(1-c)+y*s; |
| 134 |
m9 = y*z*(1-c)-x*s; |
| 135 |
m10 = z*z*(1-c)+c; |
| 136 |
m11 = 0; |
| 137 |
|
| 138 |
m12 = 0; |
| 139 |
m13 = 0; |
| 140 |
m14 = 0; |
| 141 |
m15 = 1; |
| 142 |
|
| 143 |
return *this; |
| 144 |
} |
| 145 |
|
| 146 |
/** Apply scale into this matrix |
| 147 |
*/ |
| 148 |
inline Matrix& applyScale(T x, T y, T z) { |
| 149 |
static Matrix<T> temp; |
| 150 |
temp.loadScale(x,y,z); |
| 151 |
*this = temp.mult4by4(*this); |
| 152 |
return *this; |
| 153 |
} |
| 154 |
|
| 155 |
/** Make this a scale matrix |
| 156 |
*/ |
| 157 |
inline Matrix& loadScale(T x, T y, T z = 1) { |
| 158 |
m0 = x; |
| 159 |
m4 = 0; |
| 160 |
m8 = 0; |
| 161 |
m12 = 0; |
| 162 |
m1 = 0; |
| 163 |
m5 = y; |
| 164 |
m9 = 0; |
| 165 |
m13 = 0; |
| 166 |
m2 = 0; |
| 167 |
m6 = 0; |
| 168 |
m10 = z; |
| 169 |
m14 = 0; |
| 170 |
m3 = 0; |
| 171 |
m7 = 0; |
| 172 |
m11 = 0; |
| 173 |
m15 = 1; |
| 174 |
|
| 175 |
return *this; |
| 176 |
} |
| 177 |
|
| 178 |
/** Apply translation into this matrix |
| 179 |
*/ |
| 180 |
inline Matrix& applyTranslate(T x, T y, T z) { |
| 181 |
static Matrix<T> temp; |
| 182 |
temp.loadTranslate(x,y,z); |
| 183 |
*this = temp.mult4by4(*this); |
| 184 |
return *this; |
| 185 |
} |
| 186 |
|
| 187 |
/** Make this a translation matrix |
| 188 |
*/ |
| 189 |
inline Matrix& loadTranslate( const T x, const T y, const T z) { |
| 190 |
m0 = 1; |
| 191 |
m4 = 0; |
| 192 |
m8 = 0; |
| 193 |
m12 = x; |
| 194 |
m1 = 0; |
| 195 |
m5 = 1; |
| 196 |
m9 = 0; |
| 197 |
m13 = y; |
| 198 |
m2 = 0; |
| 199 |
m6 = 0; |
| 200 |
m10 = 1; |
| 201 |
m14 = z; |
| 202 |
m3 = 0; |
| 203 |
m7 = 0; |
| 204 |
m11 = 0; |
| 205 |
m15 = 1; |
| 206 |
|
| 207 |
return *this; |
| 208 |
} |
| 209 |
|
| 210 |
/** Transform given point using this matrix |
| 211 |
*/ |
| 212 |
inline PointType3D<T> operator* (const PointType3D<T>& vec) { |
| 213 |
return PointType3D<T> ( |
| 214 |
vec.x * m0 + vec.y * m4 + vec.z * m8 + m12, |
| 215 |
vec.x * m1 + vec.y * m5 + vec.z * m9 + m13, |
| 216 |
vec.x * m2 + vec.y * m6 + vec.z * m10 + m14 |
| 217 |
); |
| 218 |
} |
| 219 |
|
| 220 |
/** Direct access to the matrix elements, just remember they are in column major format!! |
| 221 |
*/ |
| 222 |
inline T& operator[] (int32_t ind) { |
| 223 |
assert(ind > -1 && ind < 16); |
| 224 |
return m[ind]; |
| 225 |
} |
| 226 |
inline const T& operator[] (int32_t ind) const { |
| 227 |
assert(ind > -1 && ind < 16); |
| 228 |
return m[ind]; |
| 229 |
} |
| 230 |
|
| 231 |
/** Apply the matrix dot product to this matrix |
| 232 |
*/ |
| 233 |
inline Matrix& mult3by3(const Matrix& mat) { |
| 234 |
Matrix temp(*this); |
| 235 |
m0 = temp.m0*mat.m0+temp.m4*mat.m1+temp.m8*mat.m2; |
| 236 |
m4 = temp.m0*mat.m4+temp.m4*mat.m5+temp.m8*mat.m6; |
| 237 |
m8 = temp.m0*mat.m8+temp.m4*mat.m9+temp.m8*mat.m10; |
| 238 |
|
| 239 |
m1 = temp.m1*mat.m0+temp.m5*mat.m1+temp.m9*mat.m2; |
| 240 |
m5 = temp.m1*mat.m4+temp.m5*mat.m5+temp.m9*mat.m6; |
| 241 |
m9 = temp.m1*mat.m8+temp.m5*mat.m9+temp.m9*mat.m10; |
| 242 |
|
| 243 |
m2 = temp.m2*mat.m0+temp.m6*mat.m1+temp.m10*mat.m2; |
| 244 |
m6 = temp.m2*mat.m4+temp.m6*mat.m5+temp.m10*mat.m6; |
| 245 |
m10 = temp.m2*mat.m8+temp.m6*mat.m9+temp.m10*mat.m10; |
| 246 |
|
| 247 |
m3 = temp.m3*mat.m0+temp.m7*mat.m1+temp.m11*mat.m2; |
| 248 |
m7 = temp.m3*mat.m4+temp.m7*mat.m5+temp.m11*mat.m6; |
| 249 |
m11 = temp.m3*mat.m8+temp.m7*mat.m9+temp.m11*mat.m10; |
| 250 |
return *this; |
| 251 |
} |
| 252 |
|
| 253 |
/** this->Rmult4by4(temp) == [temp] X [*this] **/ |
| 254 |
/** also equal to temp->mult4by4(*this) **/ |
| 255 |
inline Matrix<T>& Rmult4by4(const Matrix<T>& mat) { |
| 256 |
Matrix temp(*this); |
| 257 |
|
| 258 |
m0 = mat.m0*temp.m0+mat.m4*temp.m1+mat.m8*temp.m2+mat.m12*temp.m3; |
| 259 |
m4 = mat.m0*temp.m4+mat.m4*temp.m5+mat.m8*temp.m6+mat.m12*temp.m7; |
| 260 |
m8 = mat.m0*temp.m8+mat.m4*temp.m9+mat.m8*temp.m10+mat.m12*temp.m11; |
| 261 |
m12 = mat.m0*temp.m12+mat.m4*temp.m13+mat.m8*temp.m14+mat.m12*temp.m15; |
| 262 |
|
| 263 |
m1 = mat.m1*temp.m0 + mat.m5*temp.m1 + mat.m9*temp.m2+mat.m13*temp.m3; |
| 264 |
m5 = mat.m1*temp.m4 + mat.m5*temp.m5 + mat.m9*temp.m6+mat.m13*temp.m7; |
| 265 |
m9 = mat.m1*temp.m8 + mat.m5*temp.m9 + mat.m9*temp.m10+mat.m13*temp.m11; |
| 266 |
m13 = mat.m1*temp.m12+ mat.m5*temp.m13 + mat.m9*temp.m14+mat.m13*temp.m15; |
| 267 |
|
| 268 |
m2 = mat.m2*temp.m0+mat.m6*temp.m1+mat.m10*temp.m2+mat.m14*temp.m3; |
| 269 |
m6 = mat.m2*temp.m4+mat.m6*temp.m5+mat.m10*temp.m6+mat.m14*temp.m7; |
| 270 |
m10 = mat.m2*temp.m8+mat.m6*temp.m9+mat.m10*temp.m10+mat.m14*temp.m11; |
| 271 |
m14 = mat.m2*temp.m12+mat.m6*temp.m13+mat.m10*temp.m14+mat.m14*temp.m15; |
| 272 |
|
| 273 |
m3 = mat.m3*temp.m0+mat.m7*temp.m1+mat.m11*temp.m2+mat.m15*temp.m3; |
| 274 |
m7 = mat.m3*temp.m4+mat.m7*temp.m5+mat.m11*temp.m6+mat.m15*temp.m7; |
| 275 |
m11 = mat.m3*temp.m8+mat.m7*temp.m9+mat.m11*temp.m10+mat.m15*temp.m11; |
| 276 |
m15 = mat.m3*temp.m12+mat.m7*temp.m13+mat.m11*temp.m14+mat.m15*temp.m15; |
| 277 |
return *this; |
| 278 |
} |
| 279 |
|
| 280 |
|
| 281 |
inline Matrix<T>& mult4by4(const Matrix<T>& mat) { |
| 282 |
Matrix temp(*this); |
| 283 |
|
| 284 |
m0 = temp.m0*mat.m0+temp.m4*mat.m1+temp.m8*mat.m2+temp.m12*mat.m3; |
| 285 |
m4 = temp.m0*mat.m4+temp.m4*mat.m5+temp.m8*mat.m6+temp.m12*mat.m7; |
| 286 |
m8 = temp.m0*mat.m8+temp.m4*mat.m9+temp.m8*mat.m10+temp.m12*mat.m11; |
| 287 |
m12 = temp.m0*mat.m12+temp.m4*mat.m13+temp.m8*mat.m14+temp.m12*mat.m15; |
| 288 |
|
| 289 |
m1 = temp.m1*mat.m0 + temp.m5*mat.m1 + temp.m9*mat.m2+temp.m13*mat.m3; |
| 290 |
m5 = temp.m1*mat.m4 + temp.m5*mat.m5 + temp.m9*mat.m6+temp.m13*mat.m7; |
| 291 |
m9 = temp.m1*mat.m8 + temp.m5*mat.m9 + temp.m9*mat.m10+temp.m13*mat.m11; |
| 292 |
m13 = temp.m1*mat.m12+ temp.m5*mat.m13 + temp.m9*mat.m14+temp.m13*mat.m15; |
| 293 |
|
| 294 |
m2 = temp.m2*mat.m0+temp.m6*mat.m1+temp.m10*mat.m2+temp.m14*mat.m3; |
| 295 |
m6 = temp.m2*mat.m4+temp.m6*mat.m5+temp.m10*mat.m6+temp.m14*mat.m7; |
| 296 |
m10 = temp.m2*mat.m8+temp.m6*mat.m9+temp.m10*mat.m10+temp.m14*mat.m11; |
| 297 |
m14 = temp.m2*mat.m12+temp.m6*mat.m13+temp.m10*mat.m14+temp.m14*mat.m15; |
| 298 |
|
| 299 |
m3 = temp.m3*mat.m0+temp.m7*mat.m1+temp.m11*mat.m2+temp.m15*mat.m3; |
| 300 |
m7 = temp.m3*mat.m4+temp.m7*mat.m5+temp.m11*mat.m6+temp.m15*mat.m7; |
| 301 |
m11 = temp.m3*mat.m8+temp.m7*mat.m9+temp.m11*mat.m10+temp.m15*mat.m11; |
| 302 |
m15 = temp.m3*mat.m12+temp.m7*mat.m13+temp.m11*mat.m14+temp.m15*mat.m15; |
| 303 |
return *this; |
| 304 |
} |
| 305 |
|
| 306 |
Matrix& applyRotate(T angle, T x, T y, T z) { |
| 307 |
static Matrix<T> temp; |
| 308 |
temp.loadRotate(angle,x,y,z); |
| 309 |
*this = temp.mult4by4(*this); |
| 310 |
return *this; |
| 311 |
} |
| 312 |
|
| 313 |
|
| 314 |
private: |
| 315 |
#define cofactor_maker(f1,mj1,mi1, f2,mj2,mi2, f3,mj3,mi3) \ |
| 316 |
f1*(mj1*mi1-mj2*mi3) + f2*(mj2*mi2-mj3*mi1) + f3*(mj3*mi3-mj1*mi2) |
| 317 |
|
| 318 |
inline T cofactorm0() const { |
| 319 |
return cofactor_maker(m5,m10,m15, m6,m11,m13, m7,m9,m14); |
| 320 |
} |
| 321 |
inline T cofactorm1() const { |
| 322 |
return cofactor_maker(m6,m11,m12, m7,m8,m14, m4,m10,m15); |
| 323 |
} |
| 324 |
inline T cofactorm2() const { |
| 325 |
return cofactor_maker(m7,m8,m13, m4,m9,m15, m5,m11,m12); |
| 326 |
} |
| 327 |
inline T cofactorm3() const { |
| 328 |
return cofactor_maker(m4,m9,m14, m5,m10,m12, m6,m8,m13); |
| 329 |
} |
| 330 |
inline T cofactorm4() const { |
| 331 |
return cofactor_maker(m9,m14,m3, m10,m15,m1, m11,m13,m2); |
| 332 |
} |
| 333 |
inline T cofactorm5() const { |
| 334 |
return cofactor_maker(m10,m15,m0, m11,m12,m2, m8,m14,m3); |
| 335 |
} |
| 336 |
inline T cofactorm6() const { |
| 337 |
return cofactor_maker(m11,m12,m1, m8,m13,m3, m9,m15,m0); |
| 338 |
} |
| 339 |
inline T cofactorm7() const { |
| 340 |
return cofactor_maker(m8,m13,m2, m9,m14,m0, m10,m12,m1); |
| 341 |
} |
| 342 |
inline T cofactorm8() const { |
| 343 |
return cofactor_maker(m13,m2,m7, m14,m3,m5, m15,m1,m6); |
| 344 |
} |
| 345 |
inline T cofactorm9() const { |
| 346 |
return cofactor_maker(m14,m13,m4, m15,m0,m6, m12,m2,m7); |
| 347 |
} |
| 348 |
inline T cofactorm10() const { |
| 349 |
return cofactor_maker(m15,m0,m5, m12,m1,m7, m13,m3,m4); |
| 350 |
} |
| 351 |
inline T cofactorm11() const { |
| 352 |
return cofactor_maker(m12,m1,m6, m13,m2,m4, m14,m0,m5); |
| 353 |
} |
| 354 |
inline T cofactorm12() const { |
| 355 |
return cofactor_maker(m1,m6,m11, m2,m7,m9, m3,m5,m10); |
| 356 |
} |
| 357 |
inline T cofactorm13() const { |
| 358 |
return cofactor_maker(m2,m7,m8, m3,m4,m10, m10,m6,m11); |
| 359 |
} |
| 360 |
inline T cofactorm14() const { |
| 361 |
return cofactor_maker(m3,m4,m9, m0,m5,m11, m1,m7,m8); |
| 362 |
} |
| 363 |
inline T cofactorm15() const { |
| 364 |
return cofactor_maker(m0,m5,m10, m1,m6,m8, m2,m4,m9); |
| 365 |
} |
| 366 |
|
| 367 |
public: |
| 368 |
union { |
| 369 |
T m[16]; |
| 370 |
struct { |
| 371 |
T m0,m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,m11,m12,m13,m14,m15; |
| 372 |
}; |
| 373 |
}; |
| 374 |
}; |
| 375 |
|
| 376 |
typedef Matrix<double> DoubleMatrix; |
| 377 |
typedef Matrix<int32_t> IntMatrix; |
| 378 |
|
| 379 |
/** Print coords of the Matrix to a stream |
| 380 |
*/ |
| 381 |
template<typename T> |
| 382 |
std::ostream& operator<<(std::ostream& os, const Matrix<T>& m) { |
| 383 |
|
| 384 |
return os << "\n|" << m[0] << "," << m[4] << "," << m[8] << "," << m[12] << "|\n" << \ |
| 385 |
"|" << m[1] << "," << m[5] << "," << m[9] << "," << m[13] << "|\n" << \ |
| 386 |
"|" << m[2] << "," << m[6] << "," << m[10] << "," << m[14] << "|\n" << \ |
| 387 |
"|" << m[3] << "," << m[7] << "," << m[11] << "," << m[15] << "|\n"; |
| 388 |
} |
| 389 |
|
| 390 |
|
| 391 |
} |
| 392 |
|
| 393 |
#endif |