1 /*
2 * vp_linalg.c
3 *
4 * A simple linear algebra package.
5 *
6 * Copyright (c) 1994 The Board of Trustees of The Leland Stanford
7 * Junior University. All rights reserved.
8 *
9 * Permission to use, copy, modify and distribute this software and its
10 * documentation for any purpose is hereby granted without fee, provided
11 * that the above copyright notice and this permission notice appear in
12 * all copies of this software and that you do not sell the software.
13 * Commercial licensing is available by contacting the author.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS" AND WITHOUT WARRANTY OF ANY KIND,
16 * EXPRESS, IMPLIED OR OTHERWISE, INCLUDING WITHOUT LIMITATION, ANY
17 * WARRANTY OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE.
18 *
19 * Author:
20 * Phil Lacroute
21 * Computer Systems Laboratory
22 * Electrical Engineering Dept.
23 * Stanford University
24 */
25
26 /*
27 * $Date: 1994/12/30 23:52:38 $
28 * $Revision: 1.27 $
29 */
30
31 #include "vp_global.h"
32
33 static void MatrixMult ANSI_ARGS((double* p, double *a, double *b,
34 int l, int m, int n));
35
36 /*
37 * vpIdentity3
38 *
39 * Initialize a Matrix3 to the identity.
40 */
41
42 void
vpIdentity3(m)43 vpIdentity3(m)
44 vpMatrix3 m;
45 {
46 m[0][0] = 1.; m[0][1] = 0.; m[0][2] = 0.;
47 m[1][0] = 0.; m[1][1] = 1.; m[1][2] = 0.;
48 m[2][0] = 0.; m[2][1] = 0.; m[2][2] = 1.;
49 }
50
51 /*
52 * vpIdentity4
53 *
54 * Initialize a Matrix4 to the identity.
55 */
56
57 void
vpIdentity4(m)58 vpIdentity4(m)
59 vpMatrix4 m;
60 {
61 m[0][0] = 1.; m[0][1] = 0.; m[0][2] = 0.; m[0][3] = 0.;
62 m[1][0] = 0.; m[1][1] = 1.; m[1][2] = 0.; m[1][3] = 0.;
63 m[2][0] = 0.; m[2][1] = 0.; m[2][2] = 1.; m[2][3] = 0.;
64 m[3][0] = 0.; m[3][1] = 0.; m[3][2] = 0.; m[3][3] = 1.;
65 }
66
67 /*
68 * vpNormalize3
69 *
70 * Normalize a vector (divide it by its magnitude). Return VPERROR_SINGULAR
71 * if the magnitude is too small.
72 */
73
74 vpResult
vpNormalize3(v)75 vpNormalize3(v)
76 vpVector3 v;
77 {
78 double magsqr, invmag;
79 int i;
80
81 magsqr = 0.;
82 for (i = 0; i < 3; i++)
83 magsqr += v[i]*v[i];
84 if (fabs(magsqr) < VP_EPS)
85 return(VPERROR_SINGULAR);
86 invmag = 1. / sqrt(magsqr);
87 for (i = 0; i < 3; i++)
88 v[i] *= invmag;
89 return(VP_OK);
90 }
91
92 /*
93 * vpMatrixVectorMult4
94 *
95 * Perform the matrix-vector multiplication v2 = m*v1.
96 */
97
98 void
vpMatrixVectorMult4(v2,m,v1)99 vpMatrixVectorMult4(v2, m, v1)
100 vpVector4 v2, v1;
101 vpMatrix4 m;
102 {
103 int i, j;
104
105 for (i = 0; i < 4; i++) {
106 v2[i] = 0;
107 for (j = 0; j < 4; j++)
108 v2[i] += m[i][j] * v1[j];
109 }
110 }
111
112 /*
113 * vpMatrixMult4
114 *
115 * Perform the matrix multiplication m3 = m2 * m1.
116 */
117
118 void
vpMatrixMult4(m3,m2,m1)119 vpMatrixMult4(m3, m2, m1)
120 vpMatrix4 m3, m2, m1;
121 {
122 MatrixMult((double *)m3, (double *)m2, (double *)m1, 4, 4, 4);
123 }
124
125 /*
126 * MatrixMult
127 *
128 * Perform the matrix multiplication p = a * b.
129 */
130
131 static void
MatrixMult(p,a,b,l,m,n)132 MatrixMult(p, a, b, l, m, n)
133 double *p; /* result matrix, size l by n */
134 double *a; /* first factor, size l by m */
135 double *b; /* second factor, size m by n */
136 int l, m, n;
137 {
138 int i, j, k;
139
140 if (l <= 0 || m <= 0 || n <= 0)
141 VPBug("MatrixMult called with non-positive matrix size");
142 for (i = 0; i < l; i++) {
143 for (j = 0; j < n; j++) {
144 p[i*n+j] = 0;
145 for (k = 0; k < m; k++)
146 p[i*n+j] += a[i*n+k] * b[k*n+j];
147 }
148 }
149 }
150
151 /*
152 * vpCrossProduct
153 *
154 * Compute the cross product p = v * w.
155 */
156
157 void
vpCrossProduct(p,v,w)158 vpCrossProduct(p, v, w)
159 vpVector3 p, v, w;
160 {
161 p[0] = v[1]*w[2] - v[2]*w[1];
162 p[1] = v[2]*w[0] - v[0]*w[2];
163 p[2] = v[0]*w[1] - v[1]*w[0];
164 }
165
166 /*
167 * vpSolveSystem4
168 *
169 * Solve the linear system a*xi = bi where a is a 4-by-4 matrix and bi
170 * is a column of the 4-by-m matrix b. Each column bi in b is replaced
171 * by the corresponding solution vector xi. The matrix a is destroyed.
172 * The method used is Gauss-Jordan elimination with partial pivoting and
173 * implicit scaling (based on the discussion in Numerical Recipes in C
174 * by Press, Flannery, Teukolsky and Vetterling).
175 *
176 * Return VPERROR_SINGULAR if matrix is singular.
177 */
178
179 vpResult
vpSolveSystem4(a,b,m)180 vpSolveSystem4(a, b, m)
181 vpMatrix4 a; /* linear system matrix */
182 double **b; /* RHS vectors on input, solution vectors on output;
183 b[i] is a Vector4 */
184 int m; /* number of vectors in b */
185 {
186 vpVector4 row_scale_factor; /* normalization for each row */
187 int ipivot; /* row containing pivot */
188 int pivot[4]; /* after the reduction loop, row i has
189 been pivoted to row pivot[i] */
190 int i, j, k, l; /* loop indices */
191 double *aptr; /* pointer into a */
192 double entry; /* entry in a */
193 double max_entry; /* maximum entry in row */
194 double inv_entry; /* inverse of an entry in a */
195 vpVector4 tmpv; /* temporary vector for undoing row
196 interchange in solution vectors */
197
198 /* initialize */
199 for (i = 0; i < 4; i++)
200 pivot[i] = -1;
201
202 /* find the largest element in each row and compute normalization
203 for implicit scaling */
204 aptr = &a[0][0];
205 for (i = 0; i < 4; i++) {
206 max_entry = 0.;
207 for (j = 0; j < 4; j++) {
208 if (*aptr < 0) {
209 if (-*aptr > max_entry)
210 max_entry = -*aptr;
211 } else {
212 if (*aptr > max_entry)
213 max_entry = *aptr;
214 }
215 aptr++;
216 }
217 if (fabs(max_entry) < VP_EPS)
218 return(VPERROR_SINGULAR);
219 row_scale_factor[i] = 1. / max_entry;
220 }
221
222 /* loop over the columns of a */
223 for (j = 0; j < 4; j++) {
224 /* loop over the rows of a and choose a pivot element in the
225 current column, ignoring rows containing previous pivots */
226 max_entry = 0.;
227 for (i = 0; i < 4; i++) {
228 if (pivot[i] < 0) {
229 entry = a[i][j] * row_scale_factor[i];
230 if (entry < 0) {
231 if (-entry > max_entry) {
232 max_entry = -entry;
233 ipivot = i;
234 }
235 } else {
236 if (entry > max_entry) {
237 max_entry = entry;
238 ipivot = i;
239 }
240 }
241 }
242 }
243 if (fabs(max_entry) < VP_EPS)
244 return(VPERROR_SINGULAR);
245 pivot[ipivot] = j;
246 inv_entry = 1. / a[ipivot][j];
247
248 /* scale the pivot row by the pivot element */
249 for (l = j+1; l < 4; l++)
250 a[ipivot][l] *= inv_entry;
251 for (l = 0; l < m; l++)
252 b[l][ipivot] *= inv_entry;
253
254 /* subtract a multiple of the pivot row from the other rows */
255 for (k = 0; k < 4; k++) {
256 if (k != ipivot) {
257 entry = a[k][j];
258 for (l = j+1; l < 4; l++)
259 a[k][l] -= a[ipivot][l] * entry;
260 for (l = 0; l < m; l++)
261 b[l][k] -= b[l][ipivot] * entry;
262 }
263 }
264 }
265
266 /* undo row interchanges in solution vectors */
267 for (j = 0; j < m; j++) {
268 for (i = 0; i < 4; i++)
269 tmpv[pivot[i]] = b[j][i];
270 for (i = 0; i < 4; i++)
271 b[j][i] = tmpv[i];
272 }
273 return(VP_OK);
274 }
275
276 /*
277 * VPLoadTranslation
278 *
279 * Load a translation matrix.
280 */
281
282 void
VPLoadTranslation(m,tx,ty,tz)283 VPLoadTranslation(m, tx, ty, tz)
284 vpMatrix4 m;
285 double tx, ty, tz;
286 {
287 vpIdentity4(m);
288 m[0][3] = tx;
289 m[1][3] = ty;
290 m[2][3] = tz;
291 }
292
293 /*
294 * VPLoadRotation
295 *
296 * Load a rotation matrix.
297 */
298
299 void
VPLoadRotation(m,axis,degrees)300 VPLoadRotation(m, axis, degrees)
301 vpMatrix4 m;
302 int axis;
303 double degrees;
304 {
305 vpMatrix4 tmp;
306 double radians, sintheta, costheta;
307
308 radians = degrees * M_PI / 180.;
309 sintheta = sin(radians);
310 costheta = cos(radians);
311 vpIdentity4(m);
312 switch (axis) {
313 case VP_X_AXIS:
314 m[1][1] = costheta;
315 m[1][2] = sintheta;
316 m[2][1] = -sintheta;
317 m[2][2] = costheta;
318 break;
319 case VP_Y_AXIS:
320 m[0][0] = costheta;
321 m[0][2] = -sintheta;
322 m[2][0] = sintheta;
323 m[2][2] = costheta;
324 break;
325 case VP_Z_AXIS:
326 m[0][0] = costheta;
327 m[0][1] = sintheta;
328 m[1][0] = -sintheta;
329 m[1][1] = costheta;
330 break;
331 default:
332 VPBug("bad axis in VPLoadRotation");
333 }
334 }
335
336 /*
337 * VPLoadScale
338 *
339 * Load a scale matrix.
340 */
341
342 void
VPLoadScale(m,sx,sy,sz)343 VPLoadScale(m, sx, sy, sz)
344 vpMatrix4 m;
345 double sx, sy, sz;
346 {
347 vpIdentity4(m);
348 m[0][0] = sx;
349 m[1][1] = sy;
350 m[2][2] = sz;
351 }
352