goat3d

annotate libs/vmath/matrix.cc @ 51:fa5c52ea9d59

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