00001 /*
00002  * vp_linalg.c
00003  *
00004  * A simple linear algebra package.
00005  *
00006  * Copyright (c) 1994 The Board of Trustees of The Leland Stanford
00007  * Junior University.  All rights reserved.
00008  *
00009  * Permission to use, copy, modify and distribute this software and its
00010  * documentation for any purpose is hereby granted without fee, provided
00011  * that the above copyright notice and this permission notice appear in
00012  * all copies of this software and that you do not sell the software.
00013  * Commercial licensing is available by contacting the author.
00014  * 
00015  * THE SOFTWARE IS PROVIDED "AS IS" AND WITHOUT WARRANTY OF ANY KIND,
00016  * EXPRESS, IMPLIED OR OTHERWISE, INCLUDING WITHOUT LIMITATION, ANY
00017  * WARRANTY OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE.
00018  *
00019  * Author:
00020  *    Phil Lacroute
00021  *    Computer Systems Laboratory
00022  *    Electrical Engineering Dept.
00023  *    Stanford University
00024  */
00025 
00026 /*
00027  * $Date: 2001/12/17 16:16:23 $
00028  * $Revision: 1.1 $
00029  */
00030 
00031 #include "vp_global.h"
00032 
00033 static void MatrixMult ANSI_ARGS((double* p, double *a, double *b,
00034                                   int l, int m, int n));
00035 
00036 /*
00037  * vpIdentity3
00038  *
00039  * Initialize a Matrix3 to the identity.
00040  */
00041 
00042 void
00043 vpIdentity3(m)
00044 vpMatrix3 m;
00045 {
00046     m[0][0] = 1.;    m[0][1] = 0.;    m[0][2] = 0.;
00047     m[1][0] = 0.;    m[1][1] = 1.;    m[1][2] = 0.;
00048     m[2][0] = 0.;    m[2][1] = 0.;    m[2][2] = 1.;
00049 }
00050 
00051 /*
00052  * vpIdentity4
00053  *
00054  * Initialize a Matrix4 to the identity.
00055  */
00056 
00057 void
00058 vpIdentity4(m)
00059 vpMatrix4 m;
00060 {
00061     m[0][0] = 1.;    m[0][1] = 0.;    m[0][2] = 0.;    m[0][3] = 0.;
00062     m[1][0] = 0.;    m[1][1] = 1.;    m[1][2] = 0.;    m[1][3] = 0.;
00063     m[2][0] = 0.;    m[2][1] = 0.;    m[2][2] = 1.;    m[2][3] = 0.;
00064     m[3][0] = 0.;    m[3][1] = 0.;    m[3][2] = 0.;    m[3][3] = 1.;
00065 }
00066 
00067 /*
00068  * vpNormalize3
00069  *
00070  * Normalize a vector (divide it by its magnitude).  Return VPERROR_SINGULAR
00071  * if the magnitude is too small.
00072  */
00073 
00074 vpResult
00075 vpNormalize3(v)
00076 vpVector3 v;
00077 {
00078     double magsqr, invmag;
00079     int i;
00080 
00081     magsqr = 0.;
00082     for (i = 0; i < 3; i++)
00083         magsqr += v[i]*v[i];
00084     if (fabs(magsqr) < VP_EPS)
00085         return(VPERROR_SINGULAR);
00086     invmag = 1. / sqrt(magsqr);
00087     for (i = 0; i < 3; i++)
00088         v[i] *= invmag;
00089     return(VP_OK);
00090 }
00091 
00092 /*
00093  * vpMatrixVectorMult4
00094  *
00095  * Perform the matrix-vector multiplication v2 = m*v1.
00096  */
00097 
00098 void
00099 vpMatrixVectorMult4(v2, m, v1)
00100 vpVector4 v2, v1;
00101 vpMatrix4 m;
00102 {
00103     int i, j;
00104 
00105     for (i = 0; i < 4; i++) {
00106         v2[i] = 0;
00107         for (j = 0; j < 4; j++)
00108             v2[i] += m[i][j] * v1[j];
00109     }
00110 }
00111 
00112 /*
00113  * vpMatrixMult4
00114  *
00115  * Perform the matrix multiplication m3 = m2 * m1.
00116  */
00117 
00118 void
00119 vpMatrixMult4(m3, m2, m1)
00120 vpMatrix4 m3, m2, m1;
00121 {
00122     MatrixMult((double *)m3, (double *)m2, (double *)m1, 4, 4, 4);
00123 }
00124 
00125 /*
00126  * MatrixMult
00127  *
00128  * Perform the matrix multiplication p = a * b.
00129  */
00130 
00131 static void
00132 MatrixMult(p, a, b, l, m, n)
00133 double *p;      /* result matrix, size l by n */
00134 double *a;      /* first factor, size l by m */
00135 double *b;      /* second factor, size m by n */
00136 int l, m, n;
00137 {
00138     int i, j, k;
00139 
00140     if (l <= 0 || m <= 0 || n <= 0)
00141         VPBug("MatrixMult called with non-positive matrix size");
00142     for (i = 0; i < l; i++) {
00143         for (j = 0; j < n; j++) {
00144             p[i*n+j] = 0;
00145             for (k = 0; k < m; k++)
00146                 p[i*n+j] += a[i*n+k] * b[k*n+j];
00147         }
00148     }
00149 }
00150 
00151 /*
00152  * vpCrossProduct
00153  *
00154  * Compute the cross product p = v * w.
00155  */
00156 
00157 void
00158 vpCrossProduct(p, v, w)
00159 vpVector3 p, v, w;
00160 {
00161     p[0] = v[1]*w[2] - v[2]*w[1];
00162     p[1] = v[2]*w[0] - v[0]*w[2];
00163     p[2] = v[0]*w[1] - v[1]*w[0];
00164 }
00165 
00166 /*
00167  * vpSolveSystem4
00168  *
00169  * Solve the linear system a*xi = bi where a is a 4-by-4 matrix and bi
00170  * is a column of the 4-by-m matrix b.  Each column bi in b is replaced
00171  * by the corresponding solution vector xi.  The matrix a is destroyed.
00172  * The method used is Gauss-Jordan elimination with partial pivoting and
00173  * implicit scaling (based on the discussion in Numerical Recipes in C
00174  * by Press, Flannery, Teukolsky and Vetterling).
00175  *
00176  * Return VPERROR_SINGULAR if matrix is singular.
00177  */
00178 
00179 vpResult
00180 vpSolveSystem4(a, b, m)
00181 vpMatrix4 a;    /* linear system matrix */
00182 double **b;     /* RHS vectors on input, solution vectors on output;
00183                    b[i] is a Vector4 */
00184 int m;          /* number of vectors in b */
00185 {
00186     vpVector4 row_scale_factor; /* normalization for each row */
00187     int ipivot;                 /* row containing pivot */
00188     int pivot[4];               /* after the reduction loop, row i has
00189                                    been pivoted to row pivot[i] */
00190     int i, j, k, l;             /* loop indices */
00191     double *aptr;               /* pointer into a */
00192     double entry;               /* entry in a */
00193     double max_entry;           /* maximum entry in row */
00194     double inv_entry;           /* inverse of an entry in a */
00195     vpVector4 tmpv;             /* temporary vector for undoing row
00196                                    interchange in solution vectors */
00197 
00198     /* initialize */
00199     for (i = 0; i < 4; i++)
00200         pivot[i] = -1;
00201 
00202     /* find the largest element in each row and compute normalization
00203        for implicit scaling */
00204     aptr = &a[0][0];
00205     for (i = 0; i < 4; i++) {
00206         max_entry = 0.;
00207         for (j = 0; j < 4; j++) {
00208             if (*aptr < 0) {
00209                 if (-*aptr > max_entry)
00210                     max_entry = -*aptr;
00211             } else {
00212                 if (*aptr > max_entry)
00213                     max_entry = *aptr;
00214             }
00215             aptr++;
00216         }
00217         if (fabs(max_entry) < VP_EPS)
00218             return(VPERROR_SINGULAR);
00219         row_scale_factor[i] = 1. / max_entry;
00220     }
00221 
00222     /* loop over the columns of a */
00223     for (j = 0; j < 4; j++) {
00224         /* loop over the rows of a and choose a pivot element in the
00225            current column, ignoring rows containing previous pivots */
00226         max_entry = 0.;
00227         for (i = 0; i < 4; i++) {
00228             if (pivot[i] < 0) {
00229                 entry = a[i][j] * row_scale_factor[i];
00230                 if (entry < 0) {
00231                     if (-entry > max_entry) {
00232                         max_entry = -entry;
00233                         ipivot = i;
00234                     }
00235                 } else {
00236                     if (entry > max_entry) {
00237                         max_entry = entry;
00238                         ipivot = i;
00239                     }
00240                 }
00241             }
00242         }
00243         if (fabs(max_entry) < VP_EPS)
00244             return(VPERROR_SINGULAR);
00245         pivot[ipivot] = j;
00246         inv_entry = 1. / a[ipivot][j];
00247 
00248         /* scale the pivot row by the pivot element */
00249         for (l = j+1; l < 4; l++)
00250             a[ipivot][l] *= inv_entry;
00251         for (l = 0; l < m; l++)
00252             b[l][ipivot] *= inv_entry;
00253 
00254         /* subtract a multiple of the pivot row from the other rows */
00255         for (k = 0; k < 4; k++) {
00256             if (k != ipivot) {
00257                 entry = a[k][j];
00258                 for (l = j+1; l < 4; l++)
00259                     a[k][l] -= a[ipivot][l] * entry;
00260                 for (l = 0; l < m; l++)
00261                     b[l][k] -= b[l][ipivot] * entry;
00262             }
00263         }
00264     }
00265 
00266     /* undo row interchanges in solution vectors */
00267     for (j = 0; j < m; j++) {
00268         for (i = 0; i < 4; i++)
00269             tmpv[pivot[i]] = b[j][i];
00270         for (i = 0; i < 4; i++)
00271             b[j][i] = tmpv[i];
00272     }
00273     return(VP_OK);
00274 }
00275 
00276 /*
00277  * VPLoadTranslation
00278  *
00279  * Load a translation matrix.
00280  */
00281 
00282 void
00283 VPLoadTranslation(m, tx, ty, tz)
00284 vpMatrix4 m;
00285 double tx, ty, tz;
00286 {
00287     vpIdentity4(m);
00288     m[0][3] = tx;
00289     m[1][3] = ty;
00290     m[2][3] = tz;
00291 }
00292 
00293 /*
00294  * VPLoadRotation
00295  *
00296  * Load a rotation matrix.
00297  */
00298 
00299 void
00300 VPLoadRotation(m, axis, degrees)
00301 vpMatrix4 m;
00302 int axis;
00303 double degrees;
00304 {
00305     vpMatrix4 tmp;
00306     double radians, sintheta, costheta;
00307 
00308     radians = degrees * M_PI / 180.;
00309     sintheta = sin(radians);
00310     costheta = cos(radians);
00311     vpIdentity4(m);
00312     switch (axis) {
00313     case VP_X_AXIS:
00314         m[1][1] = costheta;
00315         m[1][2] = sintheta;
00316         m[2][1] = -sintheta;
00317         m[2][2] = costheta;
00318         break;
00319     case VP_Y_AXIS:
00320         m[0][0] = costheta;
00321         m[0][2] = -sintheta;
00322         m[2][0] = sintheta;
00323         m[2][2] = costheta;
00324         break;
00325     case VP_Z_AXIS:
00326         m[0][0] = costheta;
00327         m[0][1] = sintheta;
00328         m[1][0] = -sintheta;
00329         m[1][1] = costheta;
00330         break;
00331     default:
00332         VPBug("bad axis in VPLoadRotation");
00333     }
00334 }
00335 
00336 /*
00337  * VPLoadScale
00338  *
00339  * Load a scale matrix.
00340  */
00341 
00342 void
00343 VPLoadScale(m, sx, sy, sz)
00344 vpMatrix4 m;
00345 double sx, sy, sz;
00346 {
00347     vpIdentity4(m);
00348     m[0][0] = sx;
00349     m[1][1] = sy;
00350     m[2][2] = sz;
00351 }