source/matrix.cpp

   1 #include <algorithm>
   2 #include <cmath>
   3 #include "error.h"
   4 #include "matrix.h"
   5
   6 using namespace std;
   7
   8 namespace Msp {
   9 namespace GL {
  10
  11 Matrix::Matrix():
  12         flags(IDENTITY)
  13 {
  14         for(unsigned i=0; i<16; ++i)
  15                 matrix[i] = (i%5 ? 0 : 1);
  16 }
  17
  18 Matrix::Matrix(const float *m):
  19         flags(IDENTITY)
  20 {
  21         copy(m, m+16, matrix);
  22         check_flags();
  23 }
  24
  25 Matrix::Matrix(const double *m):
  26         flags(IDENTITY)
  27 {
  28         copy(m, m+16, matrix);
  29         check_flags();
  30 }
  31
  32 void Matrix::check_flags()
  33 {
  34         const double *m = matrix;
  35         if(m[12]!=0 || m[13]!=0 || m[14]!=0)
  36                 flags |= TRANSLATE;
  37         if(m[0]!=1)
  38         {
  39                 flags |= SCALE;
  40                 if(m[5]!=m[0] || m[10]!=m[0])
  41                         flags |= ARBITARY;
  42         }
  43         if(m[1]!=0 || m[2]!=0 || m[4]!=0 || m[6]!=0 || m[8]!=0 || m[9]!=0)
  44         {
  45                 flags |= ROTATE;
  46                 double x_dot_y = m[0]*m[1]+m[4]*m[5]+m[8]*m[9];
  47                 double x_dot_z = m[0]*m[2]+m[4]*m[6]+m[8]*m[10];
  48                 double y_dot_z = m[1]*m[2]+m[5]*m[6]+m[9]*m[10];
  49                 if(x_dot_y!=0 || x_dot_z!=0 || y_dot_z!=0)
  50                         flags |= ARBITARY;
  51         }
  52         if(m[3]!=0 || m[7]!=0 || m[11]!=0 || m[15]!=1)
  53                 flags |= ARBITARY;
  54 }
  55
  56 void Matrix::multiply(const Matrix &other)
  57 {
  58         *this = *this*other;
  59 }
  60
  61 void Matrix::translate(double x, double y, double z)
  62 {
  63         multiply(translation(x, y, z));
  64 }
  65
  66 void Matrix::rotate(double a, double x, double y, double z)
  67 {
  68         multiply(rotation(a, x, y, z));
  69 }
  70
  71 void Matrix::rotate_deg(double a, double x, double y, double z)
  72 {
  73         multiply(rotation_deg(a, x, y, z));
  74 }
  75
  76 void Matrix::scale(double s)
  77 {
  78         multiply(scaling(s));
  79 }
  80
  81 void Matrix::scale(double x, double y, double z)
  82 {
  83         multiply(scaling(x, y, z));
  84 }
  85
  86 Matrix Matrix::operator*(const Matrix &other) const
  87 {
  88         if(flags==IDENTITY)
  89                 return other;
  90         else if(other.flags==IDENTITY)
  91                 return *this;
  92         else if(flags==TRANSLATE && !(other.flags&ARBITARY))
  93         {
  94                 Matrix result = other;
  95                 result.matrix[12] += matrix[12];
  96                 result.matrix[13] += matrix[13];
  97                 result.matrix[14] += matrix[14];
  98                 result.flags |= flags;
  99                 return result;
 100         }
 101         else if(!(flags&ARBITARY) && other.flags==TRANSLATE)
 102         {
 103                 Matrix result = *this;
 104                 const double *m = other.matrix;
 105                 result.matrix[12] += matrix[0]*m[12]+matrix[4]*m[13]+matrix[8]*m[14];
 106                 result.matrix[13] += matrix[1]*m[12]+matrix[5]*m[13]+matrix[9]*m[14];
 107                 result.matrix[14] += matrix[2]*m[12]+matrix[6]*m[13]+matrix[10]*m[14];
 108                 result.flags |= other.flags;
 109                 return result;
 110         }
 111         else
 112         {
 113                 Matrix result;
 114                 fill(result.matrix, result.matrix+16, 0.0);
 115                 for(unsigned i=0; i<4; ++i)
 116                         for(unsigned j=0; j<4; ++j)
 117                                 for(unsigned k=0; k<4; ++k)
 118                                         result.matrix[i+j*4] += matrix[i+k*4]*other.matrix[k+j*4];
 119                 result.flags = flags|other.flags;
 120                 return result;
 121         }
 122 }
 123
 124 Matrix &Matrix::operator*=(const Matrix &other)
 125 {
 126         multiply(other);
 127         return *this;
 128 }
 129
 130 Vector4 Matrix::operator*(const Vector4 &vec) const
 131 {
 132         if(flags==IDENTITY)
 133                 return vec;
 134         else if(flags==TRANSLATE)
 135                 return Vector4(vec.x+vec.w*matrix[12], vec.y+vec.w*matrix[13], vec.z+vec.w*matrix[14], vec.w);
 136         else if(flags==SCALE)
 137                 return Vector4(vec.x*matrix[0], vec.y*matrix[5], vec.z*matrix[10], vec.w);
 138         else
 139         {
 140                 Vector4 result;
 141                 result.x = vec.x*matrix[0]+vec.y*matrix[4]+vec.z*matrix[8]+vec.w*matrix[12];
 142                 result.y = vec.x*matrix[1]+vec.y*matrix[5]+vec.z*matrix[9]+vec.w*matrix[13];
 143                 result.z = vec.x*matrix[2]+vec.y*matrix[6]+vec.z*matrix[10]+vec.w*matrix[14];
 144                 result.w = vec.x*matrix[3]+vec.y*matrix[7]+vec.z*matrix[11]+vec.w*matrix[15];
 145                 return result;
 146         }
 147 }
 148
 149 double Matrix::operator[](unsigned i) const
 150 {
 151         if(i>=16)
 152                 throw out_of_range("Matrix::operator[]");
 153         return matrix[i];
 154 }
 155
 156 Matrix Matrix::translation(double x, double y, double z)
 157 {
 158         Matrix result;
 159         result.matrix[12] = x;
 160         result.matrix[13] = y;
 161         result.matrix[14] = z;
 162         result.flags |= TRANSLATE;
 163         return result;
 164 }
 165
 166 Matrix Matrix::rotation(double a, double x, double y, double z)
 167 {
 168         double l = sqrt(x*x+y*y+z*z);
 169         x /= l;
 170         y /= l;
 171         z /= l;
 172         double c = cos(a);
 173         double s = sin(a);
 174
 175         // http://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_given_an_axis_and_an_angle
 176         Matrix result;
 177         result.matrix[0] = c+x*x*(1-c);
 178         result.matrix[1] = y*x*(1-c)+z*s;
 179         result.matrix[2] = z*x*(1-c)-y*s;
 180         result.matrix[4] = x*y*(1-c)-z*s;
 181         result.matrix[5] = c+y*y*(1-c);
 182         result.matrix[6] = z*y*(1-c)+x*s;
 183         result.matrix[8] = x*z*(1-c)+y*s;
 184         result.matrix[9] = y*z*(1-c)-x*s;
 185         result.matrix[10] = c+z*z*(1-c);
 186         result.flags |= ROTATE;
 187         return result;
 188 }
 189
 190 Matrix Matrix::rotation_deg(double a, double x, double y, double z)
 191 {
 192         return rotation(a*M_PI/180, x, y, z);
 193 }
 194
 195 Matrix Matrix::scaling(double s)
 196 {
 197         Matrix result;
 198         result.matrix[0] = s;
 199         result.matrix[5] = s;
 200         result.matrix[10] = s;
 201         result.flags |= SCALE;
 202         return result;
 203 }
 204
 205 Matrix Matrix::scaling(double x, double y, double z)
 206 {
 207         Matrix result;
 208         result.matrix[0] = x;
 209         result.matrix[5] = y;
 210         result.matrix[10] = z;
 211         result.flags |= SCALE|ARBITARY;
 212         return result;
 213 }
 214
 215 Matrix Matrix::ortho(double l, double r, double b, double t, double n, double f)
 216 {
 217         if(l==r || b==t || n==f)
 218                 throw invalid_argument("Matrix::ortho");
 219
 220         Matrix result;
 221         result.matrix[0] = 2/(r-l);
 222         result.matrix[5] = 2/(t-b);
 223         result.matrix[10] = -2/(f-n);
 224         result.matrix[12] = -(r+l)/(r-l);
 225         result.matrix[13] = -(t+b)/(t-b);
 226         result.matrix[14] = -(f+n)/(f-n);
 227         result.flags = TRANSLATE|SCALE|ARBITARY;
 228         return result;
 229 }
 230
 231 Matrix Matrix::ortho_centered(double w, double h)
 232 {
 233         return ortho(-w/2, w/2, -h/2, h/2, -1, 1);
 234 }
 235
 236 Matrix Matrix::ortho_bottomleft(double w, double h)
 237 {
 238         return ortho(0, w, 0, h, -1, 1);
 239 }
 240
 241 Matrix Matrix::ortho_topleft(double w, double h)
 242 {
 243         return ortho(0, w, h, 0, -1, 1);
 244 }
 245
 246 Matrix Matrix::frustum(double l, double r, double b, double t, double n, double f)
 247 {
 248         if(l==r || b==t || n<=0 || f<=n)
 249                 throw invalid_argument("Matrix::frustum");
 250
 251         Matrix result;
 252         result.matrix[0] = 2*n/(r-l);
 253         result.matrix[5] = 2*n/(t-b);
 254         result.matrix[8] = (r+l)/(r-l);
 255         result.matrix[9] = (t+b)/(t-b);
 256         result.matrix[10] = -(f+n)/(f-n);
 257         result.matrix[11] = -1;
 258         result.matrix[14] = -2*f*n/(f-n);
 259         result.matrix[15] = 0;
 260         result.flags = ARBITARY;
 261         return result;
 262 }
 263
 264 Matrix Matrix::frustum_centered(double w, double h, double n, double f)
 265 {
 266         return frustum(-w/2, w/2, -h/2, h/2, n, f);
 267 }
 268
 269 Matrix Matrix::perspective(double h, double a, double n, double f)
 270 {
 271         double hh = tan(h/2)*n;
 272         return frustum(-hh*a, hh*a, -hh, hh, n, f);
 273 }
 274
 275
 276 GLenum MatrixStack::current_mode = GL_MODELVIEW;
 277
 278 MatrixStack::MatrixStack(GLenum m):
 279         mode(m)
 280 {
 281         matrices.reserve(mode==GL_MODELVIEW ? 32 : 4);
 282         matrices.push_back(Matrix());
 283 }
 284
 285 MatrixStack::MatrixStack():
 286         mode(0)
 287 {
 288         matrices.reserve(32);
 289         matrices.push_back(Matrix());
 290 }
 291
 292 const Matrix &MatrixStack::top() const
 293 {
 294         return matrices.back();
 295 }
 296
 297 void MatrixStack::load(const Matrix &m)
 298 {
 299         matrices.back() = m;
 300         update();
 301 }
 302
 303 void MatrixStack::multiply(const Matrix &m)
 304 {
 305         matrices.back() *= m;
 306         update();
 307 }
 308
 309 void MatrixStack::push()
 310 {
 311         matrices.push_back(top());
 312 }
 313
 314 void MatrixStack::pop()
 315 {
 316         if(matrices.size()==1)
 317                 throw stack_underflow("MatrixStack::pop()");
 318
 319         matrices.pop_back();
 320         update();
 321 }
 322
 323 void MatrixStack::update()
 324 {
 325         if(!mode)
 326                 return;
 327
 328         if(mode!=current_mode)
 329         {
 330                 glMatrixMode(mode);
 331                 current_mode = mode;
 332         }
 333
 334         glLoadMatrixd(matrices.back().data());
 335 }
 336
 337 MatrixStack &MatrixStack::operator=(const Matrix &m)
 338 {
 339         load(m);
 340         return *this;
 341 }
 342
 343 MatrixStack &MatrixStack::operator*=(const Matrix &m)
 344 {
 345         multiply(m);
 346         return *this;
 347 }
 348
 349 MatrixStack &MatrixStack::modelview()
 350 {
 351         static MatrixStack ms(GL_MODELVIEW);
 352         return ms;
 353 }
 354
 355 MatrixStack &MatrixStack::projection()
 356 {
 357         static MatrixStack ms(GL_PROJECTION);
 358         return ms;
 359 }
 360
 361
 362 // Deprecated stuff
 363
 364 MatrixStack *active_stack = &MatrixStack::modelview();
 365
 366 void matrix_mode(MatrixMode m)
 367 {
 368         if(m==MODELVIEW)
 369                 active_stack = &MatrixStack::modelview();
 370         else if(m==PROJECTION)
 371                 active_stack = &MatrixStack::projection();
 372         else
 373                 throw invalid_argument("matrix_mode");
 374 }
 375
 376 void load_identity()
 377 {
 378         *active_stack = Matrix();
 379 }
 380
 381 void load_matrix(const float *matrix)
 382 {
 383         *active_stack = Matrix(matrix);
 384 }
 385
 386 void load_matrix(const double *matrix)
 387 {
 388         *active_stack = Matrix(matrix);
 389 }
 390
 391 void mult_matrix(const float *matrix)
 392 {
 393         *active_stack *= Matrix(matrix);
 394 }
 395
 396 void mult_matrix(const double *matrix)
 397 {
 398         *active_stack *= Matrix(matrix);
 399 }
 400
 401 void push_matrix()
 402 {
 403         active_stack->push();
 404 }
 405
 406 void pop_matrix()
 407 {
 408         active_stack->pop();
 409 }
 410
 411 void translate(float x, float y, float z)
 412 {
 413         *active_stack *= Matrix::translation(x, y, z);
 414 }
 415
 416 void rotate(float a, float x, float y, float z)
 417 {
 418         *active_stack *= Matrix::rotation_deg(a, x, y, z);
 419 }
 420
 421 void scale(float x, float y, float z)
 422 {
 423         *active_stack *= Matrix::scaling(x, y, z);
 424 }
 425
 426 void scale_uniform(float s)
 427 {
 428         *active_stack *= Matrix::scaling(s);
 429 }
 430
 431 } // namespace GL
 432 } // namespace Msp