3dphotoshoot

annotate libs/vmath/matrix.cc @ 27:3d082c566b53

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