gpuray_glsl

annotate vmath/matrix.cc @ 3:297dbc5080c4

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