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(bool* singular)
  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     if (0 != singular)
  82     {
  83       *singular = false;
  84     }
  85
  86     // Go through columns
  87     for (c=0; c<3; c++) {
  88       // Find the row with max value in this column
  89       rowMax = c;
  90       for (r=c+1; r<3; r++) {
  91         if (fabs(a[c][r]) > fabs(a[c][rowMax])) {
  92           rowMax = r;
  93         }
  94       }
  95
  96       // If the max value here is 0, we can't invert.  Return identity.
  97       if (a[rowMax][c] == 0.0F)
  98       {
  99         if (0 != singular)
 100         {
 101           *singular = true;
 102         }
 103         return(identity());
 104       }
 105
 106       // Swap row "rowMax" with row "c"
 107       for (cc=0; cc<3; cc++)
 108       {
 109         tmp = a[cc][c];
 110         a[cc][c] = a[cc][rowMax];
 111         a[cc][rowMax] = tmp;
 112         tmp = b[cc][c];
 113         b[cc][c] = b[cc][rowMax];
 114         b[cc][rowMax] = tmp;
 115       }
 116
 117       // Now everything we do is on row "c".
 118       // Set the max cell to 1 by dividing the entire row by that value
 119       tmp = a[c][c];
 120       for (cc=0; cc<3; cc++) {
 121         a[cc][c] /= tmp;
 122         b[cc][c] /= tmp;
 123       }
 124
 125       // Now do the other rows, so that this column only has a 1 and 0's
 126       for (row = 0; row < 3; row++) {
 127         if (row != c) {
 128           tmp = a[c][row];
 129           for (cc=0; cc<3; cc++) {
 130             a[cc][row] -= a[cc][c] * tmp;
 131             b[cc][row] -= b[cc][c] * tmp;
 132           }
 133         }
 134       }
 135
 136     }
 137
 138     *this = b;
 139
 140     return *this;
 141   }
 142
 143   // Invert the Matrix<4,float>
 144   template <>
 145   Matrix<4,float> & Matrix<4,float>::invert(bool* singular)
 146   {
 147     Matrix<4,float> a(*this);
 148     Matrix<4,float> b;
 149     b.identity();
 150
 151     unsigned int r, c;
 152     unsigned int cc;
 153     unsigned int rowMax; // Points to max abs value row in this column
 154     unsigned int row;
 155     float tmp;
 156
 157     if (0 != singular)
 158     {
 159       *singular = false;
 160     }
 161
 162     // Go through columns
 163     for (c=0; c<4; c++) {
 164       // Find the row with max value in this column
 165       rowMax = c;
 166       for (r=c+1; r<4; r++) {
 167         if (fabs(a[c][r]) > fabs(a[c][rowMax])) {
 168           rowMax = r;
 169         }
 170       }
 171
 172       // If the max value here is 0, we can't invert.  Return identity.
 173       if (a[rowMax][c] == 0.0F)
 174       {
 175         if (0 != singular)
 176         {
 177           *singular = true;
 178         }
 179         return(identity());
 180       }
 181
 182       // Swap row "rowMax" with row "c"
 183       for (cc=0; cc<4; cc++) {
 184         tmp = a[cc][c];
 185         a[cc][c] = a[cc][rowMax];
 186         a[cc][rowMax] = tmp;
 187         tmp = b[cc][c];
 188         b[cc][c] = b[cc][rowMax];
 189         b[cc][rowMax] = tmp;
 190       }
 191
 192       // Now everything we do is on row "c".
 193       // Set the max cell to 1 by dividing the entire row by that value
 194       tmp = a[c][c];
 195       for (cc=0; cc<4; cc++) {
 196         a[cc][c] /= tmp;
 197         b[cc][c] /= tmp;
 198       }
 199
 200       // Now do the other rows, so that this column only has a 1 and 0's
 201       for (row = 0; row < 4; row++) {
 202         if (row != c) {
 203           tmp = a[c][row];
 204           for (cc=0; cc<4; cc++) {
 205             a[cc][row] -= a[cc][c] * tmp;
 206             b[cc][row] -= b[cc][c] * tmp;
 207           }
 208         }
 209       }
 210     }
 211
 212     *this = b;
 213
 214     return *this;
 215   }
 216
 217
 218   // Return a 3D axis-rotation maths::Matrix<4,float>
 219   // Pass in 'x', 'y', or 'z' for the axis.
 220   Matrix<4,float> RotateRadMatrix44(char axis, float rad)
 221   {
 222     Matrix<4,float> ret;
 223     float sinA, cosA;
 224
 225     sinA = sin(rad);
 226     cosA = cos(rad);
 227
 228     switch (axis)
 229     {
 230       case 'x':
 231       case 'X':
 232         ret[0][0] =  1.0F; ret[1][0] =  0.0F; ret[2][0] =  0.0F;
 233         ret[0][1] =  0.0F; ret[1][1] =  cosA; ret[2][1] = -sinA;
 234         ret[0][2] =  0.0F; ret[1][2] =  sinA; ret[2][2] =  cosA;
 235         break;
 236
 237       case 'y':
 238       case 'Y':
 239         ret[0][0] =  cosA; ret[1][0] =  0.0F; ret[2][0] =  sinA;
 240         ret[0][1] =  0.0F; ret[1][1] =  1.0F; ret[2][1] =  0.0F;
 241         ret[0][2] = -sinA; ret[1][2] =  0.0F; ret[2][2] =  cosA;
 242         break;
 243
 244       case 'z':
 245       case 'Z':
 246         ret[0][0] =  cosA; ret[1][0] = -sinA; ret[2][0] =  0.0F;
 247         ret[0][1] =  sinA; ret[1][1] =  cosA; ret[2][1] =  0.0F;
 248         ret[0][2] =  0.0F; ret[1][2] =  0.0F; ret[2][2] =  1.0F;
 249         break;
 250     }
 251
 252     ret[0][3] = 0.0F; ret[1][3] = 0.0F; ret[2][3] = 0.0F;
 253     ret[3][0] = 0.0F;
 254     ret[3][1] = 0.0F;
 255     ret[3][2] = 0.0F;
 256     ret[3][3] = 1.0F;
 257
 258     return ret;
 259   }
 260
 261   // Return a 3D axis-rotation maths::Matrix<4,float>
 262   // Pass in an arbitrary maths::Vector<3,float> axis.
 263   Matrix<4,float> RotateRadMatrix44(const Vector<3,float> &axis, float rad)
 264   {
 265     Matrix<4,float> ret;
 266     float sinA, cosA;
 267     float invCosA;
 268     Vector<3,float> nrm = axis;
 269     float x, y, z;
 270     float xSq, ySq, zSq;
 271
 272     nrm.normalize();
 273     sinA = sin(rad);
 274     cosA = cos(rad);
 275     invCosA = 1.0F - cosA;
 276
 277     x = nrm[0];
 278     y = nrm[1];
 279     z = nrm[2];
 280
 281     xSq = x * x;
 282     ySq = y * y;
 283     zSq = z * z;
 284
 285     ret[0][0] = (invCosA * xSq) + (cosA);
 286     ret[1][0] = (invCosA * x * y) - (sinA * z );
 287     ret[2][0] = (invCosA * x * z) + (sinA * y );
 288     ret[3][0] = 0.0F;
 289
 290     ret[0][1] = (invCosA * x * y) + (sinA * z);
 291     ret[1][1] = (invCosA * ySq) + (cosA);
 292     ret[2][1] = (invCosA * y * z) - (sinA * x);
 293     ret[3][1] = 0.0F;
 294
 295     ret[0][2] = (invCosA * x * z) - (sinA * y);
 296     ret[1][2] = (invCosA * y * z) + (sinA * x);
 297     ret[2][2] = (invCosA * zSq) + (cosA);
 298     ret[3][2] = 0.0F;
 299
 300     ret[0][3] = 0.0F;
 301     ret[1][3] = 0.0F;
 302     ret[2][3] = 0.0F;
 303     ret[3][3] = 1.0F;
 304
 305     return ret;
 306   }
 307
 308   // Return a 3D translation Matrix<4,float>
 309   Matrix<4,float> TranslateMatrix44(const maths::Vector<3,float> & v)
 310   {
 311     Matrix<4,float> ret;
 312
 313     ret.identity();
 314     ret[3].slice<0,3>() = v;
 315
 316     return ret;
 317   }
 318
 319   // Return a 3D/4D scale Matrix<4,float>
 320   Matrix<4,float> ScaleMatrix44(const maths::Vector<4,float> & v)
 321   {
 322     Matrix<4,float> ret;
 323
 324     ret.identity();
 325     for (int i = 0; i < 4; ++i) {
 326       ret[i][i] = v[i];
 327     }
 328
 329     return ret;
 330   }
 331 }