1 /**************************************************************************\
2 * Copyright (c) Kongsberg Oil & Gas Technologies AS
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are
7 * met:
8 *
9 * Redistributions of source code must retain the above copyright notice,
10 * this list of conditions and the following disclaimer.
11 *
12 * Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
15 *
16 * Neither the name of the copyright holder nor the names of its
17 * contributors may be used to endorse or promote products derived from
18 * this software without specific prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
24 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
25 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
26 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
27 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
28 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
29 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 \**************************************************************************/
32
33 /*!
34 \class SbMatrix SbMatrix.h Inventor/SbMatrix.h
35 \brief The SbMatrix class is a 4x4 dimensional representation of a matrix.
36
37 \ingroup base
38
39 SbMatrix is used by many other classes in Coin. It provides storage
40 for a 4x4 matrix of single-precision floating point values.
41
42 By definition, matrices in Coin should be set up in column-order
43 mode. This is the same order as used by e.g. OpenGL, but note that
44 books on geometry often uses the opposite row-order mode, which can
45 confuse new-comers to the API.
46
47 Another way to think of column-order matrices is that they use
48 post-order multiplications: that is, to concatenate a transformation
49 from a second matrix with your current matrix, it should be
50 multiplied on the right-hand side, i.e. with the
51 SbMatrix::multRight() method.
52
53 If you have a matrix in row-order from some other source, it can be
54 "converted" to column-order by transposing it with
55 SbMatrix::transpose(). A simple example will help to explain
56 this.
57
58 With row-order matrices, a transformation matrix with position,
59 rotation and scale looks like this:
60
61 \code
62 M = T * R * S
63 \endcode
64
65 Where T is translation, R is rotation and S is the scale. What this
66 means is that scale is applied first. The scaled matrix is then
67 rotated, and finally the scaled and rotated matrix is
68 translated. When using column-order matrices, as done in Coin,
69 matrices are represented slightly differently; the order of
70 multiplication is reversed:
71
72 \code
73 M = S * R * T
74 \endcode
75
76 The transformation is just the same as the row-order matrix. The
77 only difference being the order of multiplication. To understand why
78 this is so, consider the sample transformation:
79
80 \code
81 M = T * R * S
82 \endcode
83
84 Converting M from a row-order matrix to a column-order matrix is
85 done as follows:
86
87 \code
88 M^t = (T * R * S)^t
89 M^t = ((T * R) * S)^t
90 M^t = S^t * (T * R)^t
91 M^t = S^t * R^t * T^t
92 \endcode
93
94 All left to be done is to remove the transpose symbols, and the
95 matrices have been converted to column-order matrices:
96
97 \code
98 M = S * R * T
99 \endcode
100
101 This was done using the fact that:
102
103 \code
104 A^t = (B * C)^t = C^t * B^t
105 \endcode
106
107 Converting from column-order to row-order is done using the same
108 principle.
109 */
110
111 // FIXME:
112 //
113 // * The SbMatrix::factor() function has not been implemented yet.
114 //
115 // * The element access methods should be inlined.
116 //
117 // * Not a lot of optimizations have been done yet, so there's a lot
118 // of room for improvements.
119
120
121 #include <Inventor/SbMatrix.h>
122
123 #include <cassert>
124 #include <cstring>
125 #include <cfloat>
126
127 #include <Inventor/SbDPMatrix.h>
128 #include <Inventor/SbRotation.h>
129 #include <Inventor/SbLine.h>
130
131 #if COIN_DEBUG
132 #include <Inventor/errors/SoDebugError.h>
133 #endif // COIN_DEBUG
134
135 #include "coindefs.h" // COIN_STUB()
136
137 #ifndef COIN_WORKAROUND_NO_USING_STD_FUNCS
138 using std::memmove;
139 using std::memcmp;
140 using std::memcpy;
141 #endif // !COIN_WORKAROUND_NO_USING_STD_FUNCS
142
143 class SbMatrixP {
144 public:
145 // FIXME: should merge all the PD code we're using from GGIV into
146 // SbMatrix, SbRotation and SbVec3f proper (for two reasons: 1)
147 // there's a lot of duplicated code here (like for instance the
148 // matrix->quaternion decomposition, which also exists in
149 // SbRotation::setValue(SbMatrix&)), and 2) the remaining code
150 // snippets look generally useful outside the purpose of breaking down
151 // a matrix into it's transformation components). 20010114 mortene.
152
153 /*
154 * declarations for polar_decomp algorithm from Graphics Gems IV,
155 * by Ken Shoemake <shoemake@graphics.cis.upenn.edu>
156 */
157 enum QuatPart {X, Y, Z, W};
158 typedef float HMatrix[4][4]; /* Right-handed, for column vectors */
159 typedef struct {
160 SbVec4f t; /* Translation components */
161 SbRotation q; /* Essential rotation */
162 SbRotation u; /* Stretch rotation */
163 SbVec4f k; /* Stretch factors */
164 float f; /* Sign of determinant */
165 } AffineParts;
166 static void decomp_affine(HMatrix A, AffineParts * parts);
167 static SbRotation snuggle(SbRotation q, SbVec4f & k);
168 static SbVec4f spect_decomp(SbMatrixP::HMatrix S, SbMatrixP::HMatrix U);
169 static float polar_decomp(SbMatrixP::HMatrix M, SbMatrixP::HMatrix Q, SbMatrixP::HMatrix S);
170
171 static const SbMat IDENTITYMATRIX;
172
isIdentity(const float fm[][4])173 static SbBool isIdentity(const float fm[][4]) {
174 #if 0 // I would assume that the memcmp() version is faster..? Should run some profile checks.
175 return ((fm[0][0] == 1.0f) && (fm[0][1] == 0.0f) && (fm[0][2] == 0.0f) && (fm[0][3] == 0.0f) &&
176 (fm[1][0] == 0.0f) && (fm[1][1] == 1.0f) && (fm[1][2] == 0.0f) && (fm[1][3] == 0.0f) &&
177 (fm[2][0] == 0.0f) && (fm[2][1] == 0.0f) && (fm[2][2] == 1.0f) && (fm[2][3] == 0.0f) &&
178 (fm[3][0] == 0.0f) && (fm[3][1] == 0.0f) && (fm[3][2] == 0.0f) && (fm[3][3] == 1.0f));
179 #else
180 // Note: as far as I know, memcmp() only compares bytes until
181 // there's a mismatch (and does *not* run over the full array and
182 // adds up a total, as it sometimes seems from documentation). So
183 // this should be very quick for non-identity matrices.
184 //
185 // Also, we check the first value on it's own, to avoid the function
186 // call for the most common case.
187 return (fm[0][0]==1.0f) && memcmp(&fm[0][1], &IDENTITYMATRIX[0][1], (4 * 3 + 3) * sizeof(float)) == 0;
188 #endif
189 }
190
191 static HMatrix mat_id;
192
193 private:
194 static void mat_mult(SbMatrixP::HMatrix A, SbMatrixP::HMatrix B, SbMatrixP::HMatrix AB);
195 static float vdot(float * va, float * vb);
196 static void vcross(float * va, float * vb, float * v);
197 static void adjoint_transpose(SbMatrixP::HMatrix M, SbMatrixP::HMatrix MadjT);
198 static float mat_norm(SbMatrixP::HMatrix M, int tpose);
norm_inf(SbMatrixP::HMatrix M)199 static float norm_inf(SbMatrixP::HMatrix M) {return mat_norm(M, 0);}
norm_one(SbMatrixP::HMatrix M)200 static float norm_one(SbMatrixP::HMatrix M) {return mat_norm(M, 1);}
201 static int find_max_col(SbMatrixP::HMatrix M);
202 static void make_reflector(float * v, float * u);
203 static void reflect_cols(SbMatrixP::HMatrix M, float * u);
204 static void reflect_rows(SbMatrixP::HMatrix M, float * u);
205 static void do_rank1(SbMatrixP::HMatrix M, SbMatrixP::HMatrix Q);
206 static void do_rank2(SbMatrixP::HMatrix M, SbMatrixP::HMatrix MadjT, SbMatrixP::HMatrix Q);
207 };
208
209 const SbMat SbMatrixP::IDENTITYMATRIX = {
210 { 1.0f, 0.0f, 0.0f, 0.0f },
211 { 0.0f, 1.0f, 0.0f, 0.0f },
212 { 0.0f, 0.0f, 1.0f, 0.0f },
213 { 0.0f, 0.0f, 0.0f, 1.0f }
214 };
215
216 SbMatrixP::HMatrix SbMatrixP::mat_id = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
217
218 /*!
219 The default constructor does nothing. The matrix will be uninitialized.
220 */
SbMatrix(void)221 SbMatrix::SbMatrix(void)
222 {
223 }
224
225
226 /*!
227 Constructs a matrix instance with the given initial elements.
228 */
SbMatrix(const float a11,const float a12,const float a13,const float a14,const float a21,const float a22,const float a23,const float a24,const float a31,const float a32,const float a33,const float a34,const float a41,const float a42,const float a43,const float a44)229 SbMatrix::SbMatrix(const float a11, const float a12,
230 const float a13, const float a14,
231 const float a21, const float a22,
232 const float a23, const float a24,
233 const float a31, const float a32,
234 const float a33, const float a34,
235 const float a41, const float a42,
236 const float a43, const float a44)
237 {
238 const SbMat m = { { a11, a12, a13, a14 },
239 { a21, a22, a23, a24 },
240 { a31, a32, a33, a34 },
241 { a41, a42, a43, a44 } };
242 this->setValue(m);
243 }
244
245 /*!
246 Constructs a matrix instance with the initial elements from the
247 \a matrix argument.
248 */
SbMatrix(const SbMat & matrixref)249 SbMatrix::SbMatrix(const SbMat & matrixref)
250 {
251 this->setValue(matrixref);
252 }
253
254 /*!
255 This constructor is courtesy of the Microsoft Visual C++ compiler.
256 */
SbMatrix(const SbMat * matrixptr)257 SbMatrix::SbMatrix(const SbMat * matrixptr)
258 {
259 this->setValue(*matrixptr);
260 }
261
262 /*!
263 This constructor is courtesy of the Microsoft Visual C++ compiler.
264 */
SbMatrix(const SbDPMatrix & matrixref)265 SbMatrix::SbMatrix(const SbDPMatrix & matrixref)
266 {
267 this->setValue(matrixref);
268 }
269
270 /*!
271 Default destructor does nothing.
272 */
~SbMatrix(void)273 SbMatrix::~SbMatrix(void)
274 {
275 }
276
277 /*!
278 Returns a pointer to the 2 dimensional float array with the matrix
279 elements.
280
281 \sa setValue().
282 */
283 const SbMat &
getValue(void) const284 SbMatrix::getValue(void) const
285 {
286 return this->matrix;
287 }
288
289 /*!
290 Copies the elements from \a m into the matrix.
291
292 \sa getValue().
293 */
294 void
setValue(const SbMat & m)295 SbMatrix::setValue(const SbMat & m)
296 {
297 (void)memmove(this->matrix, m, sizeof(float)*4*4);
298 }
299
300 /*!
301 Copies the elements from \a pMat into the matrix.
302
303 \sa getValue().
304 */
305 void
setValue(const float * pMat)306 SbMatrix::setValue(const float * pMat)
307 {
308 (void)memmove(this->matrix, pMat, sizeof(float)*4*4);
309 }
310
311 /*!
312 Copies the elements from \a m into the matrix.
313 */
314 void
setValue(const SbDPMatrix & m)315 SbMatrix::setValue(const SbDPMatrix & m)
316 {
317 const SbDPMat & dmat = m.getValue();
318 const SbMat smat = { { float(dmat[0][0]), float(dmat[0][1]),
319 float(dmat[0][2]), float(dmat[0][3]) },
320 { float(dmat[1][0]), float(dmat[1][1]),
321 float(dmat[1][2]), float(dmat[1][3]) },
322 { float(dmat[2][0]), float(dmat[2][1]),
323 float(dmat[2][2]), float(dmat[2][3]) },
324 { float(dmat[3][0]), float(dmat[3][1]),
325 float(dmat[3][2]), float(dmat[3][3]) } };
326 this->setValue(smat);
327 }
328
329 /*!
330 Assignment operator. Copies the elements from \a m to the matrix.
331 */
332 SbMatrix &
operator =(const SbMat & m)333 SbMatrix::operator=(const SbMat & m)
334 {
335 this->setValue(m);
336 return *this;
337 }
338
339 /*!
340 Assignment operator. Copies the elements from \a m to the matrix.
341 */
342 SbMatrix &
operator =(const SbMatrix & m)343 SbMatrix::operator=(const SbMatrix & m)
344 {
345 this->setValue(m.matrix);
346 return *this;
347 }
348
349 /*!
350 Set the matrix to be the identity matrix.
351
352 \sa identity().
353 */
354 void
makeIdentity(void)355 SbMatrix::makeIdentity(void)
356 {
357 this->matrix[0][0]=this->matrix[1][1]=
358 this->matrix[2][2]=this->matrix[3][3] = 1.0f;
359
360 this->matrix[0][1]=this->matrix[0][2]=this->matrix[0][3]=
361 this->matrix[1][0]=this->matrix[1][2]=this->matrix[1][3]=
362 this->matrix[2][0]=this->matrix[2][1]=this->matrix[2][3]=
363 this->matrix[3][0]=this->matrix[3][1]=this->matrix[3][2] = 0.0f;
364 }
365
366 /*!
367 Set matrix to be a rotation matrix with the given rotation.
368
369 \sa setTranslate(), setScale().
370 */
371 void
setRotate(const SbRotation & q)372 SbMatrix::setRotate(const SbRotation & q)
373 {
374 q.getValue(*this);
375 }
376
377 /*!
378 Multiply all element values in the matrix with \a v.
379 */
380 void
operator *=(const float v)381 SbMatrix::operator*=(const float v)
382 {
383 for (int i=0; i < 4; i++) {
384 for (int j=0; j < 4; j++) {
385 this->matrix[i][j] *= v;
386 }
387 }
388 }
389
390 /*!
391 Divide all element values in the matrix on \a v.
392 */
393 void
operator /=(const float v)394 SbMatrix::operator/=(const float v)
395 {
396 #if COIN_DEBUG
397 if (v==0.0f)
398 SoDebugError::postWarning("SbMatrix::operator/=",
399 "Division by zero.");
400 #endif // COIN_DEBUG
401
402 this->operator*=(1.0f/v);
403 }
404
405 /*!
406 Returns the determinant of the 3x3 submatrix specified by the row and
407 column indices.
408 */
409 float
det3(int r1,int r2,int r3,int c1,int c2,int c3) const410 SbMatrix::det3(int r1, int r2, int r3,
411 int c1, int c2, int c3) const
412 {
413 #if COIN_EXTRA_DEBUG
414 // Check indices.
415 if (r1<0 || r1>3 || r2<0 || r2>3 || r3<0 || r3>3 ||
416 c1<0 || c1>3 || c2<0 || c2>3 || c3<0 || c3>3) {
417 SoDebugError::post("SbMatrix::det3",
418 "At least one idx out of bounds [0, 3]. ");
419 }
420 if (r1==r2 || r1==r3 || r2==r3 ||
421 c1==c2 || c1==c3 || c2==c3)
422 SoDebugError::post("SbMatrix::det3", "Indices should be distinct.");
423 #endif // COIN_EXTRA_DEBUG
424
425 // More or less directly from "Advanced Engineering Mathematics"
426 // (E. Kreyszig), 6th edition.
427
428 float a11 = this->matrix[r1][c1];
429 float a12 = this->matrix[r1][c2];
430 float a13 = this->matrix[r1][c3];
431 float a21 = this->matrix[r2][c1];
432 float a22 = this->matrix[r2][c2];
433 float a23 = this->matrix[r2][c3];
434 float a31 = this->matrix[r3][c1];
435 float a32 = this->matrix[r3][c2];
436 float a33 = this->matrix[r3][c3];
437
438 float M11 = a22 * a33 - a32 * a23;
439 float M21 = -(a12 * a33 - a32 * a13);
440 float M31 = a12 * a23 - a22 * a13;
441
442 return (a11 * M11 + a21 * M21 + a31 * M31);
443 }
444
445 /*!
446 Returns the determinant of the upper left 3x3 submatrix.
447 */
448 float
det3(void) const449 SbMatrix::det3(void) const
450 {
451 return this->det3(0, 1, 2, 0, 1, 2);
452 }
453
454 /*!
455 Returns the determinant of the matrix.
456 */
457 float
det4(void) const458 SbMatrix::det4(void) const
459 {
460 float det = 0.0f;
461 det += this->matrix[0][0] * det3(1, 2, 3, 1, 2, 3);
462 det -= this->matrix[1][0] * det3(0, 2, 3, 1, 2, 3);
463 det += this->matrix[2][0] * det3(0, 1, 3, 1, 2, 3);
464 det -= this->matrix[3][0] * det3(0, 1, 2, 1, 2, 3);
465 return det;
466 }
467
468 /*!
469 Return a new matrix which is the inverse matrix of this.
470
471 The user is responsible for checking that this is a valid operation
472 to execute, by first making sure that the result of SbMatrix::det4()
473 is not equal to zero.
474 */
475 SbMatrix
inverse(void) const476 SbMatrix::inverse(void) const
477 {
478 #if 1 // new optimized version
479
480 // check for identity matrix
481 if (SbMatrixP::isIdentity(this->matrix)) { return SbMatrix::identity(); }
482
483 SbMatrix result;
484
485 // use local pointers for speed
486 float (*dst)[4];
487 dst = reinterpret_cast<float (*)[4]>(result.matrix[0]);
488 const float (*src)[4];
489 src = reinterpret_cast<const float (*)[4]>(this->matrix[0]);
490
491 // check for affine matrix (common case)
492 if (src[0][3] == 0.0f && src[1][3] == 0.0f &&
493 src[2][3] == 0.0f && src[3][3] == 1.0f) {
494
495 // More or less directly from:
496 // Kevin Wu, "Fast Matrix Inversion", Graphics Gems II
497 float det_1;
498 float pos, neg, temp;
499
500 #define ACCUMULATE \
501 if (temp >= 0.0f) \
502 pos += temp; \
503 else \
504 neg += temp
505
506 /*
507 * Calculate the determinant of submatrix A and determine if the
508 * the matrix is singular as limited by floating-point data
509 * representation.
510 */
511 pos = neg = 0.0f;
512 temp = src[0][0] * src[1][1] * src[2][2];
513 ACCUMULATE;
514 temp = src[0][1] * src[1][2] * src[2][0];
515 ACCUMULATE;
516 temp = src[0][2] * src[1][0] * src[2][1];
517 ACCUMULATE;
518 temp = -src[0][2] * src[1][1] * src[2][0];
519 ACCUMULATE;
520 temp = -src[0][1] * src[1][0] * src[2][2];
521 ACCUMULATE;
522 temp = -src[0][0] * src[1][2] * src[2][1];
523 ACCUMULATE;
524 det_1 = pos + neg;
525
526 #undef ACCUMULATE
527
528 /* Is the submatrix A singular? */
529 if ((det_1 == 0.0f) || (SbAbs(det_1 / (pos - neg)) < FLT_EPSILON)) {
530 /* Matrix M has no inverse */
531 #if COIN_DEBUG
532 SoDebugError::postWarning("SbMatrix::inverse",
533 "Matrix is singular.");
534 #endif // COIN_DEBUG
535 return *this;
536 }
537 else {
538 /* Calculate inverse(A) = adj(A) / det(A) */
539 det_1 = 1.0f / det_1;
540 dst[0][0] = (src[1][1] * src[2][2] -
541 src[1][2] * src[2][1]) * det_1;
542 dst[1][0] = - (src[1][0] * src[2][2] -
543 src[1][2] * src[2][0]) * det_1;
544 dst[2][0] = (src[1][0] * src[2][1] -
545 src[1][1] * src[2][0]) * det_1;
546 dst[0][1] = - (src[0][1] * src[2][2] -
547 src[0][2] * src[2][1]) * det_1;
548 dst[1][1] = (src[0][0] * src[2][2] -
549 src[0][2] * src[2][0]) * det_1;
550 dst[2][1] = - (src[0][0] * src[2][1] -
551 src[0][1] * src[2][0]) * det_1;
552 dst[0][2] = (src[0][1] * src[1][2] -
553 src[0][2] * src[1][1]) * det_1;
554 dst[1][2] = - (src[0][0] * src[1][2] -
555 src[0][2] * src[1][0]) * det_1;
556 dst[2][2] = (src[0][0] * src[1][1] -
557 src[0][1] * src[1][0]) * det_1;
558
559 /* Calculate -C * inverse(A) */
560 dst[3][0] = - (src[3][0] * dst[0][0] +
561 src[3][1] * dst[1][0] +
562 src[3][2] * dst[2][0]);
563 dst[3][1] = - (src[3][0] * dst[0][1] +
564 src[3][1] * dst[1][1] +
565 src[3][2] * dst[2][1]);
566 dst[3][2] = - (src[3][0] * dst[0][2] +
567 src[3][1] * dst[1][2] +
568 src[3][2] * dst[2][2]);
569
570 /* Fill in last column */
571 dst[0][3] = dst[1][3] = dst[2][3] = 0.0f;
572 dst[3][3] = 1.0f;
573 }
574 }
575 else { // non-affine matrix
576 float max, sum, tmp, inv_pivot;
577 int p[4];
578 int i, j, k;
579
580 // algorithm from: Schwarz, "Numerische Mathematik"
581 result = *this;
582
583 for (k = 0; k < 4; k++) {
584 max = 0.0f;
585 p[k] = 0;
586
587 for (i = k; i < 4; i++) {
588 sum = 0.0f;
589 for (j = k; j < 4; j++)
590 sum += SbAbs(dst[i][j]);
591 if (sum > 0.0f) {
592 tmp = SbAbs(dst[i][k]) / sum;
593 if (tmp > max) {
594 max = tmp;
595 p[k] = i;
596 }
597 }
598 }
599
600 if (max == 0.0f) {
601 #if COIN_DEBUG
602 SoDebugError::postWarning("SbMatrix::inverse",
603 "Matrix is singular.");
604 #endif // COIN_DEBUG
605 return *this;
606 }
607
608 if (p[k] != k) {
609 for (j = 0; j < 4; j++) {
610 tmp = dst[k][j];
611 dst[k][j] = dst[p[k]][j];
612 dst[p[k]][j] = tmp;
613 }
614 }
615
616 inv_pivot = 1.0f / dst[k][k];
617 for (j = 0; j < 4; j++) {
618 if (j != k) {
619 dst[k][j] = - dst[k][j] * inv_pivot;
620 for (i = 0; i < 4; i++) {
621 if (i != k) dst[i][j] += dst[i][k] * dst[k][j];
622 }
623 }
624 }
625
626 for (i = 0; i < 4; i++) dst[i][k] *= inv_pivot;
627 dst[k][k] = inv_pivot;
628 }
629
630 for (k = 2; k >= 0; k--) {
631 if (p[k] != k) {
632 for (i = 0; i < 4; i++) {
633 tmp = dst[i][k];
634 dst[i][k] = dst[i][p[k]];
635 dst[i][p[k]] = tmp;
636 }
637 }
638 }
639 }
640 return result;
641
642 #else // old unoptimized version
643
644 float det = this->det4();
645 #if COIN_DEBUG
646 if (fabs(det) < FLT_EPSILON) {
647 SoDebugError::postWarning("SbMatrix::inverse",
648 "Determinant of matrix is zero.");
649 return *this;
650 }
651 #endif // COIN_DEBUG
652
653 SbMatrix result;
654
655 // FIXME: we should be using an optimized way of calculating the
656 // inverse matrix. 20010114 mortene.
657 // UPDATE 20020513 mortene: pederb has written one which is now
658 // available in Coin/patches/.
659
660 result.matrix[0][0] = this->det3(1, 2, 3, 1, 2, 3);
661 result.matrix[1][0] = -this->det3(1, 2, 3, 0, 2, 3);
662 result.matrix[2][0] = this->det3(1, 2, 3, 0, 1, 3);
663 result.matrix[3][0] = -this->det3(1, 2, 3, 0, 1, 2);
664 result.matrix[0][1] = -this->det3(0, 2, 3, 1, 2, 3);
665 result.matrix[1][1] = this->det3(0, 2, 3, 0, 2, 3);
666 result.matrix[2][1] = -this->det3(0, 2, 3, 0, 1, 3);
667 result.matrix[3][1] = this->det3(0, 2, 3, 0, 1, 2);
668 result.matrix[0][2] = this->det3(0, 1, 3, 1, 2, 3);
669 result.matrix[1][2] = -this->det3(0, 1, 3, 0, 2, 3);
670 result.matrix[2][2] = this->det3(0, 1, 3, 0, 1, 3);
671 result.matrix[3][2] = -this->det3(0, 1, 3, 0, 1, 2);
672 result.matrix[0][3] = -this->det3(0, 1, 2, 1, 2, 3);
673 result.matrix[1][3] = this->det3(0, 1, 2, 0, 2, 3);
674 result.matrix[2][3] = -this->det3(0, 1, 2, 0, 1, 3);
675 result.matrix[3][3] = this->det3(0, 1, 2, 0, 1, 2);
676
677 result /= det;
678 return result;
679
680 #endif // old unoptimized version
681 }
682
683 /*!
684 Check if the \a m matrix is equal to this one, within the given tolerance
685 value. The tolerance value is applied in the comparison on a component by
686 component basis.
687 */
688 SbBool
equals(const SbMatrix & m,float tolerance) const689 SbMatrix::equals(const SbMatrix & m, float tolerance) const
690 {
691 #if COIN_DEBUG
692 if (tolerance<0.0f)
693 SoDebugError::postWarning("SbMatrix::equals",
694 "tolerance should be >=0.0f.");
695 #endif // COIN_DEBUG
696
697 for (int i=0; i < 4; i++) {
698 for (int j=0; j< 4; j++) {
699 if (fabs(this->matrix[i][j] - m.matrix[i][j]) > tolerance) return FALSE;
700 }
701 }
702
703 return TRUE;
704 }
705
706
707 /*!
708 Return pointer to the matrix' 4x4 float array.
709 */
operator float*(void)710 SbMatrix::operator float*(void)
711 {
712 return &(this->matrix[0][0]);
713 }
714
715
716 /*!
717 Return pointer to the matrix' 4x4 float array.
718 */
operator SbMat&(void)719 SbMatrix::operator SbMat&(void)
720 {
721 return this->matrix;
722 }
723
724 /*!
725 Returns pointer to the 4 element array representing a matrix row.
726 \a i should be within [0, 3].
727
728 \sa getValue(), setValue().
729 */
730 float *
operator [](int i)731 SbMatrix::operator [](int i)
732 {
733 #if COIN_EXTRA_DEBUG
734 if (i<0 || i>3) {
735 SoDebugError::post("SbMatrix::operator[]", "Index out of bounds. ");
736 }
737 #endif // COIN_EXTRA_DEBUG
738
739 return this->matrix[i];
740 }
741
742 /*!
743 Returns pointer to the 4 element array representing a matrix row.
744 \a i should be within [0, 3].
745
746 \sa getValue(), setValue().
747 */
748 const float *
operator [](int i) const749 SbMatrix::operator [](int i) const
750 {
751 #if COIN_EXTRA_DEBUG
752 if (i<0 || i>3) {
753 SoDebugError::postWarning("SbMatrix::operator[]", "Index out of bounds. ");
754 }
755 #endif // COIN_EXTRA_DEBUG
756
757 return this->matrix[i];
758 }
759
760 /*!
761 Set matrix to be a rotation matrix with the given rotation.
762
763 \sa setRotate().
764 */
765 SbMatrix&
operator =(const SbRotation & q)766 SbMatrix::operator =(const SbRotation & q)
767 {
768 this->setRotate(q);
769 return *this;
770 }
771
772 /*!
773 Right-multiply with the \a m matrix.
774
775 \sa multRight().
776 */
777 SbMatrix&
operator *=(const SbMatrix & m)778 SbMatrix::operator *=(const SbMatrix & m)
779 {
780 return this->multRight(m);
781 }
782
783 /*!
784 \relates SbMatrix
785
786 Multiplies matrix \a m1 with matrix \a m2 and returns the resultant
787 matrix.
788 */
789 SbMatrix
operator *(const SbMatrix & m1,const SbMatrix & m2)790 operator *(const SbMatrix & m1, const SbMatrix & m2)
791 {
792 SbMatrix result = m1;
793 result *= m2;
794 return result;
795 }
796
797 /*!
798 \relates SbMatrix
799
800 Compare matrices to see if they are equal. For two matrices to be equal,
801 all their individual elements must be equal.
802
803 \sa equals().
804 */
805 int
operator ==(const SbMatrix & m1,const SbMatrix & m2)806 operator ==(const SbMatrix & m1, const SbMatrix & m2)
807 {
808 for (int i=0; i < 4; i++) {
809 for (int j=0; j < 4; j++) {
810 if (m1.matrix[i][j] != m2.matrix[i][j]) return FALSE;
811 }
812 }
813
814 return TRUE;
815 }
816
817 /*!
818 \relates SbMatrix
819
820 Compare matrices to see if they are not equal. For two matrices to not be
821 equal, it is enough that at least one of their elements are unequal.
822
823 \sa equals().
824 */
825 int
operator !=(const SbMatrix & m1,const SbMatrix & m2)826 operator !=(const SbMatrix & m1, const SbMatrix & m2)
827 {
828 return !(m1 == m2);
829 }
830
831 /*!
832 Return matrix components in the SbMat structure.
833
834 \sa setValue().
835 */
836 void
getValue(SbMat & m) const837 SbMatrix::getValue(SbMat & m) const
838 {
839 (void)memmove(&m[0][0], &(this->matrix[0][0]), sizeof(float)*4*4);
840 }
841
842 /*!
843 Return the identity matrix.
844
845 \sa makeIdentity().
846 */
847 SbMatrix
identity(void)848 SbMatrix::identity(void)
849 {
850 return SbMatrix(&SbMatrixP::IDENTITYMATRIX);
851 }
852
853 /*!
854 Set matrix to be a pure scaling matrix. Scale factors are specified
855 by \a s.
856
857 \sa setRotate(), setTranslate().
858 */
859 void
setScale(const float s)860 SbMatrix::setScale(const float s)
861 {
862 this->makeIdentity();
863 this->matrix[0][0] = s;
864 this->matrix[1][1] = s;
865 this->matrix[2][2] = s;
866 }
867
868 /*!
869 Set matrix to be a pure scaling matrix. Scale factors in x, y and z
870 is specified by the \a s vector.
871
872 \sa setRotate(), setTranslate().
873 */
874 void
setScale(const SbVec3f & s)875 SbMatrix::setScale(const SbVec3f & s)
876 {
877 this->makeIdentity();
878 this->matrix[0][0] = s[0];
879 this->matrix[1][1] = s[1];
880 this->matrix[2][2] = s[2];
881 }
882
883 /*!
884 Make this matrix into a pure translation matrix (no scale or rotation
885 components) with the given vector \a t as the translation.
886
887 \sa setRotate(), setScale().
888 */
889 void
setTranslate(const SbVec3f & t)890 SbMatrix::setTranslate(const SbVec3f & t)
891 {
892 this->makeIdentity();
893 this->matrix[3][0] = t[0];
894 this->matrix[3][1] = t[1];
895 this->matrix[3][2] = t[2];
896 }
897
898 /*!
899 Set translation, rotation and scaling all at once. The resulting
900 matrix gets calculated like this:
901
902 \code
903 M = S * R * T
904 \endcode
905
906 where \a S, \a R and \a T is scaling, rotation and translation
907 matrices.
908
909 \sa setTranslate(), setRotate(), setScale() and getTransform().
910 */
911 void
setTransform(const SbVec3f & t,const SbRotation & r,const SbVec3f & s)912 SbMatrix::setTransform(const SbVec3f & t, const SbRotation & r, const SbVec3f & s)
913 {
914 // Unoptimized version:
915 //
916 // this->setScale(s);
917 // tmp.setRotate(r);
918 // this->multRight(tmp);
919 // tmp.setTranslate(t);
920 // this->multRight(tmp);
921
922 // Optimized version
923 SbMatrix tmp;
924 const SbRotation identity = SbRotation::identity();
925
926 if (s != SbVec3f(1.0f, 1.0f, 1.0f)) {
927 this->setScale(s);
928 if (r != identity) {
929 tmp.setRotate(r);
930 this->multRight(tmp);
931 }
932 if (t != SbVec3f(0.0f, 0.0f, 0.0f)) {
933 tmp.setTranslate(t);
934 this->multRight(tmp);
935 }
936 }
937 else {
938 if (r != identity) {
939 this->setRotate(r);
940 if (t != SbVec3f(0.f, 0.0f, 0.0f)) {
941 tmp.setTranslate(t);
942 this->multRight(tmp);
943 }
944 }
945 else {
946 this->setTranslate(t);
947 }
948 }
949 }
950
951 /*!
952 Set translation, rotation and scaling all at once with a specified
953 scale orientation. The resulting matrix gets calculated like this:
954
955 \code
956 M = Ro-� * S * Ro * R * T
957 \endcode
958
959 where \a Ro is the scale orientation, and \a S, \a R
960 and \a T is scaling, rotation and translation.
961
962 \sa setTranslate(), setRotate(), setScale() and getTransform().
963 */
964 void
setTransform(const SbVec3f & t,const SbRotation & r,const SbVec3f & s,const SbRotation & so)965 SbMatrix::setTransform(const SbVec3f & t, const SbRotation & r,
966 const SbVec3f & s, const SbRotation & so)
967 {
968
969 // Unoptimized version:
970 //
971 // this->setRotate(so.inverse());
972 // tmp.setScale(s);
973 // this->multRight(tmp);
974 // tmp.setRotate(so);
975 // this->multRight(tmp);
976 // tmp.setRotate(r);
977 // this->multRight(tmp);
978 // tmp.setTranslate(t);
979 // this->multRight(tmp);
980
981 // Optimized version. Much faster for the common case where only
982 // zero or one of the parameters are set.
983 SbMatrix tmp;
984 const SbRotation identity = SbRotation::identity();
985
986 if (s != SbVec3f(1.0f, 1.0f, 1.0f)) {
987 if (so != identity) {
988 this->setRotate(so.inverse());
989 tmp.setScale(s);
990 this->multRight(tmp);
991 tmp.setRotate(so);
992 this->multRight(tmp);
993 }
994 else {
995 this->setScale(s);
996 }
997 if (r != identity) {
998 tmp.setRotate(r);
999 this->multRight(tmp);
1000 }
1001 if (t != SbVec3f(0.0f, 0.0f, 0.0f)) {
1002 tmp.setTranslate(t);
1003 this->multRight(tmp);
1004 }
1005 }
1006 else {
1007 if (r != identity) {
1008 this->setRotate(r);
1009 if (t != SbVec3f(0.0f, 0.0f, 0.0f)) {
1010 tmp.setTranslate(t);
1011 this->multRight(tmp);
1012 }
1013 }
1014 else {
1015 this->setTranslate(t);
1016 }
1017 }
1018 }
1019
1020 /*!
1021 Set translation, rotation and scaling all at once with a specified
1022 scale orientation and center point. The resulting matrix gets
1023 calculated like this:
1024
1025 \code
1026 M = -Tc * Ro-� * S * Ro * R * T * Tc
1027 \endcode
1028
1029 where \a Tc is the center point, \a Ro the scale orientation, \a S,
1030 \a R and \a T is scaling, rotation and translation.
1031
1032 \sa setTranslate(), setRotate(), setScale() and getTransform().
1033 */
1034 void
setTransform(const SbVec3f & translation,const SbRotation & rotation,const SbVec3f & scaleFactor,const SbRotation & scaleOrientation,const SbVec3f & center)1035 SbMatrix::setTransform(const SbVec3f & translation,
1036 const SbRotation & rotation,
1037 const SbVec3f & scaleFactor,
1038 const SbRotation & scaleOrientation,
1039 const SbVec3f & center)
1040 {
1041 // Unoptimized version:
1042 //
1043 // this->setTranslate(-center);
1044 // tmp.setRotate(scaleOrientation.inverse());
1045 // this->multRight(tmp);
1046 // tmp.setScale(scaleFactor);
1047 // this->multRight(tmp);
1048 // tmp.setRotate(scaleOrientation);
1049 // this->multRight(tmp);
1050 // tmp.setRotate(rotation);
1051 // this->multRight(tmp);
1052 // tmp.setTranslate(translation);
1053 // this->multRight(tmp);
1054 // tmp.setTranslate(center);
1055 // this->multRight(tmp);
1056
1057 // Optimized version. Much faster for the common case where only
1058 // zero or one of the parameters are set.
1059 SbMatrix tmp;
1060 const SbRotation identity = SbRotation::identity();
1061
1062 this->setTranslate(-center);
1063
1064 if (scaleFactor != SbVec3f(1.0f, 1.0f, 1.0f)) {
1065 if (scaleOrientation != identity ) {
1066 tmp.setRotate(scaleOrientation.inverse());
1067 this->multRight(tmp);
1068 }
1069 tmp.setScale(scaleFactor);
1070 this->multRight(tmp);
1071
1072 if (scaleOrientation != identity) {
1073 tmp.setRotate(scaleOrientation);
1074 this->multRight(tmp);
1075 }
1076 }
1077
1078 if (rotation != identity) {
1079 tmp.setRotate(rotation);
1080 this->multRight(tmp);
1081 }
1082
1083 const SbVec3f sum = center + translation;
1084 if (sum != SbVec3f(0.f, 0.0f, 0.0f)) {
1085 tmp.setTranslate(sum);
1086 this->multRight(tmp);
1087 }
1088 }
1089
1090 /*!
1091 Factor the matrix back into its translation, rotation, scale and
1092 scaleorientation components.
1093
1094 \sa factor()
1095 */
1096 void
getTransform(SbVec3f & t,SbRotation & r,SbVec3f & s,SbRotation & so) const1097 SbMatrix::getTransform(SbVec3f & t, SbRotation & r, SbVec3f & s,
1098 SbRotation & so) const
1099 {
1100 // FIXME: test if this code works with non-affine matrices.
1101 // pederb, 2000-01-17
1102
1103 SbMatrixP::AffineParts parts;
1104 SbMatrixP::HMatrix hmatrix;
1105
1106 // transpose-copy
1107 hmatrix[0][0] = this->matrix[0][0];
1108 hmatrix[0][1] = this->matrix[1][0];
1109 hmatrix[0][2] = this->matrix[2][0];
1110 hmatrix[0][3] = this->matrix[3][0];
1111
1112 hmatrix[1][0] = this->matrix[0][1];
1113 hmatrix[1][1] = this->matrix[1][1];
1114 hmatrix[1][2] = this->matrix[2][1];
1115 hmatrix[1][3] = this->matrix[3][1];
1116
1117 hmatrix[2][0] = this->matrix[0][2];
1118 hmatrix[2][1] = this->matrix[1][2];
1119 hmatrix[2][2] = this->matrix[2][2];
1120 hmatrix[2][3] = this->matrix[3][2];
1121
1122 hmatrix[3][0] = this->matrix[0][3];
1123 hmatrix[3][1] = this->matrix[1][3];
1124 hmatrix[3][2] = this->matrix[2][3];
1125 hmatrix[3][3] = this->matrix[3][3];
1126
1127 SbMatrixP::decomp_affine(hmatrix, &parts);
1128
1129 float mul = 1.0f;
1130 if (parts.t[SbMatrixP::W] != 0.0f) mul = 1.0f / parts.t[SbMatrixP::W];
1131 t[0] = parts.t[SbMatrixP::X] * mul;
1132 t[1] = parts.t[SbMatrixP::Y] * mul;
1133 t[2] = parts.t[SbMatrixP::Z] * mul;
1134
1135 r = parts.q;
1136 mul = 1.0f;
1137 if (parts.k[SbMatrixP::W] != 0.0f) mul = 1.0f / parts.k[SbMatrixP::W];
1138 // mul be sign of determinant to support negative scales.
1139 mul *= parts.f;
1140 s[0] = parts.k[SbMatrixP::X] * mul;
1141 s[1] = parts.k[SbMatrixP::Y] * mul;
1142 s[2] = parts.k[SbMatrixP::Z] * mul;
1143
1144 so = parts.u;
1145 }
1146
1147 /*!
1148 Factor the matrix back into its \a translation, \a rotation,
1149 \a scaleFactor and \a scaleorientation components. Will eliminate
1150 the \a center variable from the matrix.
1151
1152 \sa factor()
1153 */
1154 void
getTransform(SbVec3f & translation,SbRotation & rotation,SbVec3f & scaleFactor,SbRotation & scaleOrientation,const SbVec3f & center) const1155 SbMatrix::getTransform(SbVec3f & translation,
1156 SbRotation & rotation,
1157 SbVec3f & scaleFactor,
1158 SbRotation & scaleOrientation,
1159 const SbVec3f & center) const
1160 {
1161 SbMatrix m2 = *this;
1162 SbMatrix trans;
1163 trans.setTranslate(center);
1164 m2.multLeft(trans);
1165 trans.setTranslate(-center);
1166 m2.multRight(trans);
1167
1168 m2.getTransform(translation, rotation, scaleFactor, scaleOrientation);
1169 }
1170
1171 /*!
1172 This function is not implemented in Coin.
1173
1174 \sa getTransform()
1175 */
1176 SbBool
factor(SbMatrix & COIN_UNUSED_ARG (r),SbVec3f & COIN_UNUSED_ARG (s),SbMatrix & COIN_UNUSED_ARG (u),SbVec3f & COIN_UNUSED_ARG (t),SbMatrix & COIN_UNUSED_ARG (proj)) const1177 SbMatrix::factor(SbMatrix & COIN_UNUSED_ARG(r), SbVec3f & COIN_UNUSED_ARG(s), SbMatrix & COIN_UNUSED_ARG(u), SbVec3f & COIN_UNUSED_ARG(t),
1178 SbMatrix & COIN_UNUSED_ARG(proj)) const
1179 {
1180 // FIXME: not implemented, not documented. 1998MMDD mortene.
1181 COIN_STUB();
1182 return FALSE;
1183 }
1184
1185 /*!
1186 This function produces a permuted LU decomposition of the matrix. It
1187 uses the common single-row-pivoting strategy.
1188
1189 \a FALSE is returned if the matrix is singular, which it never is, because
1190 of small adjustment values inserted if a singularity is found (as Open
1191 Inventor does too).
1192
1193 The parity argument is always set to 1.0 or -1.0. Don't really know what
1194 it's for, so it's not checked for correctness.
1195
1196 The index[] argument returns the permutation that was done on the matrix
1197 to LU-decompose it. index[i] is the row that row i was swapped with at
1198 step i in the decomposition, so index[] is not the actual permutation of
1199 the row indexes!
1200
1201 BUGS: The function does not produce results that are numerically identical
1202 with those produced by Open Inventor for the same matrices, because the
1203 pivoting strategy in OI was never fully understood.
1204
1205 \sa SbMatrix::LUBackSubstitution
1206 */
1207
1208
1209 SbBool
LUDecomposition(int index[4],float & d)1210 SbMatrix::LUDecomposition(int index[4], float & d)
1211 {
1212 int i;
1213 for (i = 0; i < 4; i++) index[i] = i;
1214 d = 1.0f;
1215
1216 const float MINIMUM_PIVOT = 1e-6f; // Inventor fix...
1217
1218 for (int row = 1; row < 4; row++) {
1219 int swap_row = row;
1220 float max_pivot = 0.0f;
1221 for (int test_row = row; test_row < 4; test_row++) {
1222 const float test_pivot = SbAbs(matrix[test_row][row]);
1223 if (test_pivot > max_pivot) {
1224 swap_row = test_row;
1225 max_pivot = test_pivot;
1226 }
1227 }
1228
1229 // swap rows
1230 if (swap_row != row) {
1231 d = -d;
1232 index[row] = swap_row;
1233 for (i = 0; i < 4; i++)
1234 SbSwap(matrix[row][i], matrix[swap_row][i]);
1235 }
1236
1237 float pivot = matrix[row][row];
1238 if (matrix[row][row] == 0.0f) {
1239 // return FALSE;
1240 // instead of returning FALSE on singulars...
1241 matrix[row][row] = pivot = MINIMUM_PIVOT;
1242 }
1243
1244 // the factorization
1245 for (i = row + 1; i < 4; i++) {
1246 const float factor = (matrix[i][row] /= pivot);
1247 for (int j = row + 1; j < 4; j++)
1248 matrix[i][j] -= factor * matrix[row][j];
1249 }
1250 }
1251 return TRUE;
1252 }
1253
1254 /*!
1255 This function does a solve on the "Ax = b" system, given that the matrix
1256 is LU-decomposed in advance. First, a forward substitution is done on the
1257 lower system (Ly = b), and then a backwards substitution is done on the
1258 upper triangular system (Ux = y).
1259
1260 The index[] argument is the one returned from
1261 SbMatrix::LUDecomposition(), so see that function for an explanation.
1262
1263 The b[] argument must contain the b vector in "Ax = b" when calling the
1264 function. After the function has solved the system, the b[] vector contains
1265 the x vector.
1266
1267 BUGS: As is done by Open Inventor, unsolvable x values will not return
1268 NaN but 0.
1269 */
1270
1271 void
LUBackSubstitution(int index[4],float b[4]) const1272 SbMatrix::LUBackSubstitution(int index[4], float b[4]) const
1273 {
1274 int i;
1275
1276 // permute b[] the way matrix[][] is permuted
1277 for (i = 0; i < 4; i++)
1278 if (i != index[i])
1279 SbSwap(b[i], b[index[i]]);
1280
1281 // forward substitution on L (Ly = b)
1282 float y[4];
1283 for (i = 0; i < 4; i++) {
1284 float sum = 0.0f;
1285 for (int j = 0; j < i; j++)
1286 sum += matrix[i][j] * y[j];
1287 y[i] = b[i] - sum;
1288 }
1289
1290 // backwards substitution on U (Ux = y)
1291 float x[4];
1292 for (i = 3; i >= 0; i--) {
1293 float sum = 0.0f;
1294 for (int j = i + 1; j < 4; j++)
1295 sum += matrix[i][j] * x[j];
1296 if (matrix[i][i] != 0.0f)
1297 x[i] = (y[i] - sum) / matrix[i][i];
1298 else
1299 x[i] = 0.0f;
1300 }
1301
1302 // de-permute x[]? doesn't look like it
1303 // for (i = 3; i >= 0; i--)
1304 // if (i != index[i])
1305 // SbSwap(x[i], x[index[i]]);
1306
1307 // copy x[] into b[] for "return to sender"
1308 for (i = 0; i < 4; i++) b[i] = x[i];
1309 }
1310
1311 /*!
1312 Returns the transpose of this matrix.
1313 */
1314
1315 SbMatrix
transpose(void) const1316 SbMatrix::transpose(void) const
1317 {
1318 SbMatrix trans = (*this);
1319
1320 for (int i=0; i < 3; i++) {
1321 for (int j=i+1; j < 4; j++) {
1322 SbSwap(trans[i][j], trans[j][i]);
1323 }
1324 }
1325
1326 return trans;
1327 }
1328
1329 /*!
1330 Let this matrix be right-multiplied by \a m. Returns reference to
1331 self.
1332
1333 This is the most common multiplication / concatenation operation
1334 when using column-order matrices, as SbMatrix instances are, by
1335 definition.
1336
1337 \sa multLeft()
1338 */
1339 SbMatrix &
multRight(const SbMatrix & m)1340 SbMatrix::multRight(const SbMatrix & m)
1341 {
1342 // Checks if one or the other matrix is equal to the identity matrix
1343 // before multiplying them. We do this because it's a major
1344 // optimization if one of them _is_, and the isIdentity() check
1345 // should be very quick in the common case where a matrix is not the
1346 // identity matrix.
1347 const SbMat & mfm = m.matrix;
1348 if (SbMatrixP::isIdentity(mfm)) { return *this; }
1349 SbMat & tfm = this->matrix;
1350 if (SbMatrixP::isIdentity(tfm)) { *this = m; return *this; }
1351
1352 SbMat tmp;
1353 (void)memcpy(tmp, tfm, 4*4*sizeof(float));
1354
1355 for (int i=0; i < 4; i++) {
1356 for (int j=0; j < 4; j++) {
1357 tfm[i][j] =
1358 tmp[i][0] * mfm[0][j] +
1359 tmp[i][1] * mfm[1][j] +
1360 tmp[i][2] * mfm[2][j] +
1361 tmp[i][3] * mfm[3][j];
1362 }
1363 }
1364
1365 return *this;
1366 }
1367
1368 /*!
1369 Let this matrix be left-multiplied by \a m. Returns reference to
1370 self.
1371
1372 (Be aware that it is more common to use the SbMatrix::multRight()
1373 operation, when doing concatenation of transformations, as SbMatrix
1374 instances are by definition in column-order, and uses
1375 post-multiplication for common geometry operations.)
1376
1377 \sa multRight()
1378 */
1379 SbMatrix&
multLeft(const SbMatrix & m)1380 SbMatrix::multLeft(const SbMatrix & m)
1381 {
1382 // Checks if one or the other matrix is equal to the identity
1383 // matrix. See also code comments at the start of
1384 // SbMatrix::multRight().
1385 const SbMat & mfm = m.matrix;
1386 if (SbMatrixP::isIdentity(mfm)) { return *this; }
1387 SbMat & tfm = this->matrix;
1388 if (SbMatrixP::isIdentity(tfm)) { *this = m; return *this; }
1389
1390 SbMat tmp;
1391 (void)memcpy(tmp, tfm, 4*4*sizeof(float));
1392
1393 for (int i=0; i < 4; i++) {
1394 for (int j=0; j < 4; j++) {
1395 tfm[i][j] =
1396 tmp[0][j] * mfm[i][0] +
1397 tmp[1][j] * mfm[i][1] +
1398 tmp[2][j] * mfm[i][2] +
1399 tmp[3][j] * mfm[i][3];
1400 }
1401 }
1402 return *this;
1403 }
1404
1405 /*!
1406 Multiply \a src vector with this matrix and return the result in \a dst.
1407 Multiplication is done with the vector on the right side of the
1408 expression, i.e. dst = M * src.
1409
1410 (Be aware that it is more common to use the
1411 SbMatrix::multVecMatrix() operation, when doing vector
1412 transformations, as SbMatrix instances are by definition in
1413 column-order, and uses post-multiplication for common geometry
1414 operations.)
1415
1416 \sa multVecMatrix(), multDirMatrix() and multLineMatrix().
1417 */
1418 void
multMatrixVec(const SbVec3f & src,SbVec3f & dst) const1419 SbMatrix::multMatrixVec(const SbVec3f & src, SbVec3f & dst) const
1420 {
1421 // Checks if the "this" matrix is equal to the identity matrix. See
1422 // also code comments at the start of SbMatrix::multRight().
1423 if (SbMatrixP::isIdentity(this->matrix)) { dst = src; return; }
1424
1425 const float * t0 = (*this)[0];
1426 const float * t1 = (*this)[1];
1427 const float * t2 = (*this)[2];
1428 const float * t3 = (*this)[3];
1429 // Copy the src vector, just in case src and dst is the same vector.
1430 SbVec3f s = src;
1431
1432 float W = s[0]*t3[0] + s[1]*t3[1] + s[2]*t3[2] + t3[3];
1433
1434 dst[0] = (s[0]*t0[0] + s[1]*t0[1] + s[2]*t0[2] + t0[3])/W;
1435 dst[1] = (s[0]*t1[0] + s[1]*t1[1] + s[2]*t1[2] + t1[3])/W;
1436 dst[2] = (s[0]*t2[0] + s[1]*t2[1] + s[2]*t2[2] + t2[3])/W;
1437 }
1438
1439 /*!
1440 Multiply \a src vector with this matrix and return the result in \a dst.
1441 Multiplication is done with the vector on the left side of the
1442 expression, i.e. dst = src * M.
1443
1444 It is safe to let \a src and \a dst be the same SbVec3f instance.
1445
1446 This method can be used (using the current model matrix) to
1447 transform a point from an object coordinate systems to the world
1448 coordinate system.
1449
1450 This operation is what you would usually do when transforming
1451 vectors, as SbMatrix instances are, by definition, column-order
1452 matrices.
1453
1454 \sa multMatrixVec(), multDirMatrix() and multLineMatrix().
1455 */
1456 void
multVecMatrix(const SbVec3f & src,SbVec3f & dst) const1457 SbMatrix::multVecMatrix(const SbVec3f & src, SbVec3f & dst) const
1458 {
1459 // Checks if the "this" matrix is equal to the identity matrix. See
1460 // also code comments at the start of SbMatrix::multRight().
1461 if (SbMatrixP::isIdentity(this->matrix)) { dst = src; return; }
1462
1463 const float * t0 = this->matrix[0];
1464 const float * t1 = this->matrix[1];
1465 const float * t2 = this->matrix[2];
1466 const float * t3 = this->matrix[3];
1467 // Copy the src vector, just in case src and dst is the same vector.
1468 SbVec3f s = src;
1469
1470 float W = s[0]*t0[3] + s[1]*t1[3] + s[2]*t2[3] + t3[3];
1471
1472 dst[0] = (s[0]*t0[0] + s[1]*t1[0] + s[2]*t2[0] + t3[0])/W;
1473 dst[1] = (s[0]*t0[1] + s[1]*t1[1] + s[2]*t2[1] + t3[1])/W;
1474 dst[2] = (s[0]*t0[2] + s[1]*t1[2] + s[2]*t2[2] + t3[2])/W;
1475 }
1476
1477 /*!
1478 \overload
1479 */
1480 void
multVecMatrix(const SbVec4f & src,SbVec4f & dst) const1481 SbMatrix::multVecMatrix(const SbVec4f & src, SbVec4f & dst) const
1482 {
1483 // Checks if the "this" matrix is equal to the identity matrix. See
1484 // also code comments at the start of SbMatrix::multRight().
1485 if (SbMatrixP::isIdentity(this->matrix)) { dst = src; return; }
1486
1487 const float * t0 = (*this)[0];
1488 const float * t1 = (*this)[1];
1489 const float * t2 = (*this)[2];
1490 const float * t3 = (*this)[3];
1491
1492 SbVec4f s = src;
1493
1494 dst[0] = (s[0]*t0[0] + s[1]*t1[0] + s[2]*t2[0] + s[3]*t3[0]);
1495 dst[1] = (s[0]*t0[1] + s[1]*t1[1] + s[2]*t2[1] + s[3]*t3[1]);
1496 dst[2] = (s[0]*t0[2] + s[1]*t1[2] + s[2]*t2[2] + s[3]*t3[2]);
1497 dst[3] = (s[0]*t0[3] + s[1]*t1[3] + s[2]*t2[3] + s[3]*t3[3]);
1498 }
1499
1500 /*!
1501 Multiplies \a src by the matrix. \a src is assumed to be a direction
1502 vector, and the translation components of the matrix are therefore
1503 ignored.
1504
1505 Multiplication is done with the vector on the left side of the
1506 expression, i.e. dst = src * M.
1507
1508 \sa multVecMatrix(), multMatrixVec() and multLineMatrix().
1509 */
1510 void
multDirMatrix(const SbVec3f & src,SbVec3f & dst) const1511 SbMatrix::multDirMatrix(const SbVec3f & src, SbVec3f & dst) const
1512 {
1513 // Checks if the "this" matrix is equal to the identity matrix. See
1514 // also code comments at the start of SbMatrix::multRight().
1515 if (SbMatrixP::isIdentity(this->matrix)) { dst = src; return; }
1516
1517 const float * t0 = (*this)[0];
1518 const float * t1 = (*this)[1];
1519 const float * t2 = (*this)[2];
1520 // Copy the src vector, just in case src and dst is the same vector.
1521 SbVec3f s = src;
1522
1523 dst[0] = s[0]*t0[0] + s[1]*t1[0] + s[2]*t2[0];
1524 dst[1] = s[0]*t0[1] + s[1]*t1[1] + s[2]*t2[1];
1525 dst[2] = s[0]*t0[2] + s[1]*t1[2] + s[2]*t2[2];
1526 }
1527
1528 /*!
1529 Multiplies line point with the full matrix and multiplies the
1530 line direction with the matrix without the translation components.
1531
1532 \sa multVecMatrix(), multMatrixVec() and multDirMatrix().
1533 */
1534 void
multLineMatrix(const SbLine & src,SbLine & dst) const1535 SbMatrix::multLineMatrix(const SbLine & src, SbLine & dst) const
1536 {
1537 SbVec3f newpt, newdir;
1538 this->multVecMatrix(src.getPosition(), newpt);
1539 this->multDirMatrix(src.getDirection(), newdir);
1540
1541 dst.setPosDir(newpt, newdir);
1542 }
1543
1544 /*!
1545 Write out the matrix contents to the given file.
1546 */
1547 void
print(FILE * fp) const1548 SbMatrix::print(FILE * fp) const
1549 {
1550 for (int i=0; i < 4; i++) {
1551 fprintf(fp, "%10.5g\t%10.5g\t%10.5g\t%10.5g\n",
1552 this->matrix[i][0], this->matrix[i][1],
1553 this->matrix[i][2], this->matrix[i][3]);
1554 }
1555 }
1556
1557 /***********************************************************************
1558 below is the polar_decomp implementation by Ken Shoemake
1559 <shoemake@graphics.cis.upenn.edu>. It was part of the
1560 Graphics Gems IV archive.
1561 ************************************************************************/
1562
1563 // FIXME: should merge all the PD code we're using from GGIV into
1564 // SbMatrix, SbRotation and SbVec3f proper (for two reasons: 1)
1565 // there's a lot of duplicated code here (like for instance the
1566 // matrix->quaternion decomposition, which also exists in
1567 // SbRotation::setValue(SbMatrix&)), and 2) the remaining code
1568 // snippets look generally useful outside the purpose of breaking down
1569 // a matrix into it's transformation components). 20010114 mortene.
1570
1571
1572 /**** Decompose.c ****/
1573 /* Ken Shoemake, 1993 */
1574
1575 /******* Matrix Preliminaries *******/
1576
1577 /** Fill out 3x3 matrix to 4x4 **/
1578 #define sp_mat_pad(A) (A[W][X]=A[X][W]=A[W][Y]=A[Y][W]=A[W][Z]=A[Z][W]=0, A[W][W]=1)
1579
1580 /** Copy nxn matrix A to C using "gets" for assignment **/
1581 #define sp_mat_copy(C, gets, A, n) {int i, j; for (i=0;i<n;i++) for (j=0;j<n;j++)\
1582 C[i][j] gets (A[i][j]);}
1583
1584 /** Copy transpose of nxn matrix A to C using "gets" for assignment **/
1585 #define sp_mat_tpose(AT, gets, A, n) {int i, j; for (i=0;i<n;i++) for (j=0;j<n;j++)\
1586 AT[i][j] gets (A[j][i]);}
1587
1588 /** Assign nxn matrix C the element-wise combination of A and B using "op" **/
1589 #define sp_mat_binop(C, gets, A, op, B, n) {int i, j; for (i=0;i<n;i++) for (j=0;j<n;j++)\
1590 C[i][j] gets (A[i][j]) op (B[i][j]);}
1591
1592 /** Multiply the upper left 3x3 parts of A and B to get AB **/
1593 void
mat_mult(SbMatrixP::HMatrix A,SbMatrixP::HMatrix B,SbMatrixP::HMatrix AB)1594 SbMatrixP::mat_mult(SbMatrixP::HMatrix A, SbMatrixP::HMatrix B, SbMatrixP::HMatrix AB)
1595 {
1596 int i, j;
1597 for (i=0; i<3; i++) for (j=0; j<3; j++)
1598 AB[i][j] = A[i][0]*B[0][j] + A[i][1]*B[1][j] + A[i][2]*B[2][j];
1599 }
1600
1601 /** Return dot product of length 3 vectors va and vb **/
1602 float
vdot(float * va,float * vb)1603 SbMatrixP::vdot(float * va, float * vb)
1604 {
1605 return (va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2]);
1606 }
1607
1608 /** Set v to cross product of length 3 vectors va and vb **/
1609 void
vcross(float * va,float * vb,float * v)1610 SbMatrixP::vcross(float * va, float * vb, float * v)
1611 {
1612 v[0] = va[1]*vb[2] - va[2]*vb[1];
1613 v[1] = va[2]*vb[0] - va[0]*vb[2];
1614 v[2] = va[0]*vb[1] - va[1]*vb[0];
1615 }
1616
1617 /** Set MadjT to transpose of inverse of M times determinant of M **/
1618 void
adjoint_transpose(SbMatrixP::HMatrix M,SbMatrixP::HMatrix MadjT)1619 SbMatrixP::adjoint_transpose(SbMatrixP::HMatrix M, SbMatrixP::HMatrix MadjT)
1620 {
1621 vcross(M[1], M[2], MadjT[0]);
1622 vcross(M[2], M[0], MadjT[1]);
1623 vcross(M[0], M[1], MadjT[2]);
1624 }
1625
1626 /******* Decomp Auxiliaries *******/
1627
1628 /** Compute either the 1 or infinity norm of M, depending on tpose **/
1629 float
mat_norm(SbMatrixP::HMatrix M,int tpose)1630 SbMatrixP::mat_norm(SbMatrixP::HMatrix M, int tpose)
1631 {
1632 int i;
1633 float sum, max;
1634 max = 0.0;
1635 for (i=0; i<3; i++) {
1636 if (tpose) sum = static_cast<float>(fabs(M[0][i])+fabs(M[1][i])+fabs(M[2][i]));
1637 else sum = static_cast<float>((fabs(M[i][0])+fabs(M[i][1])+fabs(M[i][2])));
1638 if (max<sum) max = sum;
1639 }
1640 return max;
1641 }
1642
1643 /** Return index of column of M containing maximum abs entry, or -1 if M=0 **/
1644 int
find_max_col(SbMatrixP::HMatrix M)1645 SbMatrixP::find_max_col(SbMatrixP::HMatrix M)
1646 {
1647 float abs, max;
1648 int i, j, col;
1649 max = 0.0; col = -1;
1650 for (i=0; i<3; i++) for (j=0; j<3; j++) {
1651 abs = M[i][j]; if (abs<0.0) abs = -abs;
1652 if (abs>max) {max = abs; col = j;}
1653 }
1654 return col;
1655 }
1656
1657 /** Setup u for Household reflection to zero all v components but first **/
1658 void
make_reflector(float * v,float * u)1659 SbMatrixP::make_reflector(float * v, float * u)
1660 {
1661 float s = static_cast<float>(sqrt(vdot(v, v)));
1662 u[0] = v[0]; u[1] = v[1];
1663 u[2] = v[2] + ((v[2]<0.0) ? -s : s);
1664 s = static_cast<float>(sqrt(2.0/vdot(u, u)));
1665 u[0] = u[0]*s; u[1] = u[1]*s; u[2] = u[2]*s;
1666 }
1667
1668 /** Apply Householder reflection represented by u to column vectors of M **/
1669 void
reflect_cols(SbMatrixP::HMatrix M,float * u)1670 SbMatrixP::reflect_cols(SbMatrixP::HMatrix M, float * u)
1671 {
1672 int i, j;
1673 for (i=0; i<3; i++) {
1674 float s = u[0]*M[0][i] + u[1]*M[1][i] + u[2]*M[2][i];
1675 for (j=0; j<3; j++) M[j][i] -= u[j]*s;
1676 }
1677 }
1678 /** Apply Householder reflection represented by u to row vectors of M **/
1679 void
reflect_rows(SbMatrixP::HMatrix M,float * u)1680 SbMatrixP::reflect_rows(SbMatrixP::HMatrix M, float * u)
1681 {
1682 int i, j;
1683 for (i=0; i<3; i++) {
1684 float s = vdot(u, M[i]);
1685 for (j=0; j<3; j++) M[i][j] -= u[j]*s;
1686 }
1687 }
1688
1689 /** Find orthogonal factor Q of rank 1 (or less) M **/
1690 void
do_rank1(SbMatrixP::HMatrix M,SbMatrixP::HMatrix Q)1691 SbMatrixP::do_rank1(SbMatrixP::HMatrix M, SbMatrixP::HMatrix Q)
1692 {
1693 float v1[3], v2[3], s;
1694 int col;
1695 sp_mat_copy(Q, =, mat_id, 4);
1696 /* If rank(M) is 1, we should find a non-zero column in M */
1697 col = find_max_col(M);
1698 if (col<0) return; /* Rank is 0 */
1699 v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col];
1700 make_reflector(v1, v1); reflect_cols(M, v1);
1701 v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2];
1702 make_reflector(v2, v2); reflect_rows(M, v2);
1703 s = M[2][2];
1704 if (s<0.0) Q[2][2] = -1.0;
1705 reflect_cols(Q, v1); reflect_rows(Q, v2);
1706 }
1707
1708 /** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/
1709 void
do_rank2(SbMatrixP::HMatrix M,SbMatrixP::HMatrix MadjT,SbMatrixP::HMatrix Q)1710 SbMatrixP::do_rank2(SbMatrixP::HMatrix M, SbMatrixP::HMatrix MadjT, SbMatrixP::HMatrix Q)
1711 {
1712 float v1[3], v2[3];
1713 float w, x, y, z, c, s, d;
1714 int col;
1715 /* If rank(M) is 2, we should find a non-zero column in MadjT */
1716 col = find_max_col(MadjT);
1717 if (col<0) {do_rank1(M, Q); return;} /* Rank<2 */
1718 v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col];
1719 make_reflector(v1, v1); reflect_cols(M, v1);
1720 vcross(M[0], M[1], v2);
1721 make_reflector(v2, v2); reflect_rows(M, v2);
1722 w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1];
1723 if (w*z>x*y) {
1724 c = z+w; s = y-x; d = static_cast<float>(sqrt(c*c+s*s)); c = c/d; s = s/d;
1725 Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s);
1726 }
1727 else {
1728 c = z-w; s = y+x; d = static_cast<float>(sqrt(c*c+s*s)); c = c/d; s = s/d;
1729 Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s;
1730 }
1731 Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0;
1732 reflect_cols(Q, v1); reflect_rows(Q, v2);
1733 }
1734
1735
1736 /******* Polar Decomposition *******/
1737
1738 /* Polar Decomposition of 3x3 matrix in 4x4,
1739 * M = QS. See Nicholas Higham and Robert S. Schreiber,
1740 * Fast Polar Decomposition of An Arbitrary Matrix,
1741 * Technical Report 88-942, October 1988,
1742 * Department of Computer Science, Cornell University.
1743 */
1744 float
polar_decomp(SbMatrixP::HMatrix M,SbMatrixP::HMatrix Q,SbMatrixP::HMatrix S)1745 SbMatrixP::polar_decomp(SbMatrixP::HMatrix M, SbMatrixP::HMatrix Q, SbMatrixP::HMatrix S)
1746 {
1747 #define TOL 1.0e-6
1748 SbMatrixP::HMatrix Mk, MadjTk, Ek;
1749 float det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2;
1750 int i, j;
1751 sp_mat_tpose(Mk, =, M, 3);
1752 M_one = norm_one(Mk); M_inf = norm_inf(Mk);
1753 do {
1754 adjoint_transpose(Mk, MadjTk);
1755 det = vdot(Mk[0], MadjTk[0]);
1756 if (det==0.0) {do_rank2(Mk, MadjTk, Mk); break;}
1757 MadjT_one = norm_one(MadjTk); MadjT_inf = norm_inf(MadjTk);
1758 gamma = static_cast<float>(sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det)));
1759 g1 = gamma*0.5f;
1760 g2 = 0.5f/(gamma*det);
1761 sp_mat_copy(Ek, =, Mk, 3);
1762 sp_mat_binop(Mk, =, g1*Mk, +, g2*MadjTk, 3);
1763 sp_mat_copy(Ek, -=, Mk, 3);
1764 E_one = norm_one(Ek);
1765 M_one = norm_one(Mk); M_inf = norm_inf(Mk);
1766 } while (E_one>(M_one*TOL));
1767 sp_mat_tpose(Q, =, Mk, 3); sp_mat_pad(Q);
1768 SbMatrixP::mat_mult(Mk, M, S); sp_mat_pad(S);
1769 for (i=0; i<3; i++) for (j=i; j<3; j++)
1770 S[i][j] = S[j][i] = 0.5f*(S[i][j]+S[j][i]);
1771 return (det);
1772
1773 #undef TOL
1774 }
1775
1776 /******* Spectral Decomposition *******/
1777
1778 /* Compute the spectral decomposition of symmetric positive semi-definite S.
1779 * Returns rotation in U and scale factors in result, so that if K is a diagonal
1780 * matrix of the scale factors, then S = U K (U transpose). Uses Jacobi method.
1781 * See Gene H. Golub and Charles F. Van Loan. Matrix Computations. Hopkins 1983.
1782 */
1783 SbVec4f
spect_decomp(SbMatrixP::HMatrix S,SbMatrixP::HMatrix U)1784 SbMatrixP::spect_decomp(SbMatrixP::HMatrix S, SbMatrixP::HMatrix U)
1785 {
1786 SbVec4f kv;
1787 double Diag[3], OffD[3]; /* OffD is off-diag (by omitted index) */
1788 double g, h, fabsh, fabsOffDi, t, theta, c, s, tau, ta, OffDq;
1789 static char nxt[] = {Y, Z, X};
1790 int sweep, i, j;
1791 sp_mat_copy(U, =, mat_id, 4);
1792 Diag[X] = S[X][X]; Diag[Y] = S[Y][Y]; Diag[Z] = S[Z][Z];
1793 OffD[X] = S[Y][Z]; OffD[Y] = S[Z][X]; OffD[Z] = S[X][Y];
1794 for (sweep=20; sweep>0; sweep--) {
1795 float sm = static_cast<float>((fabs(OffD[X])+fabs(OffD[Y])+fabs(OffD[Z])));
1796 if (sm==0.0) break;
1797 for (i=Z; i>=X; i--) {
1798 int p = nxt[i]; int q = nxt[p];
1799 fabsOffDi = fabs(OffD[i]);
1800 g = 100.0*fabsOffDi;
1801 if (fabsOffDi>0.0) {
1802 h = Diag[q] - Diag[p];
1803 fabsh = fabs(h);
1804 if (fabsh+g==fabsh) {
1805 t = OffD[i]/h;
1806 }
1807 else {
1808 theta = 0.5*h/OffD[i];
1809 t = 1.0/(fabs(theta)+sqrt(theta*theta+1.0));
1810 if (theta<0.0) t = -t;
1811 }
1812 c = 1.0/sqrt(t*t+1.0); s = t*c;
1813 tau = s/(c+1.0);
1814 ta = t*OffD[i]; OffD[i] = 0.0;
1815 Diag[p] -= ta; Diag[q] += ta;
1816 OffDq = OffD[q];
1817 OffD[q] -= s*(OffD[p] + tau*OffD[q]);
1818 OffD[p] += s*(OffDq - tau*OffD[p]);
1819 for (j=Z; j>=X; j--) {
1820 // FIXME: this is a very peculiar problem.. if the code in
1821 // the #else part is activated, Valgrind 1.9.3 reports "Use
1822 // of uninitialised value of size 8" in this code, when
1823 // built with g++ 2.95.4. The code under the #if runs
1824 // without any warnings, even though the only change is some
1825 // *seemingly* irrelevant casting.
1826 //
1827 // (And even more strange: by inserting any call to external
1828 // code (for instance a line of ''(void)atoi("0");'') below,
1829 // the Valgrind error vanishes. Could it be some sort of
1830 // optimalization bug related to stack overwriting or some
1831 // such?)
1832 //
1833 // We also have a report about a crash assumed to be in this
1834 // code with Coin built with MSVC6.0 with /O2 optimalization
1835 // (very suspect that there's a problem with both g++ and
1836 // MSVC++), for the following stand-alone example:
1837 //
1838 // ----8<----- [snip] ---------8<----- [snip] -----
1839 // #include <Inventor/SoDB.h>
1840 // #include <Inventor/SbLinear.h>
1841 //
1842 // int
1843 // main( int argc, char** argv )
1844 // {
1845 // SoDB::init();
1846 //
1847 // SbVec3f translation, scale, axis;
1848 // SbRotation rot, scaleRot;
1849 // SbMatrix matrix (5.96046e-008f, 1.00000f, -2.98023e-008f, 0.000000f,
1850 // -2.98023e-008f, 5.96046e-008f, 1.00000f, 0.000000f,
1851 // 1.00000f, -2.98023e-008f, 5.96046e-008f, 0.000000f,
1852 // -162.929f, -56.2217f, 197.110f, 1.00000f
1853 // );
1854 // matrix.getTransform ( translation, rot, scale, scaleRot );
1855 // translation.print(stdout); printf(" <- translation\n");
1856 // scale.print(stdout); printf(" <- scale\n");
1857 // rot.print(stdout); printf(" <- rot\n");
1858 // scaleRot.print(stdout); printf(" <- scaleRot\n");
1859 //
1860 // return 0;
1861 // }
1862 // ----8<----- [snip] ---------8<----- [snip] -----
1863 //
1864 // I was not able to reproduce the crash, so I got stuck in
1865 // debugging this.
1866 //
1867 // Mailed a patch with the casting changes back to the
1868 // original reporter of the problem (Tore K at HitecO), and
1869 // he reports that the crash bug is then also gone! That's
1870 // the damndest thing.. really weird.
1871 //
1872 // It does seem extremely likely to be a compiler problem,
1873 // although it's very fishy that it mucks something up for
1874 // both GNU GCC (with -O2) and MS's VisualC++ (with /O2).
1875 //
1876 // I'll buy a beer for anyone who can figure out this one
1877 // and explain it to me.
1878 //
1879 // 20030214 mortene.
1880 float a = U[j][p], b = U[j][q];
1881 #if 1
1882 U[j][p] -= float(s)*(b + float(tau)*a);
1883 U[j][q] += float(s)*(a - float(tau)*b);
1884 #else
1885 U[j][p] -= (float)(s*(b + tau*a));
1886 U[j][q] += (float)(s*(a - tau*b));
1887 #endif
1888 }
1889 }
1890 }
1891 }
1892 kv[X] = static_cast<float>(Diag[X]);
1893 kv[Y] = static_cast<float>(Diag[Y]);
1894 kv[Z] = static_cast<float>(Diag[Z]);
1895 kv[W] = 1.0f;
1896 return (kv);
1897 }
1898
1899 /* Helper function for the snuggle() function below. */
1900 static inline void
cycle(float * a,SbBool flip)1901 cycle(float * a, SbBool flip)
1902 {
1903 if (flip) {
1904 a[3]=a[0]; a[0]=a[1]; a[1]=a[2]; a[2]=a[3];
1905 }
1906 else {
1907 a[3]=a[2]; a[2]=a[1]; a[1]=a[0]; a[0]=a[3];
1908 }
1909 }
1910
1911 /******* Spectral Axis Adjustment *******/
1912
1913 /* Given a unit quaternion, q, and a scale vector, k, find a unit quaternion, p,
1914 * which permutes the axes and turns freely in the plane of duplicate scale
1915 * factors, such that q p has the largest possible w component, i.e. the
1916 * smallest possible angle. Permutes k's components to go with q p instead of q.
1917 * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
1918 * Proceedings of Graphics Interface 1992. Details on p. 262-263.
1919 */
1920 SbRotation
snuggle(SbRotation q,SbVec4f & k)1921 SbMatrixP::snuggle(SbRotation q, SbVec4f & k)
1922 {
1923 #define SQRTHALF (0.7071067811865475244f)
1924 #define sgn(n, v) ((n)?-(v):(v))
1925 #define swap(a, i, j) {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];}
1926
1927 SbRotation p;
1928 float ka[4];
1929 int i, turn = -1;
1930 ka[X] = k[X]; ka[Y] = k[Y]; ka[Z] = k[Z];
1931 if (ka[X]==ka[Y]) {if (ka[X]==ka[Z]) turn = W; else turn = Z;}
1932 else {if (ka[X]==ka[Z]) turn = Y; else if (ka[Y]==ka[Z]) turn = X;}
1933 if (turn>=0) {
1934 SbRotation qtoz, qp;
1935 unsigned neg[3], win;
1936 double mag[3], t;
1937 static SbRotation qxtoz(0.0f, SQRTHALF, 0.0f, SQRTHALF);
1938 static SbRotation qytoz(SQRTHALF, 0.0f, 0.0f, SQRTHALF);
1939 static SbRotation qppmm(0.5f, 0.5f, -0.5f, -0.5f);
1940 static SbRotation qpppp(0.5f, 0.5f, 0.5f, 0.5f);
1941 static SbRotation qmpmm(-0.5f, 0.5f, -0.5f, -0.5f);
1942 static SbRotation qpppm(0.5f, 0.5f, 0.5f, -0.5f);
1943 static SbRotation q0001(0.0f, 0.0f, 0.0f, 1.0f);
1944 static SbRotation q1000(1.0f, 0.0f, 0.0f, 0.0f);
1945 switch (turn) {
1946 default: return SbRotation(q).invert();
1947 case X: q = (qtoz = qxtoz) * q; swap(ka, X, Z) break;
1948 case Y: q = (qtoz = qytoz) * q; swap(ka, Y, Z) break;
1949 case Z: qtoz = q0001; break;
1950 }
1951 q.invert();
1952 mag[0] = static_cast<double>(q.getValue()[Z]*q.getValue()[Z])+static_cast<double>(q.getValue()[W]*q.getValue()[W])-0.5;
1953 mag[1] = static_cast<double>(q.getValue()[X]*q.getValue()[Z])-static_cast<double>(q.getValue()[Y]*q.getValue()[W]);
1954 mag[2] = static_cast<double>(q.getValue()[Y]*q.getValue()[Z])+static_cast<double>(q.getValue()[X]*q.getValue()[W]);
1955 for (i=0; i<3; i++) if ((neg[i] = (mag[i] < 0.0))) mag[i] = -mag[i];
1956 if (mag[0]>mag[1]) {if (mag[0]>mag[2]) win = 0; else win = 2;}
1957 else {if (mag[1]>mag[2]) win = 1; else win = 2;}
1958 switch (win) {
1959 case 0: if (neg[0]) p = q1000; else p = q0001; break;
1960 case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka, FALSE); break;
1961 case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka, TRUE); break;
1962 }
1963 qp = p * q;
1964 t = sqrt(mag[win]+0.5);
1965 p = SbRotation(0.0, 0.0, -qp.getValue()[Z]/static_cast<float>(t), qp.getValue()[W]/static_cast<float>(t)) * p;
1966 p = SbRotation(p).invert() * qtoz;
1967 }
1968 else {
1969 float qa[4], pa[4];
1970 unsigned lo, hi, neg[4], par = 0;
1971 double all, big, two;
1972 qa[0] = q.getValue()[X]; qa[1] = q.getValue()[Y]; qa[2] = q.getValue()[Z]; qa[3] = q.getValue()[W];
1973 for (i=0; i<4; i++) {
1974 pa[i] = 0.0;
1975 if ((neg[i] = (qa[i]<0.0))) qa[i] = -qa[i];
1976 par ^= neg[i];
1977 }
1978 /* Find two largest components, indices in hi and lo */
1979 if (qa[0]>qa[1]) lo = 0; else lo = 1;
1980 if (qa[2]>qa[3]) hi = 2; else hi = 3;
1981 if (qa[lo]>qa[hi]) {
1982 if (qa[lo^1]>qa[hi]) {hi = lo; lo ^= 1;}
1983 else {hi ^= lo; lo ^= hi; hi ^= lo;}
1984 } else {if (qa[hi^1]>qa[lo]) lo = hi^1;}
1985 all = (qa[0]+qa[1]+qa[2]+qa[3])*0.5;
1986 two = (qa[hi]+qa[lo])*SQRTHALF;
1987 big = qa[hi];
1988 if (all>two) {
1989 if (all>big) {/*all*/
1990 {int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5f);}
1991 cycle(ka, par);
1992 }
1993 else {/*big*/ pa[hi] = sgn(neg[hi], 1.0f);}
1994 }
1995 else {
1996 if (two>big) {/*two*/
1997 pa[hi] = sgn(neg[hi], SQRTHALF); pa[lo] = sgn(neg[lo], SQRTHALF);
1998 if (lo>hi) {hi ^= lo; lo ^= hi; hi ^= lo;}
1999 if (hi==W) {hi = "\001\002\000"[lo]; lo = 3-hi-lo;}
2000 swap(ka, hi, lo)
2001 } else {/*big*/ pa[hi] = sgn(neg[hi], 1.0f);}
2002 }
2003 // FIXME: p = conjugate(pa)? 20010114 mortene.
2004 p.setValue(-pa[0], -pa[1], -pa[2], pa[3]);
2005 }
2006 k[X] = ka[X]; k[Y] = ka[Y]; k[Z] = ka[Z];
2007 return (p);
2008
2009 #undef SQRTHALF
2010 #undef sgn
2011 #undef swap
2012 }
2013
2014 /******* Decompose Affine Matrix *******/
2015
2016 /* Decompose 4x4 affine matrix A as TFRUK(U transpose), where t contains the
2017 * translation components, q contains the rotation R, u contains U, k contains
2018 * scale factors, and f contains the sign of the determinant.
2019 * Assumes A transforms column vectors in right-handed coordinates.
2020 * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
2021 * Proceedings of Graphics Interface 1992.
2022 */
2023 void
decomp_affine(SbMatrixP::HMatrix A,SbMatrixP::AffineParts * parts)2024 SbMatrixP::decomp_affine(SbMatrixP::HMatrix A, SbMatrixP::AffineParts * parts)
2025 {
2026 SbMatrixP::HMatrix Q, S, U;
2027 SbRotation p;
2028 parts->t = SbVec4f(A[X][W], A[Y][W], A[Z][W], 0);
2029 float det = polar_decomp(A, Q, S);
2030 if (det<0.0) {
2031 sp_mat_copy(Q, =, -Q, 3);
2032 parts->f = -1;
2033 }
2034 else parts->f = 1;
2035
2036 // Transpose for our code (we use OpenGL's convention for numbering
2037 // rows and columns).
2038 SbMatrix TQ(reinterpret_cast<const SbMat *>(&Q));
2039 parts->q = SbRotation(TQ.transpose());
2040 parts->k = spect_decomp(S, U);
2041 // Transpose for our code (we use OpenGL's convention for numbering
2042 // rows and columns).
2043 SbMatrix TU(reinterpret_cast<const SbMat *>(&U));
2044 parts->u = SbRotation(TU.transpose());
2045 p = snuggle(parts->u, parts->k);
2046 parts->u = p * parts->u;
2047 }
2048
2049 #undef sp_mat_pad
2050 #undef sp_mat_copy
2051 #undef sp_mat_tpose
2052 #undef sp_mat_binop
2053
2054 #ifdef COIN_TEST_SUITE
2055 #include <Inventor/SbDPMatrix.h>
2056
BOOST_AUTO_TEST_CASE(constructFromSbDPMatrix)2057 BOOST_AUTO_TEST_CASE(constructFromSbDPMatrix) {
2058 SbMatrixd a(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15);
2059 SbMatrix b;
2060 float c[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
2061 b.setValue(c);
2062 SbMatrix d = SbMatrix(a);
2063 BOOST_CHECK_MESSAGE(b == d,
2064 "Equality comparrison failed!");
2065 }
2066 #endif //COIN_TEST_SUITE
2067