volmetrics

annotate src/matrix.cc @ 16:add30e2d5253

find changesets by author, revision, files, or words in the commit message
foo
author Eleni Maria Stea <elene.mst@gmail.com>
date Mon, 03 Feb 2014 02:52:46 +0200
parents
children
rev   line source
eleni@0 1 #include <math.h>
eleni@0 2 #include <stdio.h>
eleni@0 3 #include <string.h>
eleni@0 4 #include "matrix.h"
eleni@0 5
eleni@0 6 Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
eleni@0 7
eleni@0 8 Matrix4x4::Matrix4x4()
eleni@0 9 {
eleni@0 10 *this = identity;
eleni@0 11 }
eleni@0 12
eleni@0 13 Matrix4x4::Matrix4x4( float m11, float m12, float m13, float m14,
eleni@0 14 float m21, float m22, float m23, float m24,
eleni@0 15 float m31, float m32, float m33, float m34,
eleni@0 16 float m41, float m42, float m43, float m44)
eleni@0 17 {
eleni@0 18 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
eleni@0 19 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
eleni@0 20 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
eleni@0 21 m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
eleni@0 22 }
eleni@0 23
eleni@0 24 Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
eleni@0 25 {
eleni@0 26 Matrix4x4 res;
eleni@0 27 const float *op1 = m1.m[0], *op2 = m2.m[0];
eleni@0 28 float *dest = res.m[0];
eleni@0 29
eleni@0 30 for(int i=0; i<16; i++) {
eleni@0 31 *dest++ = *op1++ + *op2++;
eleni@0 32 }
eleni@0 33 return res;
eleni@0 34 }
eleni@0 35
eleni@0 36 Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
eleni@0 37 {
eleni@0 38 Matrix4x4 res;
eleni@0 39 const float *op1 = m1.m[0], *op2 = m2.m[0];
eleni@0 40 float *dest = res.m[0];
eleni@0 41
eleni@0 42 for(int i=0; i<16; i++) {
eleni@0 43 *dest++ = *op1++ - *op2++;
eleni@0 44 }
eleni@0 45 return res;
eleni@0 46 }
eleni@0 47
eleni@0 48 Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
eleni@0 49 Matrix4x4 res;
eleni@0 50
eleni@0 51 for(int i=0; i<4; i++) {
eleni@0 52 for(int j=0; j<4; j++) {
eleni@0 53 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] + m1.m[i][3] * m2.m[3][j];
eleni@0 54 }
eleni@0 55 }
eleni@0 56
eleni@0 57 return res;
eleni@0 58 }
eleni@0 59
eleni@0 60 void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
eleni@0 61 {
eleni@0 62 float *op1 = m1.m[0];
eleni@0 63 const float *op2 = m2.m[0];
eleni@0 64
eleni@0 65 for(int i=0; i<16; i++) {
eleni@0 66 *op1++ += *op2++;
eleni@0 67 }
eleni@0 68 }
eleni@0 69
eleni@0 70 void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
eleni@0 71 {
eleni@0 72 float *op1 = m1.m[0];
eleni@0 73 const float *op2 = m2.m[0];
eleni@0 74
eleni@0 75 for(int i=0; i<16; i++) {
eleni@0 76 *op1++ -= *op2++;
eleni@0 77 }
eleni@0 78 }
eleni@0 79
eleni@0 80 void operator *=(Matrix4x4 &m1, const Matrix4x4 &m2)
eleni@0 81 {
eleni@0 82 Matrix4x4 res;
eleni@0 83 for(int i=0; i<4; i++) {
eleni@0 84 for(int j=0; j<4; j++) {
eleni@0 85 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] + m1.m[i][3] * m2.m[3][j];
eleni@0 86 }
eleni@0 87 }
eleni@0 88 memcpy(m1.m, res.m, 16 * sizeof(float));
eleni@0 89 }
eleni@0 90
eleni@0 91 Matrix4x4 operator *(const Matrix4x4 &mat, float scalar)
eleni@0 92 {
eleni@0 93 Matrix4x4 res;
eleni@0 94 const float *mptr = mat.m[0];
eleni@0 95 float *dptr = res.m[0];
eleni@0 96
eleni@0 97 for(int i=0; i<16; i++) {
eleni@0 98 *dptr++ = *mptr++ * scalar;
eleni@0 99 }
eleni@0 100 return res;
eleni@0 101 }
eleni@0 102
eleni@0 103 Matrix4x4 operator *(float scalar, const Matrix4x4 &mat)
eleni@0 104 {
eleni@0 105 Matrix4x4 res;
eleni@0 106 const float *mptr = mat.m[0];
eleni@0 107 float *dptr = res.m[0];
eleni@0 108
eleni@0 109 for(int i=0; i<16; i++) {
eleni@0 110 *dptr++ = *mptr++ * scalar;
eleni@0 111 }
eleni@0 112 return res;
eleni@0 113 }
eleni@0 114
eleni@0 115 void operator *=(Matrix4x4 &mat, float scalar)
eleni@0 116 {
eleni@0 117 float *mptr = mat.m[0];
eleni@0 118
eleni@0 119 for(int i=0; i<16; i++) {
eleni@0 120 *mptr++ *= scalar;
eleni@0 121 }
eleni@0 122 }
eleni@0 123
eleni@0 124 void Matrix4x4::translate(const Vector3 &trans)
eleni@0 125 {
eleni@0 126 Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
eleni@0 127 *this *= tmat;
eleni@0 128 }
eleni@0 129
eleni@0 130 void Matrix4x4::set_translation(const Vector3 &trans)
eleni@0 131 {
eleni@0 132 *this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
eleni@0 133 }
eleni@0 134
eleni@0 135 void Matrix4x4::rotate(const Vector3 &euler)
eleni@0 136 {
eleni@0 137 float sinx = sin(euler.x);
eleni@0 138 float cosx = cos(euler.x);
eleni@0 139 float siny = sin(euler.y);
eleni@0 140 float cosy = cos(euler.y);
eleni@0 141 float sinz = sin(euler.z);
eleni@0 142 float cosz = cos(euler.z);
eleni@0 143
eleni@0 144 Matrix4x4 xrot = Matrix4x4(
eleni@0 145 1, 0, 0, 0,
eleni@0 146 0, cosx, sinx, 0,
eleni@0 147 0, -sinx, cosx, 0,
eleni@0 148 0, 0, 0, 1);
eleni@0 149
eleni@0 150 Matrix4x4 yrot = Matrix4x4(
eleni@0 151 cosy, 0, -siny, 0,
eleni@0 152 0, 1, 0, 0,
eleni@0 153 siny, 0, cosy, 0,
eleni@0 154 0, 0, 0, 1);
eleni@0 155
eleni@0 156 Matrix4x4 zrot = Matrix4x4(
eleni@0 157 cosz, sinz, 0, 0,
eleni@0 158 -sinz, cosz, 0, 0,
eleni@0 159 0, 0, 1, 0,
eleni@0 160 0, 0, 0, 1);
eleni@0 161
eleni@0 162 *this *= (xrot * yrot * zrot);
eleni@0 163 }
eleni@0 164
eleni@0 165 void Matrix4x4::rotate(const Vector3 &axis, float angle)
eleni@0 166 {
eleni@0 167 float sina = (float)sin(angle);
eleni@0 168 float cosa = (float)cos(angle);
eleni@0 169 float invcosa = 1-cosa;
eleni@0 170 float nxsq = axis.x * axis.x;
eleni@0 171 float nysq = axis.y * axis.y;
eleni@0 172 float nzsq = axis.z * axis.z;
eleni@0 173
eleni@0 174 Matrix4x4 xform;
eleni@0 175 xform[0][0] = nxsq + (1-nxsq) * cosa;
eleni@0 176 xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
eleni@0 177 xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
eleni@0 178 xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
eleni@0 179 xform[1][1] = nysq + (1-nysq) * cosa;
eleni@0 180 xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
eleni@0 181 xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
eleni@0 182 xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
eleni@0 183 xform[2][2] = nzsq + (1-nzsq) * cosa;
eleni@0 184
eleni@0 185 *this *= Matrix4x4(xform);
eleni@0 186 }
eleni@0 187
eleni@0 188 void Matrix4x4::set_rotation(const Vector3 &axis, float angle)
eleni@0 189 {
eleni@0 190 float sina = (float)sin(angle);
eleni@0 191 float cosa = (float)cos(angle);
eleni@0 192 float invcosa = 1-cosa;
eleni@0 193 float nxsq = axis.x * axis.x;
eleni@0 194 float nysq = axis.y * axis.y;
eleni@0 195 float nzsq = axis.z * axis.z;
eleni@0 196
eleni@0 197 reset_identity();
eleni@0 198 m[0][0] = nxsq + (1-nxsq) * cosa;
eleni@0 199 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
eleni@0 200 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
eleni@0 201 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
eleni@0 202 m[1][1] = nysq + (1-nysq) * cosa;
eleni@0 203 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
eleni@0 204 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
eleni@0 205 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
eleni@0 206 m[2][2] = nzsq + (1-nzsq) * cosa;
eleni@0 207 }
eleni@0 208
eleni@0 209 void Matrix4x4::scale(const Vector3 &scale_vec)
eleni@0 210 {
eleni@0 211 Matrix4x4 smat( scale_vec.x, 0, 0, 0,
eleni@0 212 0, scale_vec.y, 0, 0,
eleni@0 213 0, 0, scale_vec.z, 0,
eleni@0 214 0, 0, 0, 1.0);
eleni@0 215 *this *= smat;
eleni@0 216 }
eleni@0 217
eleni@0 218 void Matrix4x4::set_scaling(const Vector3 &scale_vec)
eleni@0 219 {
eleni@0 220 *this = Matrix4x4( scale_vec.x, 0, 0, 0,
eleni@0 221 0, scale_vec.y, 0, 0,
eleni@0 222 0, 0, scale_vec.z, 0,
eleni@0 223 0, 0, 0, 1);
eleni@0 224 }
eleni@0 225
eleni@0 226 void Matrix4x4::set_column_vector(const Vector3 &vec, unsigned int col_index)
eleni@0 227 {
eleni@0 228 m[0][col_index] = vec.x;
eleni@0 229 m[1][col_index] = vec.y;
eleni@0 230 m[2][col_index] = vec.z;
eleni@0 231 m[3][col_index] = 1;
eleni@0 232 }
eleni@0 233
eleni@0 234 void Matrix4x4::set_row_vector(const Vector3 &vec, unsigned int row_index)
eleni@0 235 {
eleni@0 236 m[row_index][0] = vec.x;
eleni@0 237 m[row_index][1] = vec.y;
eleni@0 238 m[row_index][2] = vec.z;
eleni@0 239 m[row_index][3] = 1;
eleni@0 240 }
eleni@0 241
eleni@0 242 Vector3 Matrix4x4::get_column_vector(unsigned int col_index) const
eleni@0 243 {
eleni@0 244 return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
eleni@0 245 }
eleni@0 246
eleni@0 247 Vector3 Matrix4x4::get_row_vector(unsigned int row_index) const
eleni@0 248 {
eleni@0 249 return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
eleni@0 250 }
eleni@0 251
eleni@0 252 void Matrix4x4::transpose()
eleni@0 253 {
eleni@0 254 Matrix4x4 tmp = *this;
eleni@0 255 for(int i=0; i<4; i++) {
eleni@0 256 for(int j=0; j<4; j++) {
eleni@0 257 m[i][j] = tmp[j][i];
eleni@0 258 }
eleni@0 259 }
eleni@0 260 }
eleni@0 261
eleni@0 262 Matrix4x4 Matrix4x4::transposed() const
eleni@0 263 {
eleni@0 264 Matrix4x4 res;
eleni@0 265 for(int i=0; i<4; i++) {
eleni@0 266 for(int j=0; j<4; j++) {
eleni@0 267 res[i][j] = m[j][i];
eleni@0 268 }
eleni@0 269 }
eleni@0 270 return res;
eleni@0 271 }
eleni@0 272
eleni@0 273 float Matrix4x4::determinant() const
eleni@0 274 {
eleni@0 275 float det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0 276 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
eleni@0 277 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
eleni@0 278
eleni@0 279 float det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0 280 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0 281 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
eleni@0 282
eleni@0 283 float det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
eleni@0 284 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0 285 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0 286
eleni@0 287 float det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
eleni@0 288 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
eleni@0 289 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0 290
eleni@0 291 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
eleni@0 292 }
eleni@0 293
eleni@0 294
eleni@0 295 Matrix4x4 Matrix4x4::adjoint() const
eleni@0 296 {
eleni@0 297 Matrix4x4 coef;
eleni@0 298
eleni@0 299 coef.m[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0 300 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
eleni@0 301 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
eleni@0 302 coef.m[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0 303 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0 304 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
eleni@0 305 coef.m[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
eleni@0 306 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0 307 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0 308 coef.m[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
eleni@0 309 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
eleni@0 310 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0 311
eleni@0 312 coef.m[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0 313 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
eleni@0 314 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
eleni@0 315 coef.m[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0 316 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0 317 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
eleni@0 318 coef.m[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
eleni@0 319 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0 320 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0 321 coef.m[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
eleni@0 322 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
eleni@0 323 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0 324
eleni@0 325 coef.m[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
eleni@0 326 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
eleni@0 327 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
eleni@0 328 coef.m[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
eleni@0 329 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
eleni@0 330 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
eleni@0 331 coef.m[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
eleni@0 332 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
eleni@0 333 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
eleni@0 334 coef.m[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
eleni@0 335 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
eleni@0 336 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
eleni@0 337
eleni@0 338 coef.m[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
eleni@0 339 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
eleni@0 340 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
eleni@0 341 coef.m[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
eleni@0 342 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
eleni@0 343 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
eleni@0 344 coef.m[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
eleni@0 345 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
eleni@0 346 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
eleni@0 347 coef.m[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
eleni@0 348 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
eleni@0 349 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
eleni@0 350
eleni@0 351 coef.transpose();
eleni@0 352
eleni@0 353 for(int i=0; i<4; i++) {
eleni@0 354 for(int j=0; j<4; j++) {
eleni@0 355 coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
eleni@0 356 if(i%2) coef.m[i][j] = -coef.m[i][j];
eleni@0 357 }
eleni@0 358 }
eleni@0 359
eleni@0 360 return coef;
eleni@0 361 }
eleni@0 362
eleni@0 363 Matrix4x4 Matrix4x4::inverse() const
eleni@0 364 {
eleni@0 365 Matrix4x4 adj = adjoint();
eleni@0 366
eleni@0 367 return adj * (1.0f / determinant());
eleni@0 368 }
eleni@0 369
eleni@0 370 inline float *Matrix4x4::operator [](int index)
eleni@0 371 {
eleni@0 372 return m[index];
eleni@0 373 }
eleni@0 374
eleni@0 375 inline const float *Matrix4x4::operator [](int index) const
eleni@0 376 {
eleni@0 377 return m[index];
eleni@0 378 }
eleni@0 379
eleni@0 380 inline void Matrix4x4::reset_identity()
eleni@0 381 {
eleni@0 382 *this = identity;
eleni@0 383 }
eleni@0 384
eleni@0 385 std::ostream &operator <<(std::ostream &out, const Matrix4x4 &mat)
eleni@0 386 {
eleni@0 387 for(int i=0; i<4; i++) {
eleni@0 388 char str[100];
eleni@0 389 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]);
eleni@0 390 out << str;
eleni@0 391 }
eleni@0 392 return out;
eleni@0 393 }