dbf-halloween2015

annotate libs/vmath/matrix.cc @ 1:c3f5c32cb210

find changesets by author, revision, files, or words in the commit message
barfed all the libraries in the source tree to make porting easier
author John Tsiombikas <nuclear@member.fsf.org>
date Sun, 01 Nov 2015 00:36:56 +0200
parents
children
rev   line source
nuclear@1 1 #include <cstdio>
nuclear@1 2 #include <cmath>
nuclear@1 3 #include "matrix.h"
nuclear@1 4 #include "vector.h"
nuclear@1 5 #include "quat.h"
nuclear@1 6
nuclear@1 7 using namespace std;
nuclear@1 8
nuclear@1 9 // ----------- Matrix3x3 --------------
nuclear@1 10
nuclear@1 11 Matrix3x3 Matrix3x3::identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
nuclear@1 12
nuclear@1 13 Matrix3x3::Matrix3x3()
nuclear@1 14 {
nuclear@1 15 *this = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
nuclear@1 16 }
nuclear@1 17
nuclear@1 18 Matrix3x3::Matrix3x3( scalar_t m11, scalar_t m12, scalar_t m13,
nuclear@1 19 scalar_t m21, scalar_t m22, scalar_t m23,
nuclear@1 20 scalar_t m31, scalar_t m32, scalar_t m33)
nuclear@1 21 {
nuclear@1 22 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;
nuclear@1 23 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;
nuclear@1 24 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;
nuclear@1 25 }
nuclear@1 26
nuclear@1 27 Matrix3x3::Matrix3x3(const Vector3 &ivec, const Vector3 &jvec, const Vector3 &kvec)
nuclear@1 28 {
nuclear@1 29 set_row_vector(ivec, 0);
nuclear@1 30 set_row_vector(jvec, 1);
nuclear@1 31 set_row_vector(kvec, 2);
nuclear@1 32 }
nuclear@1 33
nuclear@1 34 Matrix3x3::Matrix3x3(const mat3_t cmat)
nuclear@1 35 {
nuclear@1 36 memcpy(m, cmat, sizeof(mat3_t));
nuclear@1 37 }
nuclear@1 38
nuclear@1 39 Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4)
nuclear@1 40 {
nuclear@1 41 for(int i=0; i<3; i++) {
nuclear@1 42 for(int j=0; j<3; j++) {
nuclear@1 43 m[i][j] = mat4x4[i][j];
nuclear@1 44 }
nuclear@1 45 }
nuclear@1 46 }
nuclear@1 47
nuclear@1 48 Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@1 49 {
nuclear@1 50 Matrix3x3 res;
nuclear@1 51 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
nuclear@1 52 scalar_t *dest = res.m[0];
nuclear@1 53
nuclear@1 54 for(int i=0; i<9; i++) {
nuclear@1 55 *dest++ = *op1++ + *op2++;
nuclear@1 56 }
nuclear@1 57 return res;
nuclear@1 58 }
nuclear@1 59
nuclear@1 60 Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@1 61 {
nuclear@1 62 Matrix3x3 res;
nuclear@1 63 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
nuclear@1 64 scalar_t *dest = res.m[0];
nuclear@1 65
nuclear@1 66 for(int i=0; i<9; i++) {
nuclear@1 67 *dest++ = *op1++ - *op2++;
nuclear@1 68 }
nuclear@1 69 return res;
nuclear@1 70 }
nuclear@1 71
nuclear@1 72 Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@1 73 {
nuclear@1 74 Matrix3x3 res;
nuclear@1 75 for(int i=0; i<3; i++) {
nuclear@1 76 for(int j=0; j<3; j++) {
nuclear@1 77 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j];
nuclear@1 78 }
nuclear@1 79 }
nuclear@1 80 return res;
nuclear@1 81 }
nuclear@1 82
nuclear@1 83 void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@1 84 {
nuclear@1 85 scalar_t *op1 = m1.m[0];
nuclear@1 86 const scalar_t *op2 = m2.m[0];
nuclear@1 87
nuclear@1 88 for(int i=0; i<9; i++) {
nuclear@1 89 *op1++ += *op2++;
nuclear@1 90 }
nuclear@1 91 }
nuclear@1 92
nuclear@1 93 void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@1 94 {
nuclear@1 95 scalar_t *op1 = m1.m[0];
nuclear@1 96 const scalar_t *op2 = m2.m[0];
nuclear@1 97
nuclear@1 98 for(int i=0; i<9; i++) {
nuclear@1 99 *op1++ -= *op2++;
nuclear@1 100 }
nuclear@1 101 }
nuclear@1 102
nuclear@1 103 void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@1 104 {
nuclear@1 105 Matrix3x3 res;
nuclear@1 106 for(int i=0; i<3; i++) {
nuclear@1 107 for(int j=0; j<3; j++) {
nuclear@1 108 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j];
nuclear@1 109 }
nuclear@1 110 }
nuclear@1 111 memcpy(m1.m, res.m, 9 * sizeof(scalar_t));
nuclear@1 112 }
nuclear@1 113
nuclear@1 114 Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar)
nuclear@1 115 {
nuclear@1 116 Matrix3x3 res;
nuclear@1 117 const scalar_t *mptr = mat.m[0];
nuclear@1 118 scalar_t *dptr = res.m[0];
nuclear@1 119
nuclear@1 120 for(int i=0; i<9; i++) {
nuclear@1 121 *dptr++ = *mptr++ * scalar;
nuclear@1 122 }
nuclear@1 123 return res;
nuclear@1 124 }
nuclear@1 125
nuclear@1 126 Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat)
nuclear@1 127 {
nuclear@1 128 Matrix3x3 res;
nuclear@1 129 const scalar_t *mptr = mat.m[0];
nuclear@1 130 scalar_t *dptr = res.m[0];
nuclear@1 131
nuclear@1 132 for(int i=0; i<9; i++) {
nuclear@1 133 *dptr++ = *mptr++ * scalar;
nuclear@1 134 }
nuclear@1 135 return res;
nuclear@1 136 }
nuclear@1 137
nuclear@1 138 void operator *=(Matrix3x3 &mat, scalar_t scalar)
nuclear@1 139 {
nuclear@1 140 scalar_t *mptr = mat.m[0];
nuclear@1 141
nuclear@1 142 for(int i=0; i<9; i++) {
nuclear@1 143 *mptr++ *= scalar;
nuclear@1 144 }
nuclear@1 145 }
nuclear@1 146
nuclear@1 147 void Matrix3x3::translate(const Vector2 &trans)
nuclear@1 148 {
nuclear@1 149 Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
nuclear@1 150 *this *= tmat;
nuclear@1 151 }
nuclear@1 152
nuclear@1 153 void Matrix3x3::set_translation(const Vector2 &trans)
nuclear@1 154 {
nuclear@1 155 *this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
nuclear@1 156 }
nuclear@1 157
nuclear@1 158 void Matrix3x3::rotate(scalar_t angle)
nuclear@1 159 {
nuclear@1 160 scalar_t cos_a = cos(angle);
nuclear@1 161 scalar_t sin_a = sin(angle);
nuclear@1 162 Matrix3x3 rmat( cos_a, -sin_a, 0,
nuclear@1 163 sin_a, cos_a, 0,
nuclear@1 164 0, 0, 1);
nuclear@1 165 *this *= rmat;
nuclear@1 166 }
nuclear@1 167
nuclear@1 168 void Matrix3x3::set_rotation(scalar_t angle)
nuclear@1 169 {
nuclear@1 170 scalar_t cos_a = cos(angle);
nuclear@1 171 scalar_t sin_a = sin(angle);
nuclear@1 172 *this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1);
nuclear@1 173 }
nuclear@1 174
nuclear@1 175 void Matrix3x3::rotate(const Vector3 &euler_angles)
nuclear@1 176 {
nuclear@1 177 Matrix3x3 xrot, yrot, zrot;
nuclear@1 178
nuclear@1 179 xrot = Matrix3x3( 1, 0, 0,
nuclear@1 180 0, cos(euler_angles.x), -sin(euler_angles.x),
nuclear@1 181 0, sin(euler_angles.x), cos(euler_angles.x));
nuclear@1 182
nuclear@1 183 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
nuclear@1 184 0, 1, 0,
nuclear@1 185 -sin(euler_angles.y), 0, cos(euler_angles.y));
nuclear@1 186
nuclear@1 187 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
nuclear@1 188 sin(euler_angles.z), cos(euler_angles.z), 0,
nuclear@1 189 0, 0, 1);
nuclear@1 190
nuclear@1 191 *this *= xrot * yrot * zrot;
nuclear@1 192 }
nuclear@1 193
nuclear@1 194 void Matrix3x3::set_rotation(const Vector3 &euler_angles)
nuclear@1 195 {
nuclear@1 196 Matrix3x3 xrot, yrot, zrot;
nuclear@1 197
nuclear@1 198 xrot = Matrix3x3( 1, 0, 0,
nuclear@1 199 0, cos(euler_angles.x), -sin(euler_angles.x),
nuclear@1 200 0, sin(euler_angles.x), cos(euler_angles.x));
nuclear@1 201
nuclear@1 202 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
nuclear@1 203 0, 1, 0,
nuclear@1 204 -sin(euler_angles.y), 0, cos(euler_angles.y));
nuclear@1 205
nuclear@1 206 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
nuclear@1 207 sin(euler_angles.z), cos(euler_angles.z), 0,
nuclear@1 208 0, 0, 1);
nuclear@1 209
nuclear@1 210 *this = xrot * yrot * zrot;
nuclear@1 211 }
nuclear@1 212
nuclear@1 213 void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle)
nuclear@1 214 {
nuclear@1 215 scalar_t sina = (scalar_t)sin(angle);
nuclear@1 216 scalar_t cosa = (scalar_t)cos(angle);
nuclear@1 217 scalar_t invcosa = 1-cosa;
nuclear@1 218 scalar_t nxsq = axis.x * axis.x;
nuclear@1 219 scalar_t nysq = axis.y * axis.y;
nuclear@1 220 scalar_t nzsq = axis.z * axis.z;
nuclear@1 221
nuclear@1 222 Matrix3x3 xform;
nuclear@1 223 xform.m[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@1 224 xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@1 225 xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@1 226
nuclear@1 227 xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@1 228 xform.m[1][1] = nysq + (1-nysq) * cosa;
nuclear@1 229 xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@1 230
nuclear@1 231 xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@1 232 xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@1 233 xform.m[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@1 234
nuclear@1 235 *this *= xform;
nuclear@1 236 }
nuclear@1 237
nuclear@1 238 void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle)
nuclear@1 239 {
nuclear@1 240 scalar_t sina = (scalar_t)sin(angle);
nuclear@1 241 scalar_t cosa = (scalar_t)cos(angle);
nuclear@1 242 scalar_t invcosa = 1-cosa;
nuclear@1 243 scalar_t nxsq = axis.x * axis.x;
nuclear@1 244 scalar_t nysq = axis.y * axis.y;
nuclear@1 245 scalar_t nzsq = axis.z * axis.z;
nuclear@1 246
nuclear@1 247 reset_identity();
nuclear@1 248 m[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@1 249 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@1 250 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@1 251 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@1 252 m[1][1] = nysq + (1-nysq) * cosa;
nuclear@1 253 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@1 254 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@1 255 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@1 256 m[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@1 257 }
nuclear@1 258
nuclear@1 259 // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
nuclear@1 260 // article "Quaternion Calculus and Fast Animation".
nuclear@1 261 // adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl
nuclear@1 262 Quaternion Matrix3x3::get_rotation_quat() const
nuclear@1 263 {
nuclear@1 264 static const int next[3] = {1, 2, 0};
nuclear@1 265
nuclear@1 266 float quat[4];
nuclear@1 267
nuclear@1 268 scalar_t trace = m[0][0] + m[1][1] + m[2][2];
nuclear@1 269 scalar_t root;
nuclear@1 270
nuclear@1 271 if(trace > 0.0f) {
nuclear@1 272 // |w| > 1/2
nuclear@1 273 root = sqrt(trace + 1.0f); // 2w
nuclear@1 274 quat[0] = 0.5f * root;
nuclear@1 275 root = 0.5f / root; // 1 / 4w
nuclear@1 276 quat[1] = (m[2][1] - m[1][2]) * root;
nuclear@1 277 quat[2] = (m[0][2] - m[2][0]) * root;
nuclear@1 278 quat[3] = (m[1][0] - m[0][1]) * root;
nuclear@1 279 } else {
nuclear@1 280 // |w| <= 1/2
nuclear@1 281 int i = 0;
nuclear@1 282 if(m[1][1] > m[0][0]) {
nuclear@1 283 i = 1;
nuclear@1 284 }
nuclear@1 285 if(m[2][2] > m[i][i]) {
nuclear@1 286 i = 2;
nuclear@1 287 }
nuclear@1 288 int j = next[i];
nuclear@1 289 int k = next[j];
nuclear@1 290
nuclear@1 291 root = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0f);
nuclear@1 292 quat[i + 1] = 0.5f * root;
nuclear@1 293 root = 0.5f / root;
nuclear@1 294 quat[0] = (m[k][j] - m[j][k]) * root;
nuclear@1 295 quat[j + 1] = (m[j][i] - m[i][j]) * root;
nuclear@1 296 quat[k + 1] = (m[k][i] - m[i][k]) * root;
nuclear@1 297 }
nuclear@1 298 return Quaternion(quat[0], quat[1], quat[2], quat[3]);
nuclear@1 299 }
nuclear@1 300
nuclear@1 301 void Matrix3x3::scale(const Vector3 &scale_vec)
nuclear@1 302 {
nuclear@1 303 Matrix3x3 smat( scale_vec.x, 0, 0,
nuclear@1 304 0, scale_vec.y, 0,
nuclear@1 305 0, 0, scale_vec.z);
nuclear@1 306 *this *= smat;
nuclear@1 307 }
nuclear@1 308
nuclear@1 309 void Matrix3x3::set_scaling(const Vector3 &scale_vec)
nuclear@1 310 {
nuclear@1 311 *this = Matrix3x3( scale_vec.x, 0, 0,
nuclear@1 312 0, scale_vec.y, 0,
nuclear@1 313 0, 0, scale_vec.z);
nuclear@1 314 }
nuclear@1 315
nuclear@1 316 void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index)
nuclear@1 317 {
nuclear@1 318 m[0][col_index] = vec.x;
nuclear@1 319 m[1][col_index] = vec.y;
nuclear@1 320 m[2][col_index] = vec.z;
nuclear@1 321 }
nuclear@1 322
nuclear@1 323 void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index)
nuclear@1 324 {
nuclear@1 325 m[row_index][0] = vec.x;
nuclear@1 326 m[row_index][1] = vec.y;
nuclear@1 327 m[row_index][2] = vec.z;
nuclear@1 328 }
nuclear@1 329
nuclear@1 330 Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const
nuclear@1 331 {
nuclear@1 332 return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
nuclear@1 333 }
nuclear@1 334
nuclear@1 335 Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const
nuclear@1 336 {
nuclear@1 337 return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
nuclear@1 338 }
nuclear@1 339
nuclear@1 340 void Matrix3x3::transpose()
nuclear@1 341 {
nuclear@1 342 Matrix3x3 tmp = *this;
nuclear@1 343 for(int i=0; i<3; i++) {
nuclear@1 344 for(int j=0; j<3; j++) {
nuclear@1 345 m[i][j] = tmp[j][i];
nuclear@1 346 }
nuclear@1 347 }
nuclear@1 348 }
nuclear@1 349
nuclear@1 350 Matrix3x3 Matrix3x3::transposed() const
nuclear@1 351 {
nuclear@1 352 Matrix3x3 res;
nuclear@1 353 for(int i=0; i<3; i++) {
nuclear@1 354 for(int j=0; j<3; j++) {
nuclear@1 355 res[i][j] = m[j][i];
nuclear@1 356 }
nuclear@1 357 }
nuclear@1 358 return res;
nuclear@1 359 }
nuclear@1 360
nuclear@1 361 scalar_t Matrix3x3::determinant() const
nuclear@1 362 {
nuclear@1 363 return m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
nuclear@1 364 m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
nuclear@1 365 m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
nuclear@1 366 }
nuclear@1 367
nuclear@1 368 Matrix3x3 Matrix3x3::inverse() const
nuclear@1 369 {
nuclear@1 370 // TODO: implement 3x3 inverse
nuclear@1 371 return *this;
nuclear@1 372 }
nuclear@1 373
nuclear@1 374 ostream &operator <<(ostream &out, const Matrix3x3 &mat)
nuclear@1 375 {
nuclear@1 376 for(int i=0; i<3; i++) {
nuclear@1 377 char str[100];
nuclear@1 378 sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]);
nuclear@1 379 out << str;
nuclear@1 380 }
nuclear@1 381 return out;
nuclear@1 382 }
nuclear@1 383
nuclear@1 384
nuclear@1 385
nuclear@1 386 /* ----------------- Matrix4x4 implementation --------------- */
nuclear@1 387
nuclear@1 388 Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
nuclear@1 389
nuclear@1 390 Matrix4x4::Matrix4x4()
nuclear@1 391 {
nuclear@1 392 *this = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
nuclear@1 393 }
nuclear@1 394
nuclear@1 395 Matrix4x4::Matrix4x4( scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
nuclear@1 396 scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
nuclear@1 397 scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
nuclear@1 398 scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44)
nuclear@1 399 {
nuclear@1 400 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
nuclear@1 401 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
nuclear@1 402 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
nuclear@1 403 m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
nuclear@1 404 }
nuclear@1 405
nuclear@1 406 Matrix4x4::Matrix4x4(const mat4_t cmat)
nuclear@1 407 {
nuclear@1 408 memcpy(m, cmat, sizeof(mat4_t));
nuclear@1 409 }
nuclear@1 410
nuclear@1 411 Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3)
nuclear@1 412 {
nuclear@1 413 reset_identity();
nuclear@1 414 for(int i=0; i<3; i++) {
nuclear@1 415 for(int j=0; j<3; j++) {
nuclear@1 416 m[i][j] = mat3x3[i][j];
nuclear@1 417 }
nuclear@1 418 }
nuclear@1 419 }
nuclear@1 420
nuclear@1 421 Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
nuclear@1 422 {
nuclear@1 423 Matrix4x4 res;
nuclear@1 424 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
nuclear@1 425 scalar_t *dest = res.m[0];
nuclear@1 426
nuclear@1 427 for(int i=0; i<16; i++) {
nuclear@1 428 *dest++ = *op1++ + *op2++;
nuclear@1 429 }
nuclear@1 430 return res;
nuclear@1 431 }
nuclear@1 432
nuclear@1 433 Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
nuclear@1 434 {
nuclear@1 435 Matrix4x4 res;
nuclear@1 436 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
nuclear@1 437 scalar_t *dest = res.m[0];
nuclear@1 438
nuclear@1 439 for(int i=0; i<16; i++) {
nuclear@1 440 *dest++ = *op1++ - *op2++;
nuclear@1 441 }
nuclear@1 442 return res;
nuclear@1 443 }
nuclear@1 444
nuclear@1 445 void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
nuclear@1 446 {
nuclear@1 447 scalar_t *op1 = m1.m[0];
nuclear@1 448 const scalar_t *op2 = m2.m[0];
nuclear@1 449
nuclear@1 450 for(int i=0; i<16; i++) {
nuclear@1 451 *op1++ += *op2++;
nuclear@1 452 }
nuclear@1 453 }
nuclear@1 454
nuclear@1 455 void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
nuclear@1 456 {
nuclear@1 457 scalar_t *op1 = m1.m[0];
nuclear@1 458 const scalar_t *op2 = m2.m[0];
nuclear@1 459
nuclear@1 460 for(int i=0; i<16; i++) {
nuclear@1 461 *op1++ -= *op2++;
nuclear@1 462 }
nuclear@1 463 }
nuclear@1 464
nuclear@1 465 Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar)
nuclear@1 466 {
nuclear@1 467 Matrix4x4 res;
nuclear@1 468 const scalar_t *mptr = mat.m[0];
nuclear@1 469 scalar_t *dptr = res.m[0];
nuclear@1 470
nuclear@1 471 for(int i=0; i<16; i++) {
nuclear@1 472 *dptr++ = *mptr++ * scalar;
nuclear@1 473 }
nuclear@1 474 return res;
nuclear@1 475 }
nuclear@1 476
nuclear@1 477 Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat)
nuclear@1 478 {
nuclear@1 479 Matrix4x4 res;
nuclear@1 480 const scalar_t *mptr = mat.m[0];
nuclear@1 481 scalar_t *dptr = res.m[0];
nuclear@1 482
nuclear@1 483 for(int i=0; i<16; i++) {
nuclear@1 484 *dptr++ = *mptr++ * scalar;
nuclear@1 485 }
nuclear@1 486 return res;
nuclear@1 487 }
nuclear@1 488
nuclear@1 489 void operator *=(Matrix4x4 &mat, scalar_t scalar)
nuclear@1 490 {
nuclear@1 491 scalar_t *mptr = mat.m[0];
nuclear@1 492
nuclear@1 493 for(int i=0; i<16; i++) {
nuclear@1 494 *mptr++ *= scalar;
nuclear@1 495 }
nuclear@1 496 }
nuclear@1 497
nuclear@1 498 void Matrix4x4::translate(const Vector3 &trans)
nuclear@1 499 {
nuclear@1 500 Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
nuclear@1 501 *this *= tmat;
nuclear@1 502 }
nuclear@1 503
nuclear@1 504 void Matrix4x4::set_translation(const Vector3 &trans)
nuclear@1 505 {
nuclear@1 506 *this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
nuclear@1 507 }
nuclear@1 508
nuclear@1 509 Vector3 Matrix4x4::get_translation() const
nuclear@1 510 {
nuclear@1 511 return Vector3(m[0][3], m[1][3], m[2][3]);
nuclear@1 512 }
nuclear@1 513
nuclear@1 514 void Matrix4x4::rotate(const Vector3 &euler_angles)
nuclear@1 515 {
nuclear@1 516 Matrix3x3 xrot, yrot, zrot;
nuclear@1 517
nuclear@1 518 xrot = Matrix3x3( 1, 0, 0,
nuclear@1 519 0, cos(euler_angles.x), -sin(euler_angles.x),
nuclear@1 520 0, sin(euler_angles.x), cos(euler_angles.x));
nuclear@1 521
nuclear@1 522 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
nuclear@1 523 0, 1, 0,
nuclear@1 524 -sin(euler_angles.y), 0, cos(euler_angles.y));
nuclear@1 525
nuclear@1 526 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
nuclear@1 527 sin(euler_angles.z), cos(euler_angles.z), 0,
nuclear@1 528 0, 0, 1);
nuclear@1 529
nuclear@1 530 *this *= Matrix4x4(xrot * yrot * zrot);
nuclear@1 531 }
nuclear@1 532
nuclear@1 533 void Matrix4x4::set_rotation(const Vector3 &euler_angles)
nuclear@1 534 {
nuclear@1 535 Matrix3x3 xrot, yrot, zrot;
nuclear@1 536
nuclear@1 537 xrot = Matrix3x3( 1, 0, 0,
nuclear@1 538 0, cos(euler_angles.x), -sin(euler_angles.x),
nuclear@1 539 0, sin(euler_angles.x), cos(euler_angles.x));
nuclear@1 540
nuclear@1 541 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
nuclear@1 542 0, 1, 0,
nuclear@1 543 -sin(euler_angles.y), 0, cos(euler_angles.y));
nuclear@1 544
nuclear@1 545 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
nuclear@1 546 sin(euler_angles.z), cos(euler_angles.z), 0,
nuclear@1 547 0, 0, 1);
nuclear@1 548
nuclear@1 549 *this = Matrix4x4(xrot * yrot * zrot);
nuclear@1 550 }
nuclear@1 551
nuclear@1 552 void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle)
nuclear@1 553 {
nuclear@1 554 scalar_t sina = (scalar_t)sin(angle);
nuclear@1 555 scalar_t cosa = (scalar_t)cos(angle);
nuclear@1 556 scalar_t invcosa = 1-cosa;
nuclear@1 557 scalar_t nxsq = axis.x * axis.x;
nuclear@1 558 scalar_t nysq = axis.y * axis.y;
nuclear@1 559 scalar_t nzsq = axis.z * axis.z;
nuclear@1 560
nuclear@1 561 Matrix4x4 xform;
nuclear@1 562 xform[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@1 563 xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@1 564 xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@1 565 xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@1 566 xform[1][1] = nysq + (1-nysq) * cosa;
nuclear@1 567 xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@1 568 xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@1 569 xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@1 570 xform[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@1 571
nuclear@1 572 *this *= xform;
nuclear@1 573 }
nuclear@1 574
nuclear@1 575 void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle)
nuclear@1 576 {
nuclear@1 577 scalar_t sina = (scalar_t)sin(angle);
nuclear@1 578 scalar_t cosa = (scalar_t)cos(angle);
nuclear@1 579 scalar_t invcosa = 1-cosa;
nuclear@1 580 scalar_t nxsq = axis.x * axis.x;
nuclear@1 581 scalar_t nysq = axis.y * axis.y;
nuclear@1 582 scalar_t nzsq = axis.z * axis.z;
nuclear@1 583
nuclear@1 584 reset_identity();
nuclear@1 585 m[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@1 586 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@1 587 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@1 588 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@1 589 m[1][1] = nysq + (1-nysq) * cosa;
nuclear@1 590 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@1 591 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@1 592 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@1 593 m[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@1 594 }
nuclear@1 595
nuclear@1 596 void Matrix4x4::rotate(const Quaternion &quat)
nuclear@1 597 {
nuclear@1 598 *this *= quat.get_rotation_matrix();
nuclear@1 599 }
nuclear@1 600
nuclear@1 601 void Matrix4x4::set_rotation(const Quaternion &quat)
nuclear@1 602 {
nuclear@1 603 *this = quat.get_rotation_matrix();
nuclear@1 604 }
nuclear@1 605
nuclear@1 606 Quaternion Matrix4x4::get_rotation_quat() const
nuclear@1 607 {
nuclear@1 608 Matrix3x3 mat3 = *this;
nuclear@1 609 return mat3.get_rotation_quat();
nuclear@1 610 }
nuclear@1 611
nuclear@1 612 void Matrix4x4::scale(const Vector4 &scale_vec)
nuclear@1 613 {
nuclear@1 614 Matrix4x4 smat( scale_vec.x, 0, 0, 0,
nuclear@1 615 0, scale_vec.y, 0, 0,
nuclear@1 616 0, 0, scale_vec.z, 0,
nuclear@1 617 0, 0, 0, scale_vec.w);
nuclear@1 618 *this *= smat;
nuclear@1 619 }
nuclear@1 620
nuclear@1 621 void Matrix4x4::set_scaling(const Vector4 &scale_vec)
nuclear@1 622 {
nuclear@1 623 *this = Matrix4x4( scale_vec.x, 0, 0, 0,
nuclear@1 624 0, scale_vec.y, 0, 0,
nuclear@1 625 0, 0, scale_vec.z, 0,
nuclear@1 626 0, 0, 0, scale_vec.w);
nuclear@1 627 }
nuclear@1 628
nuclear@1 629 Vector3 Matrix4x4::get_scaling() const
nuclear@1 630 {
nuclear@1 631 Vector3 vi = get_row_vector(0);
nuclear@1 632 Vector3 vj = get_row_vector(1);
nuclear@1 633 Vector3 vk = get_row_vector(2);
nuclear@1 634
nuclear@1 635 return Vector3(vi.length(), vj.length(), vk.length());
nuclear@1 636 }
nuclear@1 637
nuclear@1 638 void Matrix4x4::set_frustum(float left, float right, float bottom, float top, float znear, float zfar)
nuclear@1 639 {
nuclear@1 640 float dx = right - left;
nuclear@1 641 float dy = top - bottom;
nuclear@1 642 float dz = zfar - znear;
nuclear@1 643
nuclear@1 644 float a = (right + left) / dx;
nuclear@1 645 float b = (top + bottom) / dy;
nuclear@1 646 float c = -(zfar + znear) / dz;
nuclear@1 647 float d = -2.0 * zfar * znear / dz;
nuclear@1 648
nuclear@1 649 *this = Matrix4x4(2.0 * znear / dx, 0, a, 0,
nuclear@1 650 0, 2.0 * znear / dy, b, 0,
nuclear@1 651 0, 0, c, d,
nuclear@1 652 0, 0, -1, 0);
nuclear@1 653 }
nuclear@1 654
nuclear@1 655 void Matrix4x4::set_perspective(float vfov, float aspect, float znear, float zfar)
nuclear@1 656 {
nuclear@1 657 float f = 1.0f / tan(vfov * 0.5f);
nuclear@1 658 float dz = znear - zfar;
nuclear@1 659
nuclear@1 660 reset_identity();
nuclear@1 661
nuclear@1 662 m[0][0] = f / aspect;
nuclear@1 663 m[1][1] = f;
nuclear@1 664 m[2][2] = (zfar + znear) / dz;
nuclear@1 665 m[3][2] = -1.0f;
nuclear@1 666 m[2][3] = 2.0f * zfar * znear / dz;
nuclear@1 667 m[3][3] = 0.0f;
nuclear@1 668 }
nuclear@1 669
nuclear@1 670 void Matrix4x4::set_orthographic(float left, float right, float bottom, float top, float znear, float zfar)
nuclear@1 671 {
nuclear@1 672 float dx = right - left;
nuclear@1 673 float dy = top - bottom;
nuclear@1 674 float dz = zfar - znear;
nuclear@1 675
nuclear@1 676 reset_identity();
nuclear@1 677
nuclear@1 678 m[0][0] = 2.0 / dx;
nuclear@1 679 m[1][1] = 2.0 / dy;
nuclear@1 680 m[2][2] = -2.0 / dz;
nuclear@1 681 m[0][3] = -(right + left) / dx;
nuclear@1 682 m[1][3] = -(top + bottom) / dy;
nuclear@1 683 m[2][3] = -(zfar + znear) / dz;
nuclear@1 684 }
nuclear@1 685
nuclear@1 686 void Matrix4x4::set_lookat(const Vector3 &pos, const Vector3 &targ, const Vector3 &up)
nuclear@1 687 {
nuclear@1 688 Vector3 vk = (targ - pos).normalized();
nuclear@1 689 Vector3 vj = up.normalized();
nuclear@1 690 Vector3 vi = cross_product(vk, vj).normalized();
nuclear@1 691 vj = cross_product(vi, vk);
nuclear@1 692
nuclear@1 693 *this = Matrix4x4(
nuclear@1 694 vi.x, vi.y, vi.z, 0,
nuclear@1 695 vj.x, vj.y, vj.z, 0,
nuclear@1 696 -vk.x, -vk.y, -vk.z, 0,
nuclear@1 697 0, 0, 0, 1);
nuclear@1 698 translate(-pos);
nuclear@1 699 }
nuclear@1 700
nuclear@1 701 void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index)
nuclear@1 702 {
nuclear@1 703 m[0][col_index] = vec.x;
nuclear@1 704 m[1][col_index] = vec.y;
nuclear@1 705 m[2][col_index] = vec.z;
nuclear@1 706 m[3][col_index] = vec.w;
nuclear@1 707 }
nuclear@1 708
nuclear@1 709 void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index)
nuclear@1 710 {
nuclear@1 711 m[row_index][0] = vec.x;
nuclear@1 712 m[row_index][1] = vec.y;
nuclear@1 713 m[row_index][2] = vec.z;
nuclear@1 714 m[row_index][3] = vec.w;
nuclear@1 715 }
nuclear@1 716
nuclear@1 717 Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const
nuclear@1 718 {
nuclear@1 719 return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
nuclear@1 720 }
nuclear@1 721
nuclear@1 722 Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const
nuclear@1 723 {
nuclear@1 724 return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
nuclear@1 725 }
nuclear@1 726
nuclear@1 727 void Matrix4x4::transpose()
nuclear@1 728 {
nuclear@1 729 Matrix4x4 tmp = *this;
nuclear@1 730 for(int i=0; i<4; i++) {
nuclear@1 731 for(int j=0; j<4; j++) {
nuclear@1 732 m[i][j] = tmp[j][i];
nuclear@1 733 }
nuclear@1 734 }
nuclear@1 735 }
nuclear@1 736
nuclear@1 737 Matrix4x4 Matrix4x4::transposed() const
nuclear@1 738 {
nuclear@1 739 Matrix4x4 res;
nuclear@1 740 for(int i=0; i<4; i++) {
nuclear@1 741 for(int j=0; j<4; j++) {
nuclear@1 742 res[i][j] = m[j][i];
nuclear@1 743 }
nuclear@1 744 }
nuclear@1 745 return res;
nuclear@1 746 }
nuclear@1 747
nuclear@1 748 scalar_t Matrix4x4::determinant() const
nuclear@1 749 {
nuclear@1 750 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@1 751 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@1 752 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@1 753
nuclear@1 754 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@1 755 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@1 756 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@1 757
nuclear@1 758 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@1 759 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@1 760 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@1 761
nuclear@1 762 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@1 763 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@1 764 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@1 765
nuclear@1 766 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
nuclear@1 767 }
nuclear@1 768
nuclear@1 769
nuclear@1 770 Matrix4x4 Matrix4x4::adjoint() const
nuclear@1 771 {
nuclear@1 772 Matrix4x4 coef;
nuclear@1 773
nuclear@1 774 coef.m[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@1 775 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@1 776 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@1 777 coef.m[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@1 778 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@1 779 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@1 780 coef.m[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@1 781 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@1 782 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@1 783 coef.m[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@1 784 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@1 785 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@1 786
nuclear@1 787 coef.m[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@1 788 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@1 789 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@1 790 coef.m[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@1 791 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@1 792 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@1 793 coef.m[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@1 794 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@1 795 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@1 796 coef.m[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@1 797 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@1 798 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@1 799
nuclear@1 800 coef.m[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@1 801 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
nuclear@1 802 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
nuclear@1 803 coef.m[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@1 804 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@1 805 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
nuclear@1 806 coef.m[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
nuclear@1 807 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@1 808 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@1 809 coef.m[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
nuclear@1 810 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
nuclear@1 811 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@1 812
nuclear@1 813 coef.m[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@1 814 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
nuclear@1 815 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
nuclear@1 816 coef.m[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@1 817 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@1 818 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
nuclear@1 819 coef.m[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
nuclear@1 820 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@1 821 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@1 822 coef.m[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
nuclear@1 823 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
nuclear@1 824 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@1 825
nuclear@1 826 coef.transpose();
nuclear@1 827
nuclear@1 828 for(int i=0; i<4; i++) {
nuclear@1 829 for(int j=0; j<4; j++) {
nuclear@1 830 coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
nuclear@1 831 if(i%2) coef.m[i][j] = -coef.m[i][j];
nuclear@1 832 }
nuclear@1 833 }
nuclear@1 834
nuclear@1 835 return coef;
nuclear@1 836 }
nuclear@1 837
nuclear@1 838 Matrix4x4 Matrix4x4::inverse() const
nuclear@1 839 {
nuclear@1 840 Matrix4x4 adj = adjoint();
nuclear@1 841
nuclear@1 842 return adj * (1.0f / determinant());
nuclear@1 843 }
nuclear@1 844
nuclear@1 845 ostream &operator <<(ostream &out, const Matrix4x4 &mat)
nuclear@1 846 {
nuclear@1 847 for(int i=0; i<4; i++) {
nuclear@1 848 char str[100];
nuclear@1 849 sprintf(str, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2], (float)mat.m[i][3]);
nuclear@1 850 out << str;
nuclear@1 851 }
nuclear@1 852 return out;
nuclear@1 853 }