Debian Codegraph @

lib/database/src/sb/SbMatrix.c++

raw
1 /*
2 *
3 * Copyright (C) 2000 Silicon Graphics, Inc. All Rights Reserved.
4 *
5 * This library is free software; you can redistribute it and/or
6 * modify it under the terms of the GNU Lesser General Public
7 * License as published by the Free Software Foundation; either
8 * version 2.1 of the License, or (at your option) any later version.
9 *
10 * This library 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 GNU
13 * Lesser General Public License for more details.
14 *
15 * Further, this software is distributed without any warranty that it is
16 * free of the rightful claim of any third person regarding infringement
17 * or the like. Any license provided herein, whether implied or
18 * otherwise, applies only to this software file. Patent licenses, if
19 * any, provided herein do not apply to combinations of this program with
20 * other software, or any other product whatsoever.
21 *
22 * You should have received a copy of the GNU Lesser General Public
23 * License along with this library; if not, write to the Free Software
24 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
25 *
26 * Contact information: Silicon Graphics, Inc., 1600 Amphitheatre Pkwy,
27 * Mountain View, CA 94043, or:
28 *
29 * http://www.sgi.com
30 *
31 * For further information regarding this notice, see:
32 *
33 * http://oss.sgi.com/projects/GenInfo/NoticeExplan/
34 *
35 */
36
37 /*
38 * Copyright (C) 1990,91 Silicon Graphics, Inc.
39 *
40 _______________________________________________________________________
41 ______________ S I L I C O N G R A P H I C S I N C . ____________
42 |
43 | $Revision: 1.2 $
44 |
45 | Classes:
46 | SbMatrix
47 |
48 | Author(s) : Nick Thompson, David Mott, Howard Look
49 |
50 | Notes:
51 | Matrices are stored in row-major order.
52 |
53 ______________ S I L I C O N G R A P H I C S I N C . ____________
54 _______________________________________________________________________
55 */
56
57 #include <Inventor/SbLinear.h>
58
59 // amount squared to figure if two floats are equal
60 // (used for operator == right now)
61 #define DELTA 1e-6
62
63 //
64 // Handy absolute value macro:
65 //
66
67 #define ABS(a) ((a) < 0.0 ? -(a) : (a))
68
69
70 //
71 // Macro for checking is a matrix is idenity.
72 //
73
74 #define IS_IDENTITY(matrix) ( \
75 (matrix[0][0] == 1.0) && \
76 (matrix[0][1] == 0.0) && \
77 (matrix[0][2] == 0.0) && \
78 (matrix[0][3] == 0.0) && \
79 (matrix[1][0] == 0.0) && \
80 (matrix[1][1] == 1.0) && \
81 (matrix[1][2] == 0.0) && \
82 (matrix[1][3] == 0.0) && \
83 (matrix[2][0] == 0.0) && \
84 (matrix[2][1] == 0.0) && \
85 (matrix[2][2] == 1.0) && \
86 (matrix[2][3] == 0.0) && \
87 (matrix[3][0] == 0.0) && \
88 (matrix[3][1] == 0.0) && \
89 (matrix[3][2] == 0.0) && \
90 (matrix[3][3] == 1.0))
91
92
93 //
94 // Constructor given all 16 elements in row-major order
95 //
96
97 SbMatrix::SbMatrix(float a11, float a12, float a13, float a14,
98 float a21, float a22, float a23, float a24,
99 float a31, float a32, float a33, float a34,
100 float a41, float a42, float a43, float a44)
101 {
102 matrix[0][0] = a11;
103 matrix[0][1] = a12;
104 matrix[0][2] = a13;
105 matrix[0][3] = a14;
106
107 matrix[1][0] = a21;
108 matrix[1][1] = a22;
109 matrix[1][2] = a23;
110 matrix[1][3] = a24;
111
112 matrix[2][0] = a31;
113 matrix[2][1] = a32;
114 matrix[2][2] = a33;
115 matrix[2][3] = a34;
116
117 matrix[3][0] = a41;
118 matrix[3][1] = a42;
119 matrix[3][2] = a43;
120 matrix[3][3] = a44;
121 }
122
123 //
124 // Constructor from a 4x4 array of elements
125 //
126
127 SbMatrix::SbMatrix(const SbMat &m)
128 {
129 matrix[0][0] = m[0][0];
130 matrix[0][1] = m[0][1];
131 matrix[0][2] = m[0][2];
132 matrix[0][3] = m[0][3];
133 matrix[1][0] = m[1][0];
134 matrix[1][1] = m[1][1];
135 matrix[1][2] = m[1][2];
136 matrix[1][3] = m[1][3];
137 matrix[2][0] = m[2][0];
138 matrix[2][1] = m[2][1];
139 matrix[2][2] = m[2][2];
140 matrix[2][3] = m[2][3];
141 matrix[3][0] = m[3][0];
142 matrix[3][1] = m[3][1];
143 matrix[3][2] = m[3][2];
144 matrix[3][3] = m[3][3];
145 }
146
147 ////////////////////////////////////////////
148 //
149 // Setting matrix values
150 //
151 ////////////////////////////////////////////
152
153 //
154 // Sets value from 4x4 array of elements
155 //
156
157 void
158 SbMatrix::setValue(const SbMat &m)
159 {
160 matrix[0][0] = m[0][0];
161 matrix[0][1] = m[0][1];
162 matrix[0][2] = m[0][2];
163 matrix[0][3] = m[0][3];
164 matrix[1][0] = m[1][0];
165 matrix[1][1] = m[1][1];
166 matrix[1][2] = m[1][2];
167 matrix[1][3] = m[1][3];
168 matrix[2][0] = m[2][0];
169 matrix[2][1] = m[2][1];
170 matrix[2][2] = m[2][2];
171 matrix[2][3] = m[2][3];
172 matrix[3][0] = m[3][0];
173 matrix[3][1] = m[3][1];
174 matrix[3][2] = m[3][2];
175 matrix[3][3] = m[3][3];
176 }
177
178 //
179 // Return an identity matrix
180 //
181
182 SbMatrix
183 SbMatrix::identity()
184 {
185 return SbMatrix(1.0, 0.0, 0.0, 0.0,
186 0.0, 1.0, 0.0, 0.0,
187 0.0, 0.0, 1.0, 0.0,
188 0.0, 0.0, 0.0, 1.0);
189 }
190
191 //
192 // Sets matrix to be identity
193 //
194
195 void
196 SbMatrix::makeIdentity()
197 {
198 matrix[0][0] = 1.0;
199 matrix[0][1] = 0.0;
200 matrix[0][2] = 0.0;
201 matrix[0][3] = 0.0;
202 matrix[1][0] = 0.0;
203 matrix[1][1] = 1.0;
204 matrix[1][2] = 0.0;
205 matrix[1][3] = 0.0;
206 matrix[2][0] = 0.0;
207 matrix[2][1] = 0.0;
208 matrix[2][2] = 1.0;
209 matrix[2][3] = 0.0;
210 matrix[3][0] = 0.0;
211 matrix[3][1] = 0.0;
212 matrix[3][2] = 0.0;
213 matrix[3][3] = 1.0;
214 }
215
216 //
217 // Sets matrix to rotate by given rotation
218 //
219
220 void
221 SbMatrix::setRotate(const SbRotation &rotation)
222 {
223 rotation.getValue(*this);
224 }
225
226 //
227 // Sets matrix to scale by given uniform factor
228 //
229
230 void
231 SbMatrix::setScale(float s)
232 {
233 matrix[0][0] = s;
234 matrix[0][1] = 0.0;
235 matrix[0][2] = 0.0;
236 matrix[0][3] = 0.0;
237 matrix[1][0] = 0.0;
238 matrix[1][1] = s;
239 matrix[1][2] = 0.0;
240 matrix[1][3] = 0.0;
241 matrix[2][0] = 0.0;
242 matrix[2][1] = 0.0;
243 matrix[2][2] = s;
244 matrix[2][3] = 0.0;
245 matrix[3][0] = 0.0;
246 matrix[3][1] = 0.0;
247 matrix[3][2] = 0.0;
248 matrix[3][3] = 1.0;
249 }
250
251 //
252 // Sets matrix to scale by given vector
253 //
254
255 void
256 SbMatrix::setScale(const SbVec3f &s)
257 {
258 matrix[0][0] = s[0];
259 matrix[0][1] = 0.0;
260 matrix[0][2] = 0.0;
261 matrix[0][3] = 0.0;
262 matrix[1][0] = 0.0;
263 matrix[1][1] = s[1];
264 matrix[1][2] = 0.0;
265 matrix[1][3] = 0.0;
266 matrix[2][0] = 0.0;
267 matrix[2][1] = 0.0;
268 matrix[2][2] = s[2];
269 matrix[2][3] = 0.0;
270 matrix[3][0] = 0.0;
271 matrix[3][1] = 0.0;
272 matrix[3][2] = 0.0;
273 matrix[3][3] = 1.0;
274 }
275
276 //
277 // Sets matrix to translate by given vector
278 //
279
280 void
281 SbMatrix::setTranslate(const SbVec3f &t)
282 {
283 matrix[0][0] = 1.0;
284 matrix[0][1] = 0.0;
285 matrix[0][2] = 0.0;
286 matrix[0][3] = 0.0;
287 matrix[1][0] = 0.0;
288 matrix[1][1] = 1.0;
289 matrix[1][2] = 0.0;
290 matrix[1][3] = 0.0;
291 matrix[2][0] = 0.0;
292 matrix[2][1] = 0.0;
293 matrix[2][2] = 1.0;
294 matrix[2][3] = 0.0;
295 matrix[3][0] = t[0];
296 matrix[3][1] = t[1];
297 matrix[3][2] = t[2];
298 matrix[3][3] = 1.0;
299 }
300
301 ////////////////////////////////////////////
302 //
303 // Returning matrix values and other info
304 //
305 ////////////////////////////////////////////
306
307 //
308 // Returns 4x4 array of elements
309 //
310
311 void
312 SbMatrix::getValue(SbMat &m) const
313 {
314 m[0][0] = matrix[0][0];
315 m[0][1] = matrix[0][1];
316 m[0][2] = matrix[0][2];
317 m[0][3] = matrix[0][3];
318 m[1][0] = matrix[1][0];
319 m[1][1] = matrix[1][1];
320 m[1][2] = matrix[1][2];
321 m[1][3] = matrix[1][3];
322 m[2][0] = matrix[2][0];
323 m[2][1] = matrix[2][1];
324 m[2][2] = matrix[2][2];
325 m[2][3] = matrix[2][3];
326 m[3][0] = matrix[3][0];
327 m[3][1] = matrix[3][1];
328 m[3][2] = matrix[3][2];
329 m[3][3] = matrix[3][3];
330 }
331
332 //
333 // Returns determinant of 3x3 submatrix composed of given row and
334 // column indices (0-3 for each).
335 //
336
337 float
338 SbMatrix::det3(int r1, int r2, int r3, int c1, int c2, int c3) const
339 {
340 return ( matrix[r1][c1] * matrix[r2][c2] * matrix[r3][c3]
341 + matrix[r1][c2] * matrix[r2][c3] * matrix[r3][c1]
342 + matrix[r1][c3] * matrix[r2][c1] * matrix[r3][c2]
343 - matrix[r1][c1] * matrix[r2][c3] * matrix[r3][c2]
344 - matrix[r1][c2] * matrix[r2][c1] * matrix[r3][c3]
345 - matrix[r1][c3] * matrix[r2][c2] * matrix[r3][c1]);
346 }
347
348 //
349 // Returns determinant of entire matrix
350 //
351
352 float
353 SbMatrix::det4() const
354 {
355 return ( matrix[0][3] * det3(1, 2, 3, 0, 1, 2)
356 + matrix[1][3] * det3(0, 2, 3, 0, 1, 2)
357 + matrix[2][3] * det3(0, 1, 3, 0, 1, 2)
358 + matrix[3][3] * det3(0, 1, 2, 0, 1, 2));
359 }
360
361 //
362 // Factors a matrix m into 5 pieces: m = r s r^ u t, where r^
363 // means transpose of r, and r and u are rotations, s is a scale,
364 // and t is a translation. Any projection information is returned
365 // in proj.
366 //
367 // routines for matrix factorization taken from BAGS code, written by
368 // John Hughes (?). Original comment follows:
369 //
370 /* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
371 /* --------------------------------------------------------------------------
372 * This file contains routines to do the MAT3factor operation, which
373 * factors a matrix m:
374 * m = r s r^ u t, where r^ means transpose of r, and r and u are
375 * rotations, s is a scale, and t is a translation.
376 *
377 * It is based on the Jacobi method for diagonalizing real symmetric
378 * matrices, taken from Linear Algebra, Wilkenson and Reinsch, Springer-Verlag
379 * math series, Volume II, 1971, page 204. Call number QA251W623.
380 * In ALGOL!
381 * -------------------------------------------------------------------------*/
382 /*
383 * Variable declarations from the original source:
384 *
385 * n : order of matrix A
386 * eivec: true if eigenvectors are desired, false otherwise.
387 * a : Array [1:n, 1:n] of numbers, assumed symmetric!
388 *
389 * a : Superdiagonal elements of the original array a are destroyed.
390 * Diagonal and subdiagonal elements are untouched.
391 * d : Array [1:n] of eigenvalues of a.
392 * v : Array [1:n, 1:n] containing (if eivec = TRUE), the eigenvectors of
393 * a, with the kth column being the normalized eigenvector with
394 * eigenvalue d[k].
395 * rot : The number of jacobi rotations required to perform the operation.
396 */
397
398 SbBool
399 SbMatrix::factor(SbMatrix &r, SbVec3f &s, SbMatrix &u, SbVec3f &t,
400 SbMatrix &proj) const
401 {
402 double det; /* Determinant of matrix A */
403 double det_sign; /* -1 if det < 0, 1 if det > 0 */
404 double scratch;
405 int i, j;
406 int junk;
407 SbMatrix a, b, si;
408 float evalues[3];
409 SbVec3f evectors[3];
410
411 a = *this;
412 proj.makeIdentity();
413 scratch = 1.0;
414
415 for (i = 0; i < 3; i++) {
416 for (j = 0; j < 3; j++) {
417 a.matrix[i][j] *= scratch;
418 }
419 t[i] = matrix[3][i] * scratch;
420 a.matrix[3][i] = a.matrix[i][3] = 0.0;
421 }
422 a.matrix[3][3] = 1.0;
423
424 /* (3) Compute det A. If negative, set sign = -1, else sign = 1 */
425 det = a.det3();
426 det_sign = (det < 0.0 ? -1.0 : 1.0);
427 if (det_sign * det < 1e-12)
428 return(FALSE); // singular
429
430 /* (4) B = A * A^ (here A^ means A transpose) */
431 b = a * a.transpose();
432
433 b.jacobi3(evalues, evectors, junk);
434
435 // find min / max eigenvalues and do ratio test to determine singularity
436
437 r = SbMatrix(evectors[0][0], evectors[0][1], evectors[0][2], 0.0,
438 evectors[1][0], evectors[1][1], evectors[1][2], 0.0,
439 evectors[2][0], evectors[2][1], evectors[2][2], 0.0,
440 0.0, 0.0, 0.0, 1.0);
441
442 /* Compute s = sqrt(evalues), with sign. Set si = s-inverse */
443 si.makeIdentity();
444 for (i = 0; i < 3; i++) {
445 s[i] = det_sign * sqrt(evalues[i]);
446 si.matrix[i][i] = 1.0 / s[i];
447 }
448
449 /* (5) Compute U = R^ S! R A. */
450 u = r * si * r.transpose() * a;
451
452 return(TRUE);
453 }
454
455 //
456 // Diagonalizes 3x3 matrix. See comment for factor().
457 //
458
459 #define SB_JACOBI_RANK 3
460 void
461 SbMatrix::jacobi3(float evalues[SB_JACOBI_RANK],
462 SbVec3f evectors[SB_JACOBI_RANK], int &rots) const
463 {
464 double sm; // smallest entry
465 double theta; // angle for Jacobi rotation
466 double c, s, t; // cosine, sine, tangent of theta
467 double tau; // sine / (1 + cos)
468 double h, g; // two scrap values
469 double thresh; // threshold below which no rotation done
470 double b[SB_JACOBI_RANK]; // more scratch
471 double z[SB_JACOBI_RANK]; // more scratch
472 int p, q, i, j;
473 double a[SB_JACOBI_RANK][SB_JACOBI_RANK];
474
475 // initializations
476 for (i = 0; i < SB_JACOBI_RANK; i++) {
477 b[i] = evalues[i] = matrix[i][i];
478 z[i] = 0.0;
479 for (j = 0; j < SB_JACOBI_RANK; j++) {
480 evectors[i][j] = (i == j) ? 1.0 : 0.0;
481 a[i][j] = matrix[i][j];
482 }
483 }
484
485 rots = 0;
486
487 // Why 50? I don't know--it's the way the folks who wrote the
488 // algorithm did it:
489 for (i = 0; i < 50; i++) {
490 sm = 0.0;
491 for (p = 0; p < SB_JACOBI_RANK - 1; p++)
492 for (q = p+1; q < SB_JACOBI_RANK; q++)
493 sm += ABS(a[p][q]);
494
495 if (sm == 0.0)
496 return;
497
498 thresh = (i < 3 ?
499 (.2 * sm / (SB_JACOBI_RANK * SB_JACOBI_RANK)) :
500 0.0);
501
502 for (p = 0; p < SB_JACOBI_RANK - 1; p++) {
503 for (q = p+1; q < SB_JACOBI_RANK; q++) {
504
505 g = 100.0 * ABS(a[p][q]);
506
507 if (i > 3 && (ABS(evalues[p]) + g == ABS(evalues[p])) &&
508 (ABS(evalues[q]) + g == ABS(evalues[q])))
509 a[p][q] = 0.0;
510
511 else if (ABS(a[p][q]) > thresh) {
512 h = evalues[q] - evalues[p];
513
514 if (ABS(h) + g == ABS(h))
515 t = a[p][q] / h;
516 else {
517 theta = .5 * h / a[p][q];
518 t = 1.0 / (ABS(theta) + sqrt(1 + theta * theta));
519 if (theta < 0.0) t = -t;
520 }
521 // End of computing tangent of rotation angle
522
523 c = 1.0 / sqrt(1.0 + t*t);
524 s = t * c;
525 tau = s / (1.0 + c);
526 h = t * a[p][q];
527 z[p] -= h;
528 z[q] += h;
529 evalues[p] -= h;
530 evalues[q] += h;
531 a[p][q] = 0.0;
532
533 for (j = 0; j < p; j++) {
534 g = a[j][p];
535 h = a[j][q];
536 a[j][p] = g - s * (h + g * tau);
537 a[j][q] = h + s * (g - h * tau);
538 }
539
540 for (j = p+1; j < q; j++) {
541 g = a[p][j];
542 h = a[j][q];
543 a[p][j] = g - s * (h + g * tau);
544 a[j][q] = h + s * (g - h * tau);
545 }
546
547 for (j = q+1; j < SB_JACOBI_RANK; j++) {
548 g = a[p][j];
549 h = a[q][j];
550 a[p][j] = g - s * (h + g * tau);
551 a[q][j] = h + s * (g - h * tau);
552 }
553
554 for (j = 0; j < SB_JACOBI_RANK; j++) {
555 g = evectors[j][p];
556 h = evectors[j][q];
557 evectors[j][p] = g - s * (h + g * tau);
558 evectors[j][q] = h + s * (g - h * tau);
559 }
560 }
561 rots++;
562 }
563 }
564 for (p = 0; p < SB_JACOBI_RANK; p++) {
565 evalues[p] = b[p] += z[p];
566 z[p] = 0;
567 }
568 }
569 }
570 #undef SB_JACOBI_RANK
571
572 //
573 // Returns inverse of matrix. Results are undefined for
574 // singular matrices. Uses LU decomposition, from Numerial Recipies in C,
575 // pp 43-45. They say "There is no better way."
576 //
577 // Oh, yeah! Well, if the matrix is affine, there IS a better way.
578 // So we call affine_inverse to see if we can get away with it...
579 //
580
581 SbMatrix
582 SbMatrix::inverse() const
583 {
584 // Trivial case
585 if (IS_IDENTITY(matrix))
586 return SbMatrix::identity();
587
588 // Affine case...
589 SbMatrix affineAnswer;
590 if ( affine_inverse( SbMatrix(matrix), affineAnswer ) )
591 return affineAnswer;
592
593 int index[4];
594 float d, invmat[4][4], temp;
595 SbMatrix inverse = *this;
596 #ifdef DEBUGGING
597 int i, j;
598 #endif /* DEBUGGING */
599
600 if(inverse.LUDecomposition(index, d)) {
601
602 #ifdef DEBUGGING
603 for(j = 0; j < 4; j++) {
604 for(i = 0; i < 4; i++)
605 invmat[j][i] = 0.0;
606 invmat[j][j] = 1.0;
607 inverse.LUBackSubstitution(index, invmat[j]);
608 }
609 #else
610 invmat[0][0] = 1.0;
611 invmat[0][1] = 0.0;
612 invmat[0][2] = 0.0;
613 invmat[0][3] = 0.0;
614 inverse.LUBackSubstitution(index, invmat[0]);
615 invmat[1][0] = 0.0;
616 invmat[1][1] = 1.0;
617 invmat[1][2] = 0.0;
618 invmat[1][3] = 0.0;
619 inverse.LUBackSubstitution(index, invmat[1]);
620 invmat[2][0] = 0.0;
621 invmat[2][1] = 0.0;
622 invmat[2][2] = 1.0;
623 invmat[2][3] = 0.0;
624 inverse.LUBackSubstitution(index, invmat[2]);
625 invmat[3][0] = 0.0;
626 invmat[3][1] = 0.0;
627 invmat[3][2] = 0.0;
628 invmat[3][3] = 1.0;
629 inverse.LUBackSubstitution(index, invmat[3]);
630 #endif /* DEBUGGING */
631
632 #ifdef DEBUGGING
633 // transpose invmat
634 for(j = 0; j < 4; j++) {
635 for(i = 0; i < j; i++) {
636 temp = invmat[i][j];
637 invmat[i][j] = invmat[j][i];
638 invmat[j][i] = temp;
639 }
640 }
641 #else
642 #define SWAP(i,j) \
643 temp = invmat[i][j]; \
644 invmat[i][j] = invmat[j][i]; \
645 invmat[j][i] = temp;
646
647 SWAP(1,0);
648
649 SWAP(2,0);
650 SWAP(2,1);
651
652 SWAP(3,0);
653 SWAP(3,1);
654 SWAP(3,2);
655 #undef SWAP
656 #endif /* DEBUGGING */
657 inverse.setValue(invmat);
658 }
659
660 return inverse;
661 }
662
663 // The following method finds the inverse of an affine matrix.
664 // The last column MUST be [0 0 0 1] for this to work.
665 // This is taken from graphics gems 2, page 603
666 //
667 // computes the inverse of a 3d affine matrix; i.e. a matrix with a
668 // dimensionality of 4 where the right column has the entries (0,0,0,1).
669 //
670 // This procedure treats the 4 by 4 matrix as a block matrix and calculates
671 // the inverse of one submatrix for a significant performance
672 // improvement over a general procedure that can invert any nonsingular matrix.
673 //
674 // -1
675 // -1 | | | -1 |
676 // M = |A 0| = | A 0|
677 // | | | |
678 // | | | -1 |
679 // |C 1| |-CA 1|
680 //
681 // where M is a 4 by 4 matrix,
682 // A is the 3 by 3 upper left submatrix of M,
683 // C is the 1 by 3 lower left submatrix of M.
684 // Input:
685 // in - 3D affine matrix
686 // Output:
687 // out - inverse of 3D affine matrix
688 // Returned Value:
689 // TRUE if input matrix is nonsingular and affine
690 // FALSE otherwise
691
692 SbBool
693 SbMatrix::affine_inverse(const SbMatrix &in, SbMatrix &out) const
694 {
695 // Check if matrix is affine...
696 if ( in[0][3] != 0.0 || in[1][3] != 0.0 || in[2][3] != 0.0 ||
697 in[3][3] != 1.0 )
698 return FALSE;
699
700 // Calculate the determinant of submatrix A and determine if the matrix
701 // is singular as limited by the double precision floating
702 // point data representation
703
704 double det_1;
705 double pos, neg, temp;
706
707 #define ACCUMULATE \
708 if (temp >= 0.0) \
709 pos += temp; \
710 else \
711 neg += temp;
712
713 pos = neg = 0.0;
714 temp = in[0][0] * in[1][1] * in[2][2];
715 ACCUMULATE
716 temp = in[0][1] * in[1][2] * in[2][0];
717 ACCUMULATE
718 temp = in[0][2] * in[1][0] * in[2][1];
719 ACCUMULATE
720 temp = -in[0][2] * in[1][1] * in[2][0];
721 ACCUMULATE
722 temp = -in[0][1] * in[1][0] * in[2][2];
723 ACCUMULATE
724 temp = -in[0][0] * in[1][2] * in[2][1];
725 ACCUMULATE
726 det_1 = pos + neg;
727
728 #undef ACCUMULATE
729
730 #define PRECISION_LIMIT (1.0e-15)
731
732 // Is the submatrix A singular?
733 temp = det_1 / (pos - neg);
734 if (ABS(temp) < PRECISION_LIMIT)
735 return FALSE;
736
737 // Calculate inverse(A) = adj(A) / det(A)
738 det_1 = 1.0 / det_1;
739 out[0][0] = (in[1][1] * in[2][2] - in[1][2] * in[2][1]) * det_1;
740 out[1][0] = -(in[1][0] * in[2][2] - in[1][2] * in[2][0]) * det_1;
741 out[2][0] = (in[1][0] * in[2][1] - in[1][1] * in[2][0]) * det_1;
742 out[0][1] = -(in[0][1] * in[2][2] - in[0][2] * in[2][1]) * det_1;
743 out[1][1] = (in[0][0] * in[2][2] - in[0][2] * in[2][0]) * det_1;
744 out[2][1] = -(in[0][0] * in[2][1] - in[0][1] * in[2][0]) * det_1;
745 out[0][2] = (in[0][1] * in[1][2] - in[0][2] * in[1][1]) * det_1;
746 out[1][2] = -(in[0][0] * in[1][2] - in[0][2] * in[1][0]) * det_1;
747 out[2][2] = (in[0][0] * in[1][1] - in[0][1] * in[1][0]) * det_1;
748
749 // Calculate -C * inverse(A)
750 out[3][0] = -( in[3][0] * out[0][0] +
751 in[3][1] * out[1][0] +
752 in[3][2] * out[2][0] );
753 out[3][1] = -( in[3][0] * out[0][1] +
754 in[3][1] * out[1][1] +
755 in[3][2] * out[2][1] );
756 out[3][2] = -( in[3][0] * out[0][2] +
757 in[3][1] * out[1][2] +
758 in[3][2] * out[2][2] );
759
760 // Fill in last column
761 out[0][3] = out[1][3] = out[2][3] = 0.0;
762 out[3][3] = 1.0;
763
764 #undef PRECISION_LIMIT
765
766 return TRUE;
767 }
768
769
770 //
771 // Perform an LU decomposition of a matrix. From Numerical Recipes in C, pg 43
772 // Destroys the original matrix. The resulting matrix is as follows
773 //
774 // a b c d 1 0 0 0 a b c d
775 // LU = e f g h L = e 1 0 0 U = 0 f g h
776 // i j k l i j 1 0 0 0 k l
777 // m n o p m n o 1 0 0 0 p
778 //
779 // For singular matrices, want to calculate something
780 // that is close to the inverse, so we can pick
781 // things scaled to zero in some dimension.
782 //
783 SbBool
784 SbMatrix::LUDecomposition(int index[4], float &d)
785 {
786 int imax;
787 float big, dum, sum, temp;
788 float vv[4];
789 #ifdef DEBUGGING
790 int i, j, k;
791 #endif /* DEBUGGING */
792
793 d = 1.0;
794
795 #ifdef DEBUGGING
796 for(i = 0; i < 4; i++) {
797 big = 0.0;
798 for(j = 0; j < 4; j++)
799 if((temp = ABS(matrix[i][j])) > big) big = temp;
800 if(big == 0.0) {
801 matrix[i][i] = 1e-6;
802 big = matrix[i][i];
803 }
804 vv[i] = 1.0 / big;
805 }
806 #else
807 #define COMPUTE_VV(i) \
808 big = 0.0; \
809 if((temp = ABS(matrix[i][0])) > big) big = temp; \
810 if((temp = ABS(matrix[i][1])) > big) big = temp; \
811 if((temp = ABS(matrix[i][2])) > big) big = temp; \
812 if((temp = ABS(matrix[i][3])) > big) big = temp; \
813 if(big == 0.0) { \
814 matrix[i][i] = 1e-6; \
815 big = matrix[i][i]; \
816 } \
817 vv[i] = 1.0 / big;
818
819 COMPUTE_VV(0);
820 COMPUTE_VV(1);
821 COMPUTE_VV(2);
822 COMPUTE_VV(3);
823 #undef COMPUTE_VV
824 #endif /* DEBUGGING */
825
826 #ifdef DEBUGGING
827 // This is the code as it originally existed.
828 // Below this is the unrolled, really unreadable version.
829
830 for(j = 0; j < 4; j++) {
831
832 // BLOCK 1
833 for(i = 0; i < j; i++) {
834 sum = matrix[i][j];
835 for(k = 0; k < i; k++)
836 sum -= matrix[i][k] * matrix[k][j];
837 matrix[i][j] = sum;
838 }
839
840 big = 0.0;
841
842 // BLOCK 2
843 for(i = j; i < 4; i++) {
844 sum = matrix[i][j];
845 for(k = 0; k < j; k++)
846 sum -= matrix[i][k] * matrix[k][j];
847 matrix[i][j] = sum;
848 if((dum = vv[i] * ABS(sum)) >= big) {
849 big = dum;
850 imax = i;
851 }
852 }
853
854 // BLOCK 3
855 if(j != imax) {
856 for(k = 0; k < 4; k++) {
857 dum = matrix[imax][k];
858 matrix[imax][k] = matrix[j][k];
859 matrix[j][k] = dum;
860 }
861 d = -d;
862 vv[imax] = vv[j];
863 }
864
865 // BLOCK 4
866 index[j] = imax;
867 if(matrix[j][j] == 0.0) matrix[j][j] = 1e-20;
868
869 // BLOCK 5
870 if(j != 4 - 1) {
871 dum = 1.0 / (matrix[j][j]);
872 for(i = j + 1; i < 4; i++)
873 matrix[i][j] *= dum;
874 }
875
876 }
877
878 #else
879
880 // This is the completely unreadable, but much faster
881 // version of the above code. First, some macros that
882 // never change.
883
884 // macro for block 3, inner k loop
885 #define BLOCK3INNER(j,k) \
886 dum = matrix[imax][k]; \
887 matrix[imax][k] = matrix[j][k]; \
888 matrix[j][k] = dum;
889
890 // macro for block 4
891 #define BLOCK4(j) \
892 index[j] = imax; \
893 if(matrix[j][j] == 0.0) matrix[j][j] = 1e-20;
894
895 // Now the code...
896
897 // *********************************************
898 // j = 0
899 // *********************************************
900
901 // BLOCK 1, j = 0
902 // does nothing when j==0
903
904 big = 0.0;
905
906 // BLOCK 2, j = 0
907
908 // macro for block 2 when j == 0
909 // inner k loop does nothing when j == 0
910 #define BLOCK2J0(i) \
911 sum = matrix[i][0]; \
912 if((dum = vv[i] * ABS(sum)) >= big) { \
913 big = dum; \
914 imax = i; \
915 }
916
917 // for(i = j; i < 4; i++)
918 BLOCK2J0(0)
919 BLOCK2J0(1)
920 BLOCK2J0(2)
921 BLOCK2J0(3)
922 #undef BLOCK2J0
923
924 // BLOCK 3, j = 0
925 if(0 != imax) {
926 BLOCK3INNER(0,0);
927 BLOCK3INNER(0,1);
928 BLOCK3INNER(0,2);
929 BLOCK3INNER(0,3);
930 d = -d;
931 vv[imax] = vv[0];
932 }
933
934 // BLOCK 4, j = 0
935 BLOCK4(0);
936
937 // BLOCK 5, j = 0
938 dum = 1.0 / (matrix[0][0]);
939 // for(i = j + 1; i < 4; i++)
940 matrix[1][0] *= dum;
941 matrix[2][0] *= dum;
942 matrix[3][0] *= dum;
943
944 // *********************************************
945 // j = 1
946 // *********************************************
947
948 // BLOCK 1, j = 1
949 // for(i = 0; i < j; i++) {
950 sum = matrix[0][1];
951 // for(k = 0; k < i; k++)
952 // nothing
953
954 big = 0.0;
955
956 // BLOCK 2, j = 1
957
958 // macro for block 2 when j == 1
959 #define BLOCK2J1(i) \
960 sum = matrix[i][1]; \
961 sum -= matrix[i][0] * matrix[0][1]; \
962 matrix[i][1] = sum; \
963 if((dum = vv[i] * ABS(sum)) >= big) { \
964 big = dum; \
965 imax = i; \
966 }
967
968 // for(i = j; i < 4; i++)
969 BLOCK2J1(1)
970 BLOCK2J1(2)
971 BLOCK2J1(3)
972 #undef BLOCK2J1
973
974 // BLOCK 3, j = 1
975 if(1 != imax) {
976 BLOCK3INNER(1,0);
977 BLOCK3INNER(1,1);
978 BLOCK3INNER(1,2);
979 BLOCK3INNER(1,3);
980 d = -d;
981 vv[imax] = vv[1];
982 }
983
984 // BLOCK 4, j = 1
985 BLOCK4(1);
986
987 // BLOCK 5, j = 1
988 dum = 1.0 / (matrix[1][1]);
989 // for(i = j + 1; i < 4; i++)
990 matrix[2][1] *= dum;
991 matrix[3][1] *= dum;
992
993 // *********************************************
994 // j = 2
995 // *********************************************
996
997 // BLOCK 1, j = 2
998 // for(i = 0; i < j; i++) {
999 // i = 0
1000 sum = matrix[0][2];
1001 // for(k = 0; k < i; k++)
1002 // nothing
1003 // i = 1
1004 sum = matrix[1][2];
1005 // for(k = 0; k < i; k++)
1006 sum -= matrix[1][0] * matrix[0][2];
1007 matrix[1][2] = sum;
1008
1009 big = 0.0;
1010
1011 // BLOCK 2, j = 2
1012
1013 // macro for block 2 when j == 2
1014 #define BLOCK2J2(i) \
1015 sum = matrix[i][2]; \
1016 sum -= matrix[i][0] * matrix[0][2]; \
1017 sum -= matrix[i][1] * matrix[1][2]; \
1018 matrix[i][2] = sum; \
1019 if((dum = vv[i] * ABS(sum)) >= big) { \
1020 big = dum; \
1021 imax = i; \
1022 }
1023
1024 // for(i = j; i < 4; i++)
1025 BLOCK2J2(2)
1026 BLOCK2J2(3)
1027 #undef BLOCK2J2
1028
1029 // BLOCK 3, j = 2
1030 if(2 != imax) {
1031 BLOCK3INNER(2,0);
1032 BLOCK3INNER(2,1);
1033 BLOCK3INNER(2,2);
1034 BLOCK3INNER(2,3);
1035 d = -d;
1036 vv[imax] = vv[2];
1037 }
1038
1039 // BLOCK 4, j = 2
1040 BLOCK4(2);
1041
1042 // BLOCK 5, j = 2
1043 dum = 1.0 / (matrix[2][2]);
1044 // for(i = j + 1; i < 4; i++)
1045 matrix[3][2] *= dum;
1046
1047 // *********************************************
1048 // j = 3
1049 // *********************************************
1050
1051 // BLOCK 1, j = 3
1052 // for(i = 0; i < j; i++) {
1053 // i = 0
1054 sum = matrix[0][3];
1055 // for(k = 0; k < i; k++)
1056 // nothing
1057 // i = 1
1058 sum = matrix[1][3];
1059 // for(k = 0; k < i; k++)
1060 sum -= matrix[1][0] * matrix[0][3];
1061 matrix[1][3] = sum;
1062 // i = 2
1063 sum = matrix[2][3];
1064 // for(k = 0; k < i; k++)
1065 sum -= matrix[2][0] * matrix[0][3];
1066 sum -= matrix[2][1] * matrix[1][3];
1067 matrix[2][3] = sum;
1068
1069 big = 0.0;
1070
1071 // BLOCK 2, j = 3
1072
1073 // macro for block 2 when j == 3
1074 #define BLOCK2J3(i) \
1075 sum = matrix[i][3]; \
1076 sum -= matrix[i][0] * matrix[0][3]; \
1077 sum -= matrix[i][1] * matrix[1][3]; \
1078 sum -= matrix[i][2] * matrix[2][3]; \
1079 matrix[i][3] = sum; \
1080 if((dum = vv[i] * ABS(sum)) >= big) { \
1081 big = dum; \
1082 imax = i; \
1083 }
1084
1085 // for(i = j; i < 4; i++)
1086 BLOCK2J3(3)
1087 #undef BLOCK2J3
1088
1089 // BLOCK 3, j = 3
1090 if(3 != imax) {
1091 BLOCK3INNER(3,0);
1092 BLOCK3INNER(3,1);
1093 BLOCK3INNER(3,2);
1094 BLOCK3INNER(3,3);
1095 d = -d;
1096 vv[imax] = vv[3];
1097 }
1098
1099 // BLOCK 4, j = 3
1100 BLOCK4(3);
1101
1102 // BLOCK 5, j = 3
1103 // does not execute when j == 3
1104
1105 #endif /* DEBUGGING */
1106
1107 return TRUE;
1108 }
1109
1110
1111 //
1112 // Perform back-subtitution on input LU matrix, from Numerial Recipies in C,
1113 // pg 44
1114 //
1115 void
1116 SbMatrix::LUBackSubstitution(int index[4], float b[4]) const
1117 {
1118 int ii = -1, ip, j;
1119 float sum;
1120 #ifdef DEBUGGING
1121 int i;
1122 #endif /* DEBUGGING */
1123
1124 #ifdef DEBUGGING
1125 for(i = 0; i < 4; i++) {
1126 ip = index[i];
1127 sum = b[ip];
1128 b[ip] = b[i];
1129 if(ii >= 0)
1130 for(j = ii; j <= i - 1; j++)
1131 sum -= matrix[i][j] * b[j];
1132 else if(sum) ii = i;
1133 b[i] = sum;
1134 }
1135 #else
1136 #define BACKSUB(i) \
1137 ip = index[i]; \
1138 sum = b[ip]; \
1139 b[ip] = b[i]; \
1140 if(ii >= 0) \
1141 for(j = ii; j <= i - 1; j++) \
1142 sum -= matrix[i][j] * b[j]; \
1143 else if(sum) ii = i; \
1144 b[i] = sum;
1145
1146 BACKSUB(0);
1147 BACKSUB(1);
1148 BACKSUB(2);
1149 BACKSUB(3);
1150 #undef BACKSUB
1151 #endif /* DEBUGGING */
1152
1153 #ifdef DEBUGGING
1154 for(i = 4 - 1; i >= 0; i--) {
1155 sum = b[i];
1156 for(j = i + 1; j < 4; j++)
1157 sum -= matrix[i][j]*b[j];
1158 b[i] = sum / matrix[i][i];
1159 }
1160 #else
1161 sum = b[3];
1162 b[3] = sum / matrix[3][3];
1163
1164 sum = b[2] - matrix[2][3]*b[3];
1165 b[2] = sum / matrix[2][2];
1166
1167 sum = b[1] - matrix[1][2]*b[2] - matrix[1][3]*b[3];
1168 b[1] = sum / matrix[1][1];
1169
1170 sum = b[0] - matrix[0][1]*b[1] - matrix[0][2]*b[2] - matrix[0][3]*b[3];
1171 b[0] = sum / matrix[0][0];
1172 #endif /* DEBUGGING */
1173 }
1174
1175 //
1176 // Returns transpose of matrix
1177 //
1178
1179 SbMatrix
1180 SbMatrix::transpose() const
1181 {
1182 return SbMatrix(matrix[0][0], matrix[1][0], matrix[2][0], matrix[3][0],
1183 matrix[0][1], matrix[1][1], matrix[2][1], matrix[3][1],
1184 matrix[0][2], matrix[1][2], matrix[2][2], matrix[3][2],
1185 matrix[0][3], matrix[1][3], matrix[2][3], matrix[3][3]);
1186 }
1187
1188 //
1189 // Composes the matrix from translation, rotation, scale, etc.
1190 //
1191
1192 void
1193 SbMatrix::setTransform(const SbVec3f &translation,
1194 const SbRotation &rotation,
1195 const SbVec3f &scaleFactor,
1196 const SbRotation &scaleOrientation,
1197 const SbVec3f &center)
1198 {
1199 #define TRANSLATE(vec) m.setTranslate(vec), multLeft(m)
1200 #define ROTATE(rot) rot.getValue(m), multLeft(m)
1201 SbMatrix m;
1202
1203 makeIdentity();
1204
1205 if (translation != SbVec3f(0,0,0))
1206 TRANSLATE(translation);
1207
1208 if (center != SbVec3f(0,0,0))
1209 TRANSLATE(center);
1210
1211 if (rotation != SbRotation(0,0,0,1))
1212 ROTATE(rotation);
1213
1214 if (scaleFactor != SbVec3f(1,1,1)) {
1215 SbRotation so = scaleOrientation;
1216 if (so != SbRotation(0,0,0,1))
1217 ROTATE(so);
1218
1219 m.setScale(scaleFactor);
1220 multLeft(m);
1221
1222 if (so != SbRotation(0,0,0,1)) {
1223 so.invert();
1224 ROTATE(so);
1225 }
1226 }
1227
1228 if (center != SbVec3f(0,0,0))
1229 TRANSLATE(-center);
1230
1231 #undef TRANSLATE
1232 #undef ROTATE
1233 }
1234
1235 //
1236 // Decomposes a rotation into translation etc, based on scale
1237 //
1238
1239 void
1240 SbMatrix::getTransform(SbVec3f &translation,
1241 SbRotation &rotation,
1242 SbVec3f &scaleFactor,
1243 SbRotation &scaleOrientation,
1244 const SbVec3f &center) const
1245 {
1246 SbMatrix so, rot, proj;
1247 if (center != SbVec3f(0,0,0)) {
1248 // to get fields for a non-0 center, we
1249 // need to decompose a new matrix "m" such
1250 // that [-center][m][center] = [this]
1251 // i.e., [m] = [center][this][-center]
1252 // (this trick stolen from Showcase code)
1253 SbMatrix m,c;
1254 m.setTranslate(-center);
1255 m.multLeft(*this);
1256 c.setTranslate(center);
1257 m.multLeft(c);
1258 m.factor(so,scaleFactor,rot,translation,proj);
1259 }
1260 else
1261 this->factor(so,scaleFactor,rot,translation,proj);
1262
1263 scaleOrientation = so.transpose(); // have to transpose because factor
1264 // gives us transpose of correct answer.
1265 rotation = rot;
1266 }
1267
1268
1269
1270 ////////////////////////////////////////////
1271 //
1272 // Matrix/matrix and matrix/vector arithmetic
1273 //
1274 ////////////////////////////////////////////
1275
1276 //
1277 // Multiplies matrix by given matrix on right
1278 //
1279
1280 SbMatrix &
1281 SbMatrix::multRight(const SbMatrix &m)
1282 {
1283 // Trivial cases
1284 if (IS_IDENTITY(m))
1285 return *this;
1286 else if (IS_IDENTITY(matrix))
1287 return (*this = m);
1288
1289 SbMat tmp;
1290
1291 #define MULT_RIGHT(i,j) (matrix[i][0]*m.matrix[0][j] + \
1292 matrix[i][1]*m.matrix[1][j] + \
1293 matrix[i][2]*m.matrix[2][j] + \
1294 matrix[i][3]*m.matrix[3][j])
1295
1296 tmp[0][0] = MULT_RIGHT(0,0);
1297 tmp[0][1] = MULT_RIGHT(0,1);
1298 tmp[0][2] = MULT_RIGHT(0,2);
1299 tmp[0][3] = MULT_RIGHT(0,3);
1300 tmp[1][0] = MULT_RIGHT(1,0);
1301 tmp[1][1] = MULT_RIGHT(1,1);
1302 tmp[1][2] = MULT_RIGHT(1,2);
1303 tmp[1][3] = MULT_RIGHT(1,3);
1304 tmp[2][0] = MULT_RIGHT(2,0);
1305 tmp[2][1] = MULT_RIGHT(2,1);
1306 tmp[2][2] = MULT_RIGHT(2,2);
1307 tmp[2][3] = MULT_RIGHT(2,3);
1308 tmp[3][0] = MULT_RIGHT(3,0);
1309 tmp[3][1] = MULT_RIGHT(3,1);
1310 tmp[3][2] = MULT_RIGHT(3,2);
1311 tmp[3][3] = MULT_RIGHT(3,3);
1312
1313 #undef MULT_RIGHT
1314
1315 return (*this = tmp);
1316 }
1317
1318 //
1319 // Multiplies matrix by given matrix on left
1320 //
1321
1322 SbMatrix &
1323 SbMatrix::multLeft(const SbMatrix &m)
1324 {
1325 // Trivial cases
1326 if (IS_IDENTITY(m))
1327 return *this;
1328 else if (IS_IDENTITY(matrix))
1329 return (*this = m);
1330
1331 SbMat tmp;
1332
1333 #define MULT_LEFT(i,j) (m.matrix[i][0]*matrix[0][j] + \
1334 m.matrix[i][1]*matrix[1][j] + \
1335 m.matrix[i][2]*matrix[2][j] + \
1336 m.matrix[i][3]*matrix[3][j])
1337
1338 tmp[0][0] = MULT_LEFT(0,0);
1339 tmp[0][1] = MULT_LEFT(0,1);
1340 tmp[0][2] = MULT_LEFT(0,2);
1341 tmp[0][3] = MULT_LEFT(0,3);
1342 tmp[1][0] = MULT_LEFT(1,0);
1343 tmp[1][1] = MULT_LEFT(1,1);
1344 tmp[1][2] = MULT_LEFT(1,2);
1345 tmp[1][3] = MULT_LEFT(1,3);
1346 tmp[2][0] = MULT_LEFT(2,0);
1347 tmp[2][1] = MULT_LEFT(2,1);
1348 tmp[2][2] = MULT_LEFT(2,2);
1349 tmp[2][3] = MULT_LEFT(2,3);
1350 tmp[3][0] = MULT_LEFT(3,0);
1351 tmp[3][1] = MULT_LEFT(3,1);
1352 tmp[3][2] = MULT_LEFT(3,2);
1353 tmp[3][3] = MULT_LEFT(3,3);
1354
1355 #undef MULT_LEFT
1356
1357 return (*this = tmp);
1358
1359 }
1360
1361 //
1362 // Multiplies matrix by given column vector, giving vector result
1363 //
1364
1365 void
1366 SbMatrix::multMatrixVec(const SbVec3f &src, SbVec3f &dst) const
1367 {
1368 float x,y,z,w;
1369
1370 x = matrix[0][0]*src[0] + matrix[0][1]*src[1] +
1371 matrix[0][2]*src[2] + matrix[0][3];
1372 y = matrix[1][0]*src[0] + matrix[1][1]*src[1] +
1373 matrix[1][2]*src[2] + matrix[1][3];
1374 z = matrix[2][0]*src[0] + matrix[2][1]*src[1] +
1375 matrix[2][2]*src[2] + matrix[2][3];
1376 w = matrix[3][0]*src[0] + matrix[3][1]*src[1] +
1377 matrix[3][2]*src[2] + matrix[3][3];
1378
1379 dst.setValue(x/w, y/w, z/w);
1380 }
1381
1382 //
1383 // Multiplies given row vector by matrix, giving vector result
1384 //
1385
1386 void
1387 SbMatrix::multVecMatrix(const SbVec3f &src, SbVec3f &dst) const
1388 {
1389 float x,y,z,w;
1390
1391 x = src[0]*matrix[0][0] + src[1]*matrix[1][0] +
1392 src[2]*matrix[2][0] + matrix[3][0];
1393 y = src[0]*matrix[0][1] + src[1]*matrix[1][1] +
1394 src[2]*matrix[2][1] + matrix[3][1];
1395 z = src[0]*matrix[0][2] + src[1]*matrix[1][2] +
1396 src[2]*matrix[2][2] + matrix[3][2];
1397 w = src[0]*matrix[0][3] + src[1]*matrix[1][3] +
1398 src[2]*matrix[2][3] + matrix[3][3];
1399
1400 dst.setValue(x/w, y/w, z/w);
1401 }
1402
1403 //
1404 // Multiplies given row vector by matrix, giving vector result
1405 // src is assumed to be a direction vector, so translation part of
1406 // matrix is ignored.
1407 //
1408
1409 void
1410 SbMatrix::multDirMatrix(const SbVec3f &src, SbVec3f &dst) const
1411 {
1412 float x,y,z;
1413
1414 x = src[0]*matrix[0][0] + src[1]*matrix[1][0] + src[2]*matrix[2][0];
1415 y = src[0]*matrix[0][1] + src[1]*matrix[1][1] + src[2]*matrix[2][1];
1416 z = src[0]*matrix[0][2] + src[1]*matrix[1][2] + src[2]*matrix[2][2];
1417
1418 dst.setValue(x, y, z);
1419 }
1420
1421 //
1422 // Multiplies the given line by the matrix resulting in a new
1423 // line. The origin of the line is transformed by the whole matrix.
1424 // The translation part of the matrix is ignored when transforming
1425 // the direction of the line.
1426 //
1427
1428 void
1429 SbMatrix::multLineMatrix(const SbLine &src, SbLine &dst) const
1430 {
1431 SbVec3f pos, dir;
1432 multVecMatrix(src.getPosition(), pos);
1433 multDirMatrix(src.getDirection(), dir);
1434 dst.setValue(pos, pos+dir);
1435 }
1436
1437
1438 ////////////////////////////////////////////
1439 //
1440 // Miscellaneous
1441 //
1442 ////////////////////////////////////////////
1443
1444 ////////////////////////////////////////////////////////////////////////
1445 //
1446 // Description:
1447 // Prints a formatted version of the matrix to the given file
1448 // pointer.
1449 //
1450 // Use: public
1451
1452 void
1453 SbMatrix::print(FILE *fp) const
1454 //
1455 ////////////////////////////////////////////////////////////////////////
1456 {
1457 int i, j;
1458
1459 for (i = 0; i < 4; i++)
1460 for (j = 0; j < 4; j++)
1461 fprintf(fp, "%10.5g%c", matrix[i][j], j < 3 ? '\t' : '\n');
1462 }
1463
1464 ////////////////////////////////////////////
1465 //
1466 // Operators
1467 //
1468 ////////////////////////////////////////////
1469
1470 //
1471 // Sets value from 4x4 array of elements
1472 //
1473
1474 SbMatrix &
1475 SbMatrix::operator =(const SbMat &m)
1476 {
1477 setValue(m);
1478 return *this;
1479 }
1480
1481 //
1482 // ??? why do we need to write this out like this?
1483 // ??? it doesn't seem to work otherwise, but why not?
1484 //
1485 SbMatrix &
1486 SbMatrix::operator =(const SbMatrix &m)
1487 {
1488 matrix[0][0] = m.matrix[0][0];
1489 matrix[0][1] = m.matrix[0][1];
1490 matrix[0][2] = m.matrix[0][2];
1491 matrix[0][3] = m.matrix[0][3];
1492 matrix[1][0] = m.matrix[1][0];
1493 matrix[1][1] = m.matrix[1][1];
1494 matrix[1][2] = m.matrix[1][2];
1495 matrix[1][3] = m.matrix[1][3];
1496 matrix[2][0] = m.matrix[2][0];
1497 matrix[2][1] = m.matrix[2][1];
1498 matrix[2][2] = m.matrix[2][2];
1499 matrix[2][3] = m.matrix[2][3];
1500 matrix[3][0] = m.matrix[3][0];
1501 matrix[3][1] = m.matrix[3][1];
1502 matrix[3][2] = m.matrix[3][2];
1503 matrix[3][3] = m.matrix[3][3];
1504
1505 return *this;
1506 }
1507
1508
1509 //
1510 // Binary multiplication of matrices
1511 //
1512
1513 SbMatrix
1514 operator *(const SbMatrix &l, const SbMatrix &r)
1515 {
1516 SbMatrix m = l;
1517
1518 m *= r;
1519
1520 return m;
1521 }
1522
1523 //
1524 // Equality comparison operator. All componenents must match exactly.
1525 //
1526
1527 int
1528 operator ==(const SbMatrix &m1, const SbMatrix &m2)
1529 {
1530 return (
1531 m1.matrix[0][0] == m2.matrix[0][0] &&
1532 m1.matrix[0][1] == m2.matrix[0][1] &&
1533 m1.matrix[0][2] == m2.matrix[0][2] &&
1534 m1.matrix[0][3] == m2.matrix[0][3] &&
1535
1536 m1.matrix[1][0] == m2.matrix[1][0] &&
1537 m1.matrix[1][1] == m2.matrix[1][1] &&
1538 m1.matrix[1][2] == m2.matrix[1][2] &&
1539 m1.matrix[1][3] == m2.matrix[1][3] &&
1540
1541 m1.matrix[2][0] == m2.matrix[2][0] &&
1542 m1.matrix[2][1] == m2.matrix[2][1] &&
1543 m1.matrix[2][2] == m2.matrix[2][2] &&
1544 m1.matrix[2][3] == m2.matrix[2][3] &&
1545
1546 m1.matrix[3][0] == m2.matrix[3][0] &&
1547 m1.matrix[3][1] == m2.matrix[3][1] &&
1548 m1.matrix[3][2] == m2.matrix[3][2] &&
1549 m1.matrix[3][3] == m2.matrix[3][3]
1550 );
1551 }
1552
1553 //
1554 // Equality comparison operator within given tolerance for each component
1555 //
1556
1557 SbBool
1558 SbMatrix::equals(const SbMatrix &m, float tolerance) const
1559 {
1560 int i, j;
1561 float d;
1562
1563 for (i = 0; i < 4; i++)
1564 for (j = 0; j < 4; j++) {
1565 d = matrix[i][j] - m.matrix[i][j];
1566 if (ABS(d) > tolerance)
1567 return FALSE;
1568 }
1569
1570 return TRUE;
1571 }