maths/Matrix.cpp

   1 /***************************************************************************
   2  *   This file is part of Tecorrec.                                        *
   3  *   Copyright 2008 James Hogan <james@albanarts.com>                      *
   4  *                                                                         *
   5  *   Tecorrec 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 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   Tecorrec 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 Tecorrec.  If not, write to the Free Software Foundation,  *
  17  *   Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.   *
  18  ***************************************************************************/
  19
  20 /**
  21  * @file Matrix.cpp
  22  * @brief Square matrix class.
  23  * @author James Hogan (james@albanarts.com)
  24  * @note Copyright (C) 2007
  25  *
  26  * @version 1 : From Computer Graphics and Visualisation Open Assessment.
  27  */
  28
  29 #include "Matrix.h"
  30 #include "Quaternion.h"
  31
  32 namespace maths {
  33
  34   template <>
  35   Matrix<3,float>::Matrix(const Quaternion<float> & q)
  36   {
  37     typedef float T;
  38     T xy = q[0]*q[1];
  39     T xz = q[0]*q[2];
  40     T yy = q[1]*q[1];
  41     T yw = q[1]*q[3];
  42     T zz = q[2]*q[2];
  43     T zw = q[2]*q[3];
  44     setVector3(col[0].v, 1-2*(yy+zz),   2*(xy+zw),   2*(xz-yw));
  45     T xx = q[0]*q[0];
  46     T xw = q[0]*q[3];
  47     T yz = q[1]*q[2];
  48     setVector3(col[1].v,  2*(xy-zw), 1-2*(xx+zz),   2*(yz+xw));
  49     setVector3(col[2].v,  2*(xz+yw),   2*(yz-xw), 1-2*(xx+yy));
  50   }
  51
  52   /// Get an arbitrary matrix where the z axis points in the direction of Z.
  53   Matrix<3,float> getTbnMatrix(Vector<3, float> vZ)
  54   {
  55     Vector<3,float> vY(0, 0, -1);
  56     Vector<3,float> vX(cross(vY, vZ));
  57     if (vX.zero()) {
  58       vY.set(0,1,0);
  59       vX = cross(vY, vZ);
  60     }
  61     vX.normalize();
  62     vY = cross(vZ, vX);
  63     return Matrix<3,float>(vX, vY, vZ);
  64   }
  65
  66   // Invert the 3x3 matrix
  67   /// not written by me
  68   template <>
  69   Matrix<3,float> & Matrix<3,float>::invert()
  70   {
  71     Matrix a = *this;
  72     Matrix b;
  73     b.identity();
  74
  75     unsigned int c, r;
  76     unsigned int cc;
  77     unsigned int rowMax; // Points to max abs value row in this column
  78     unsigned int row;
  79     float tmp;
  80
  81     // Go through columns
  82     for (c=0; c<3; c++) {
  83       // Find the row with max value in this column
  84       rowMax = c;
  85       for (r=c+1; r<3; r++) {
  86         if (fabs(a[c][r]) > fabs(a[c][rowMax])) {
  87           rowMax = r;
  88         }
  89       }
  90
  91       // If the max value here is 0, we can't invert.  Return identity.
  92       if (a[rowMax][c] == 0.0F)
  93         return(identity());
  94
  95       // Swap row "rowMax" with row "c"
  96       for (cc=0; cc<3; cc++)
  97       {
  98         tmp = a[cc][c];
  99         a[cc][c] = a[cc][rowMax];
 100         a[cc][rowMax] = tmp;
 101         tmp = b[cc][c];
 102         b[cc][c] = b[cc][rowMax];
 103         b[cc][rowMax] = tmp;
 104       }
 105
 106       // Now everything we do is on row "c".
 107       // Set the max cell to 1 by dividing the entire row by that value
 108       tmp = a[c][c];
 109       for (cc=0; cc<3; cc++) {
 110         a[cc][c] /= tmp;
 111         b[cc][c] /= tmp;
 112       }
 113
 114       // Now do the other rows, so that this column only has a 1 and 0's
 115       for (row = 0; row < 3; row++) {
 116         if (row != c) {
 117           tmp = a[c][row];
 118           for (cc=0; cc<3; cc++) {
 119             a[cc][row] -= a[cc][c] * tmp;
 120             b[cc][row] -= b[cc][c] * tmp;
 121           }
 122         }
 123       }
 124
 125     }
 126
 127     *this = b;
 128
 129     return *this;
 130   }
 131
 132   // Invert the Matrix<4,float>
 133   template <>
 134   Matrix<4,float> & Matrix<4,float>::invert()
 135   {
 136     Matrix<4,float> a(*this);
 137     Matrix<4,float> b;
 138     b.identity();
 139
 140     unsigned int r, c;
 141     unsigned int cc;
 142     unsigned int rowMax; // Points to max abs value row in this column
 143     unsigned int row;
 144     float tmp;
 145
 146     // Go through columns
 147     for (c=0; c<4; c++) {
 148       // Find the row with max value in this column
 149       rowMax = c;
 150       for (r=c+1; r<4; r++) {
 151         if (fabs(a[c][r]) > fabs(a[c][rowMax])) {
 152           rowMax = r;
 153         }
 154       }
 155
 156       // If the max value here is 0, we can't invert.  Return identity.
 157       //if (a[rowMax][c] == 0.0F)
 158       //  return(identity());
 159
 160       // Swap row "rowMax" with row "c"
 161       for (cc=0; cc<4; cc++) {
 162         tmp = a[cc][c];
 163         a[cc][c] = a[cc][rowMax];
 164         a[cc][rowMax] = tmp;
 165         tmp = b[cc][c];
 166         b[cc][c] = b[cc][rowMax];
 167         b[cc][rowMax] = tmp;
 168       }
 169
 170       // Now everything we do is on row "c".
 171       // Set the max cell to 1 by dividing the entire row by that value
 172       tmp = a[c][c];
 173       for (cc=0; cc<4; cc++) {
 174         a[cc][c] /= tmp;
 175         b[cc][c] /= tmp;
 176       }
 177
 178       // Now do the other rows, so that this column only has a 1 and 0's
 179       for (row = 0; row < 4; row++) {
 180         if (row != c) {
 181           tmp = a[c][row];
 182           for (cc=0; cc<4; cc++) {
 183             a[cc][row] -= a[cc][c] * tmp;
 184             b[cc][row] -= b[cc][c] * tmp;
 185           }
 186         }
 187       }
 188     }
 189
 190     *this = b;
 191
 192     return *this;
 193   }
 194
 195
 196   // Return a 3D axis-rotation maths::Matrix<4,float>
 197   // Pass in 'x', 'y', or 'z' for the axis.
 198   Matrix<4,float> RotateRadMatrix44(char axis, float rad)
 199   {
 200     Matrix<4,float> ret;
 201     float sinA, cosA;
 202
 203     sinA = sin(rad);
 204     cosA = cos(rad);
 205
 206     switch (axis)
 207     {
 208       case 'x':
 209       case 'X':
 210         ret[0][0] =  1.0F; ret[1][0] =  0.0F; ret[2][0] =  0.0F;
 211         ret[0][1] =  0.0F; ret[1][1] =  cosA; ret[2][1] = -sinA;
 212         ret[0][2] =  0.0F; ret[1][2] =  sinA; ret[2][2] =  cosA;
 213         break;
 214
 215       case 'y':
 216       case 'Y':
 217         ret[0][0] =  cosA; ret[1][0] =  0.0F; ret[2][0] =  sinA;
 218         ret[0][1] =  0.0F; ret[1][1] =  1.0F; ret[2][1] =  0.0F;
 219         ret[0][2] = -sinA; ret[1][2] =  0.0F; ret[2][2] =  cosA;
 220         break;
 221
 222       case 'z':
 223       case 'Z':
 224         ret[0][0] =  cosA; ret[1][0] = -sinA; ret[2][0] =  0.0F;
 225         ret[0][1] =  sinA; ret[1][1] =  cosA; ret[2][1] =  0.0F;
 226         ret[0][2] =  0.0F; ret[1][2] =  0.0F; ret[2][2] =  1.0F;
 227         break;
 228     }
 229
 230     ret[0][3] = 0.0F; ret[1][3] = 0.0F; ret[2][3] = 0.0F;
 231     ret[3][0] = 0.0F;
 232     ret[3][1] = 0.0F;
 233     ret[3][2] = 0.0F;
 234     ret[3][3] = 1.0F;
 235
 236     return ret;
 237   }
 238
 239   // Return a 3D axis-rotation maths::Matrix<4,float>
 240   // Pass in an arbitrary maths::Vector<3,float> axis.
 241   Matrix<4,float> RotateRadMatrix44(const Vector<3,float> &axis, float rad)
 242   {
 243     Matrix<4,float> ret;
 244     float sinA, cosA;
 245     float invCosA;
 246     Vector<3,float> nrm = axis;
 247     float x, y, z;
 248     float xSq, ySq, zSq;
 249
 250     nrm.normalize();
 251     sinA = sin(rad);
 252     cosA = cos(rad);
 253     invCosA = 1.0F - cosA;
 254
 255     x = nrm[0];
 256     y = nrm[1];
 257     z = nrm[2];
 258
 259     xSq = x * x;
 260     ySq = y * y;
 261     zSq = z * z;
 262
 263     ret[0][0] = (invCosA * xSq) + (cosA);
 264     ret[1][0] = (invCosA * x * y) - (sinA * z );
 265     ret[2][0] = (invCosA * x * z) + (sinA * y );
 266     ret[3][0] = 0.0F;
 267
 268     ret[0][1] = (invCosA * x * y) + (sinA * z);
 269     ret[1][1] = (invCosA * ySq) + (cosA);
 270     ret[2][1] = (invCosA * y * z) - (sinA * x);
 271     ret[3][1] = 0.0F;
 272
 273     ret[0][2] = (invCosA * x * z) - (sinA * y);
 274     ret[1][2] = (invCosA * y * z) + (sinA * x);
 275     ret[2][2] = (invCosA * zSq) + (cosA);
 276     ret[3][2] = 0.0F;
 277
 278     ret[0][3] = 0.0F;
 279     ret[1][3] = 0.0F;
 280     ret[2][3] = 0.0F;
 281     ret[3][3] = 1.0F;
 282
 283     return ret;
 284   }
 285
 286   // Return a 3D translation Matrix<4,float>
 287   Matrix<4,float> TranslateMatrix44(const maths::Vector<3,float> & v)
 288   {
 289     Matrix<4,float> ret;
 290
 291     ret.identity();
 292     ret[3].slice<0,3>() = v;
 293
 294     return ret;
 295   }
 296
 297   // Return a 3D/4D scale Matrix<4,float>
 298   Matrix<4,float> ScaleMatrix44(const maths::Vector<4,float> & v)
 299   {
 300     Matrix<4,float> ret;
 301
 302     ret.identity();
 303     for (int i = 0; i < 4; ++i) {
 304       ret[i][i] = v[i];
 305     }
 306
 307     return ret;
 308   }
 309 }