matrix.c

   1 /**
   2  * @file matrix.c
   3  * @brief The implementation of the gljewel matrix module.
   4  *
   5  * Copyright 2001, 2008 David Ashley  <dashxdr@gmail.com>
   6  * Copyright 2008 Stephen M. Webb  <stephen.webb@bregmasoft.ca>
   7  *
   8  * This program is free software; you can redistribute it and/or modify
   9  * it under the terms of Version 2 of the GNU General Public License as
  10  * published by the Free Software Foundation.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program; if not, write to the Free Software
  19  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  20  */
  21
  22 #include <math.h>
  23 #include <string.h>
  24 #include <stdio.h>
  25
  26 #include "matrix.h"
  27
  28 #define PI 3.1415926f
  29
  30 matrix identityquick={
  31 {1.0f, 0.0f, 0.0f, 0.0f},
  32 {0.0f, 1.0f, 0.0f, 0.0f},
  33 {0.0f, 0.0f, 1.0f, 0.0f},
  34 {0.0f, 0.0f, 0.0f, 1.0f}
  35 };
  36
  37 matrix zeroquick={
  38 {0.0f, 0.0f, 0.0f, 0.0f},
  39 {0.0f, 0.0f, 0.0f, 0.0f},
  40 {0.0f, 0.0f, 0.0f, 0.0f},
  41 {0.0f, 0.0f, 0.0f, 0.0f}
  42 };
  43
  44 void identitymatrix(matrix *dest)
  45 {
  46         memcpy(dest,&identityquick,sizeof(matrix));
  47 }
  48 void zeromatrix(matrix *dest)
  49 {
  50         memcpy(dest,&zeroquick,sizeof(matrix));
  51 }
  52
  53 void matrixXmatrix(matrix *dest,matrix *left,matrix *right)
  54 {
  55 int i,j,k;
  56 matrix temp;
  57 float t;
  58
  59         for(i=0;i<4;++i)
  60                 for(j=0;j<4;++j)
  61                 {
  62                         t=0;
  63                         for(k=0;k<4;++k)
  64                                 t+=(*left)[k][j]*(*right)[i][k];
  65                         temp[i][j]=t;
  66                 }
  67         memcpy(dest,&temp,sizeof(matrix));
  68 }
  69 void matrixXvector(vector *dest,matrix *left,vector *right)
  70 {
  71 int i,j;
  72 vector temp;
  73 float t;
  74         for(i=0;i<4;++i)
  75         {
  76                 t=0;
  77                 for(j=0;j<4;++j)
  78                         t+=(*left)[j][i]*(*right)[j];
  79                 temp[i]=t;
  80         }
  81         memcpy(dest,&temp,sizeof(vector));
  82 }
  83 void rotationmatrix(matrix *dest,float angle,float x,float y,float z)
  84 {
  85 float mag,s,c;
  86 float c1,xx,yy,zz,xyc1,xzc1,yzc1,xs,ys,zs;
  87
  88         mag=sqrtf(x*x+y*y+z*z);
  89         if(mag==0.0)
  90         {
  91                 identitymatrix(dest);
  92                 return;
  93         }
  94         x/=mag;
  95         y/=mag;
  96         z/=mag;
  97         angle*=PI/180.0f;
  98         s=sinf(angle);
  99         c=cosf(angle);
 100         c1=1.0f-c;
 101         xx=x*x;
 102         yy=y*y;
 103         zz=z*z;
 104         xyc1=x*y*c1;
 105         xzc1=x*z*c1;
 106         yzc1=y*z*c1;
 107         xs=x*s;
 108         ys=y*s;
 109         zs=z*s;
 110
 111         (*dest)[0][0]=xx*c1+c;
 112         (*dest)[1][0]=xyc1-zs;
 113         (*dest)[2][0]=xzc1+ys;
 114         (*dest)[3][0]=0.0f;
 115         (*dest)[0][1]=xyc1+zs;
 116         (*dest)[1][1]=yy*c1+c;
 117         (*dest)[2][1]=yzc1-xs;
 118         (*dest)[3][1]=0.0f;
 119         (*dest)[0][2]=xzc1-ys;
 120         (*dest)[1][2]=yzc1+xs;
 121         (*dest)[2][2]=zz*c1+c;
 122         (*dest)[3][2]=0.0f;
 123         (*dest)[0][3]=0.0f;
 124         (*dest)[1][3]=0.0f;
 125         (*dest)[2][3]=0.0f;
 126         (*dest)[3][3]=1.0f;
 127 }
 128
 129 void xrotmatrix(matrix *dest,float angle)
 130 {
 131 matrix temp;
 132 float c,s;
 133         identitymatrix(&temp);
 134         angle*=PI/180.0f;
 135         c=cosf(angle);
 136         s=sinf(angle);
 137         temp[1][1]=c;
 138         temp[2][1]=-s;
 139         temp[1][2]=s;
 140         temp[2][2]=c;
 141         matrixXmatrix(dest,&temp,dest);
 142 }
 143 void yrotmatrix(matrix *dest,float angle)
 144 {
 145 matrix temp;
 146 float c,s;
 147         identitymatrix(&temp);
 148         angle*=PI/180.0f;
 149         c=cosf(angle);
 150         s=sinf(angle);
 151         temp[0][0]=c;
 152         temp[2][0]=s;
 153         temp[0][2]=-s;
 154         temp[2][2]=c;
 155         matrixXmatrix(dest,&temp,dest);
 156 }
 157 void zrotmatrix(matrix *dest,float angle)
 158 {
 159 matrix temp;
 160 float c,s;
 161         identitymatrix(&temp);
 162         angle*=PI/180.0f;
 163         c=cosf(angle);
 164         s=sinf(angle);
 165         temp[0][0]=c;
 166         temp[1][0]=-s;
 167         temp[0][1]=s;
 168         temp[1][1]=c;
 169         matrixXmatrix(dest,&temp,dest);
 170 }
 171
 172 float myabs(float x)
 173 {
 174         return (x<0.0) ? -x : x;
 175 }
 176
 177 void swaprows(matrix *dest,int r1,int r2)
 178 {
 179 int i;
 180 float t;
 181         for(i=0;i<4;++i)
 182         {
 183                 t=(*dest)[i][r1];
 184                 (*dest)[i][r1]=(*dest)[i][r2];
 185                 (*dest)[i][r2]=t;
 186         }
 187 }
 188 void scalerow(matrix *dest,int r1,float scale)
 189 {
 190 int i;
 191         for(i=0;i<4;++i)
 192                 (*dest)[i][r1]*=scale;
 193 }
 194 void addscalerow(matrix *dest,int r1,int r2,float scale)
 195 {
 196 int i;
 197         for(i=0;i<4;++i)
 198                 (*dest)[i][r1]+=(*dest)[i][r2]*scale;
 199 }
 200
 201 void printmatrix(matrix *m)
 202 {
 203 int i,j;
 204         printf("---\n");
 205         for(j=0;j<4;++j)
 206         {
 207                 for(i=0;i<4;++i)
 208                         printf(" %7.3f",(*m)[i][j]);
 209                 printf("\n");
 210         }
 211 }
 212 void invertmatrix(matrix *dest,matrix *source)
 213 {
 214 matrix temp;
 215 int i,k;
 216 float max=0;
 217 int maxnum = 0;
 218 float t;
 219
 220         identitymatrix(dest);
 221         memcpy(temp,source,sizeof(temp));
 222         for(k=0;k<3;++k)
 223         {
 224                 max=-1;
 225                 for(i=k;i<4;++i)
 226                 {
 227                         t=myabs(temp[k][i]);
 228                         if(t>max)
 229                         {
 230                                 max=t;
 231                                 maxnum=i;
 232                         }
 233                 }
 234                 if(maxnum!=k)
 235                 {
 236                         swaprows(dest,k,maxnum);
 237                         swaprows(&temp,k,maxnum);
 238                 }
 239                 t=1.0f/temp[k][k];
 240                 scalerow(dest,k,t);
 241                 scalerow(&temp,k,t);
 242                 for(i=k+1;i<4;++i)
 243                 {
 244                         t=-temp[k][i];
 245                         if(t!=0.0f)
 246                         {
 247                                 addscalerow(dest,i,k,t);
 248                                 addscalerow(&temp,i,k,t);
 249                         }
 250                 }
 251         }
 252         for(k=3;k>0;--k)
 253         {
 254                 for(i=0;i<k;++i)
 255                 {
 256                         t=-temp[k][i];
 257                         if(t!=0.0f)
 258                         {
 259                                 addscalerow(dest,i,k,t);
 260                                 addscalerow(&temp,i,k,t);
 261                         }
 262                 }
 263         }
 264 }