git.mutantstargoat.com Git - vkeyb/blob - src/matrix.cc

   1 /*
   2 vkeyb - camera motion detection virtual keyboard
   3 Copyright (C) 2012 Eleni Maria Stea <elene.mst@gmail.com>
   4
   5 This program is free software: you can redistribute it and/or modify
   6 it under the terms of the GNU General Public License as published by
   7 the Free Software Foundation, either version 3 of the License, or
   8 (at your option) any later version.
   9
  10 This program is distributed in the hope that it will be useful,
  11 but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 GNU General Public License for more details.
  14
  15 You should have received a copy of the GNU General Public License
  16 along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17 */
  18
  19 #include <math.h>
  20 #include <stdio.h>
  21 #include <string.h>
  22 #include "matrix.h"
  23 #include "vector.h"
  24
  25 Matrix4x4::Matrix4x4() {
  26         for (int i=0; i<4; i++) {
  27                 for (int j=0; j<4; j++) {
  28                         matrix[i][j] = (i == j ? 1 : 0);
  29                 }
  30         }
  31 }
  32
  33 void Matrix4x4::set_translation(const Vector3 &tr) {
  34         matrix[0][3] = tr.x;
  35         matrix[1][3] = tr.y;
  36         matrix[2][3] = tr.z;
  37 }
  38
  39 void Matrix4x4::set_rotation(const Vector3 &axis, double angle) {
  40         double sina = sin(angle);
  41         double cosa = cos(angle);
  42         double invcosa = 1 - cosa;
  43         double sqx = axis.x * axis.x;
  44         double sqy = axis.y * axis.y;
  45         double sqz = axis.z * axis.z;
  46
  47         matrix[0][0] = sqx + (1 - sqx) * cosa;
  48         matrix[0][1] = axis.x * axis.y * invcosa + axis.z * sina;
  49         matrix[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
  50         matrix[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
  51         matrix[1][1] = sqy + (1 - sqy) * cosa;
  52         matrix[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
  53         matrix[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
  54         matrix[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
  55         matrix[2][2] = sqz + (1 - sqz) * cosa;
  56 }
  57
  58 void Matrix4x4::set_scaling(const Vector3 &sc) {
  59         matrix[0][0] = sc.x;
  60         matrix[1][1] = sc.y;
  61         matrix[2][2] = sc.z;
  62 }
  63
  64 void Matrix4x4::transpose() {
  65         double m[4][4];
  66
  67         memcpy(m, matrix, sizeof m);
  68
  69         for(int i=0; i<4; i++) {
  70                 for(int j=0; j<4; j++) {
  71                         matrix[i][j] = m[j][i];
  72                 }
  73         }
  74 }
  75
  76 void Matrix4x4::print() {
  77         printf("\n");
  78         for (int i=0; i<4; i++) {
  79                 for (int j=0; j<4; j++) {
  80                         printf("%f", matrix[i][j]);
  81                         char nxt = (j%4 == 3 ? '\n' : '\t');
  82                         printf("%c", nxt);
  83                 }
  84         }
  85         printf("\n");
  86 }
  87