root/OpenSceneGraph/trunk/src/osg/Matrix_implementation.cpp @ 7301

Revision 7301, 26.9 kB (checked in by robert, 6 years ago)

Cleaned up the getRotate implementation selection using #defines, and made the
COMPILE_getRotate_David_Spillings_Mk1 the default.

  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
Line 
1/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
2 *
3 * This library is open source and may be redistributed and/or modified under 
4 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
5 * (at your option) any later version.  The full license is in LICENSE file
6 * included with this distribution, and on the openscenegraph.org website.
7 *
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11 * OpenSceneGraph Public License for more details.
12*/
13
14#include <osg/Quat>
15#include <osg/Notify>
16#include <osg/Math>
17#include <osg/Timer>
18
19#include <osg/GL>
20
21#include <stdlib.h>
22
23using namespace osg;
24
25#define SET_ROW(row, v1, v2, v3, v4 )    \
26    _mat[(row)][0] = (v1); \
27    _mat[(row)][1] = (v2); \
28    _mat[(row)][2] = (v3); \
29    _mat[(row)][3] = (v4);
30
31#define INNER_PRODUCT(a,b,r,c) \
32     ((a)._mat[r][0] * (b)._mat[0][c]) \
33    +((a)._mat[r][1] * (b)._mat[1][c]) \
34    +((a)._mat[r][2] * (b)._mat[2][c]) \
35    +((a)._mat[r][3] * (b)._mat[3][c])
36
37
38Matrix_implementation::Matrix_implementation( value_type a00, value_type a01, value_type a02, value_type a03,
39                  value_type a10, value_type a11, value_type a12, value_type a13,
40                  value_type a20, value_type a21, value_type a22, value_type a23,
41                  value_type a30, value_type a31, value_type a32, value_type a33)
42{
43    SET_ROW(0, a00, a01, a02, a03 )
44    SET_ROW(1, a10, a11, a12, a13 )
45    SET_ROW(2, a20, a21, a22, a23 )
46    SET_ROW(3, a30, a31, a32, a33 )
47}
48
49void Matrix_implementation::set( value_type a00, value_type a01, value_type a02, value_type a03,
50                   value_type a10, value_type a11, value_type a12, value_type a13,
51                   value_type a20, value_type a21, value_type a22, value_type a23,
52                   value_type a30, value_type a31, value_type a32, value_type a33)
53{
54    SET_ROW(0, a00, a01, a02, a03 )
55    SET_ROW(1, a10, a11, a12, a13 )
56    SET_ROW(2, a20, a21, a22, a23 )
57    SET_ROW(3, a30, a31, a32, a33 )
58}
59
60#define QX  q._v[0]
61#define QY  q._v[1]
62#define QZ  q._v[2]
63#define QW  q._v[3]
64
65void Matrix_implementation::setRotate(const Quat& q_in)
66{
67    Quat q(q_in);
68    double length2 = q.length2();
69    if (length2!=1.0 && length2!=0)
70    {
71        // normalize quat if required.
72        q /= sqrt(length2);
73    }
74
75    // Source: Gamasutra, Rotating Objects Using Quaternions
76    //
77    //http://www.gamasutra.com/features/19980703/quaternions_01.htm
78
79    double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
80
81    // calculate coefficients
82    x2 = QX + QX;
83    y2 = QY + QY;
84    z2 = QZ + QZ;
85
86    xx = QX * x2;
87    xy = QX * y2;
88    xz = QX * z2;
89
90    yy = QY * y2;
91    yz = QY * z2;
92    zz = QZ * z2;
93
94    wx = QW * x2;
95    wy = QW * y2;
96    wz = QW * z2;
97
98    // Note.  Gamasutra gets the matrix assignments inverted, resulting
99    // in left-handed rotations, which is contrary to OpenGL and OSG's
100    // methodology.  The matrix assignment has been altered in the next
101    // few lines of code to do the right thing.
102    // Don Burns - Oct 13, 2001
103    _mat[0][0] = 1.0 - (yy + zz);
104    _mat[1][0] = xy - wz;
105    _mat[2][0] = xz + wy;
106
107
108    _mat[0][1] = xy + wz;
109    _mat[1][1] = 1.0 - (xx + zz);
110    _mat[2][1] = yz - wx;
111
112    _mat[0][2] = xz - wy;
113    _mat[1][2] = yz + wx;
114    _mat[2][2] = 1.0 - (xx + yy);
115
116#if 0
117    _mat[0][3] = 0.0;
118    _mat[1][3] = 0.0;
119    _mat[2][3] = 0.0;
120
121    _mat[3][0] = 0.0;
122    _mat[3][1] = 0.0;
123    _mat[3][2] = 0.0;
124    _mat[3][3] = 1.0;
125#endif
126}
127
128#define COMPILE_getRotate_David_Spillings_Mk1
129//#define COMPILE_getRotate_David_Spillings_Mk2
130//#define COMPILE_getRotate_Original
131
132#ifdef COMPILE_getRotate_David_Spillings_Mk1
133// David Spillings implementation Mk 1
134Quat Matrix_implementation::getRotate() const
135{
136    Quat q;
137
138    value_type s;
139    value_type tq[4];
140    int    i, j;
141
142    // Use tq to store the largest trace
143    tq[0] = 1 + _mat[0][0]+_mat[1][1]+_mat[2][2];
144    tq[1] = 1 + _mat[0][0]-_mat[1][1]-_mat[2][2];
145    tq[2] = 1 - _mat[0][0]+_mat[1][1]-_mat[2][2];
146    tq[3] = 1 - _mat[0][0]-_mat[1][1]+_mat[2][2];
147
148    // Find the maximum (could also use stacked if's later)
149    j = 0;
150    for(i=1;i<4;i++) j = (tq[i]>tq[j])? i : j;
151
152    // check the diagonal
153    if (j==0)
154    {
155        /* perform instant calculation */
156        QW = tq[0];
157        QX = _mat[1][2]-_mat[2][1];
158        QY = _mat[2][0]-_mat[0][2];
159        QZ = _mat[0][1]-_mat[1][0];
160    }
161    else if (j==1)
162    {
163        QW = _mat[1][2]-_mat[2][1];
164        QX = tq[1];
165        QY = _mat[0][1]+_mat[1][0];
166        QZ = _mat[2][0]+_mat[0][2];
167    }
168    else if (j==2)
169    {
170        QW = _mat[2][0]-_mat[0][2];
171        QX = _mat[0][1]+_mat[1][0];
172        QY = tq[2];
173        QZ = _mat[1][2]+_mat[2][1];
174    }
175    else /* if (j==3) */
176    {
177        QW = _mat[0][1]-_mat[1][0];
178        QX = _mat[2][0]+_mat[0][2];
179        QY = _mat[1][2]+_mat[2][1];
180        QZ = tq[3];
181    }
182
183    s = sqrt(0.25/tq[j]);
184    QW *= s;
185    QX *= s;
186    QY *= s;
187    QZ *= s;
188
189    return q;
190
191}
192#endif
193
194
195#ifdef COMPILE_getRotate_David_Spillings_Mk2
196// David Spillings implementation Mk 2
197Quat Matrix_implementation::getRotate() const
198{
199    Quat q;
200
201    // From http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
202    QW = 0.5 * sqrt( osg::maximum( 0.0, 1.0 + _mat[0][0] + _mat[1][1] + _mat[2][2] ) );
203    QX = 0.5 * sqrt( osg::maximum( 0.0, 1.0 + _mat[0][0] - _mat[1][1] - _mat[2][2] ) );
204    QY = 0.5 * sqrt( osg::maximum( 0.0, 1.0 - _mat[0][0] + _mat[1][1] - _mat[2][2] ) );
205    QZ = 0.5 * sqrt( osg::maximum( 0.0, 1.0 - _mat[0][0] - _mat[1][1] + _mat[2][2] ) );
206
207#if 0
208    // Robert Osfield, June 7th 2007, arggg this new implementation produces many many errors, so have to revert to sign(..) orignal below.
209    QX = QX * osg::signOrZero(  _mat[1][2] - _mat[2][1]) ;
210    QY = QY * osg::signOrZero(  _mat[2][0] - _mat[0][2]) ;
211    QZ = QZ * osg::signOrZero(  _mat[0][1] - _mat[1][0]) ;
212#else
213    QX = QX * osg::sign(  _mat[1][2] - _mat[2][1]) ;
214    QY = QY * osg::sign(  _mat[2][0] - _mat[0][2]) ;
215    QZ = QZ * osg::sign(  _mat[0][1] - _mat[1][0]) ;
216#endif
217
218    return q;
219}
220#endif
221
222#ifdef COMPILE_getRotate_Original
223// Original implementation
224Quat Matrix_implementation::getRotate() const
225{
226    Quat q;
227
228    // Source: Gamasutra, Rotating Objects Using Quaternions
229    //
230    //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
231
232    value_type  tr, s;
233    value_type tq[4];
234    int    i, j, k;
235
236    int nxt[3] = {1, 2, 0};
237
238    tr = _mat[0][0] + _mat[1][1] + _mat[2][2]+1.0;
239
240    // check the diagonal
241    if (tr > 0.0)
242    {
243        s = (value_type)sqrt (tr);
244        QW = s / 2.0;
245        s = 0.5 / s;
246        QX = (_mat[1][2] - _mat[2][1]) * s;
247        QY = (_mat[2][0] - _mat[0][2]) * s;
248        QZ = (_mat[0][1] - _mat[1][0]) * s;
249    }
250    else
251    {
252        // diagonal is negative
253        i = 0;
254        if (_mat[1][1] > _mat[0][0])
255            i = 1;
256        if (_mat[2][2] > _mat[i][i])
257            i = 2;
258        j = nxt[i];
259        k = nxt[j];
260
261        s = (value_type)sqrt ((_mat[i][i] - (_mat[j][j] + _mat[k][k])) + 1.0);
262
263        tq[i] = s * 0.5;
264
265        if (s != 0.0)
266            s = 0.5 / s;
267
268        tq[3] = (_mat[j][k] - _mat[k][j]) * s;
269        tq[j] = (_mat[i][j] + _mat[j][i]) * s;
270        tq[k] = (_mat[i][k] + _mat[k][i]) * s;
271
272        QX = tq[0];
273        QY = tq[1];
274        QZ = tq[2];
275        QW = tq[3];
276    }
277
278    return q;
279}
280#endif
281
282int Matrix_implementation::compare(const Matrix_implementation& m) const
283{
284    const Matrix_implementation::value_type* lhs = reinterpret_cast<const Matrix_implementation::value_type*>(_mat);
285    const Matrix_implementation::value_type* end_lhs = lhs+16;
286    const Matrix_implementation::value_type* rhs = reinterpret_cast<const Matrix_implementation::value_type*>(m._mat);
287    for(;lhs!=end_lhs;++lhs,++rhs)
288    {
289        if (*lhs < *rhs) return -1;
290        if (*rhs < *lhs) return 1;
291    }
292    return 0;
293}
294
295void Matrix_implementation::setTrans( value_type tx, value_type ty, value_type tz )
296{
297    _mat[3][0] = tx;
298    _mat[3][1] = ty;
299    _mat[3][2] = tz;
300}
301
302
303void Matrix_implementation::setTrans( const Vec3f& v )
304{
305    _mat[3][0] = v[0];
306    _mat[3][1] = v[1];
307    _mat[3][2] = v[2];
308}
309void Matrix_implementation::setTrans( const Vec3d& v )
310{
311    _mat[3][0] = v[0];
312    _mat[3][1] = v[1];
313    _mat[3][2] = v[2];
314}
315
316void Matrix_implementation::makeIdentity()
317{
318    SET_ROW(0,    1, 0, 0, 0 )
319    SET_ROW(1,    0, 1, 0, 0 )
320    SET_ROW(2,    0, 0, 1, 0 )
321    SET_ROW(3,    0, 0, 0, 1 )
322}
323
324void Matrix_implementation::makeScale( const Vec3f& v )
325{
326    makeScale(v[0], v[1], v[2] );
327}
328
329void Matrix_implementation::makeScale( const Vec3d& v )
330{
331    makeScale(v[0], v[1], v[2] );
332}
333
334void Matrix_implementation::makeScale( value_type x, value_type y, value_type z )
335{
336    SET_ROW(0,    x, 0, 0, 0 )
337    SET_ROW(1,    0, y, 0, 0 )
338    SET_ROW(2,    0, 0, z, 0 )
339    SET_ROW(3,    0, 0, 0, 1 )
340}
341
342void Matrix_implementation::makeTranslate( const Vec3f& v )
343{
344    makeTranslate( v[0], v[1], v[2] );
345}
346
347void Matrix_implementation::makeTranslate( const Vec3d& v )
348{
349    makeTranslate( v[0], v[1], v[2] );
350}
351
352void Matrix_implementation::makeTranslate( value_type x, value_type y, value_type z )
353{
354    SET_ROW(0,    1, 0, 0, 0 )
355    SET_ROW(1,    0, 1, 0, 0 )
356    SET_ROW(2,    0, 0, 1, 0 )
357    SET_ROW(3,    x, y, z, 1 )
358}
359
360void Matrix_implementation::makeRotate( const Vec3f& from, const Vec3f& to )
361{
362    makeIdentity();
363
364    Quat quat;
365    quat.makeRotate(from,to);
366    setRotate(quat);
367}
368void Matrix_implementation::makeRotate( const Vec3d& from, const Vec3d& to )
369{
370    makeIdentity();
371
372    Quat quat;
373    quat.makeRotate(from,to);
374    setRotate(quat);
375}
376
377void Matrix_implementation::makeRotate( value_type angle, const Vec3f& axis )
378{
379    makeIdentity();
380
381    Quat quat;
382    quat.makeRotate( angle, axis);
383    setRotate(quat);
384}
385void Matrix_implementation::makeRotate( value_type angle, const Vec3d& axis )
386{
387    makeIdentity();
388
389    Quat quat;
390    quat.makeRotate( angle, axis);
391    setRotate(quat);
392}
393
394void Matrix_implementation::makeRotate( value_type angle, value_type x, value_type y, value_type z )
395{
396    makeIdentity();
397
398    Quat quat;
399    quat.makeRotate( angle, x, y, z);
400    setRotate(quat);
401}
402
403void Matrix_implementation::makeRotate( const Quat& quat )
404{
405    makeIdentity();
406
407    setRotate(quat);
408}
409
410void Matrix_implementation::makeRotate( value_type angle1, const Vec3f& axis1,
411                         value_type angle2, const Vec3f& axis2,
412                         value_type angle3, const Vec3f& axis3)
413{
414    makeIdentity();
415
416    Quat quat;
417    quat.makeRotate(angle1, axis1,
418                    angle2, axis2,
419                    angle3, axis3);
420    setRotate(quat);
421}
422
423void Matrix_implementation::makeRotate( value_type angle1, const Vec3d& axis1,
424                         value_type angle2, const Vec3d& axis2,
425                         value_type angle3, const Vec3d& axis3)
426{
427    makeIdentity();
428
429    Quat quat;
430    quat.makeRotate(angle1, axis1,
431                    angle2, axis2,
432                    angle3, axis3);
433    setRotate(quat);
434}
435
436void Matrix_implementation::mult( const Matrix_implementation& lhs, const Matrix_implementation& rhs )
437{   
438    if (&lhs==this)
439    {
440        postMult(rhs);
441        return;
442    }
443    if (&rhs==this)
444    {
445        preMult(lhs);
446        return;
447    }
448
449// PRECONDITION: We assume neither &lhs nor &rhs == this
450// if it did, use preMult or postMult instead
451    _mat[0][0] = INNER_PRODUCT(lhs, rhs, 0, 0);
452    _mat[0][1] = INNER_PRODUCT(lhs, rhs, 0, 1);
453    _mat[0][2] = INNER_PRODUCT(lhs, rhs, 0, 2);
454    _mat[0][3] = INNER_PRODUCT(lhs, rhs, 0, 3);
455    _mat[1][0] = INNER_PRODUCT(lhs, rhs, 1, 0);
456    _mat[1][1] = INNER_PRODUCT(lhs, rhs, 1, 1);
457    _mat[1][2] = INNER_PRODUCT(lhs, rhs, 1, 2);
458    _mat[1][3] = INNER_PRODUCT(lhs, rhs, 1, 3);
459    _mat[2][0] = INNER_PRODUCT(lhs, rhs, 2, 0);
460    _mat[2][1] = INNER_PRODUCT(lhs, rhs, 2, 1);
461    _mat[2][2] = INNER_PRODUCT(lhs, rhs, 2, 2);
462    _mat[2][3] = INNER_PRODUCT(lhs, rhs, 2, 3);
463    _mat[3][0] = INNER_PRODUCT(lhs, rhs, 3, 0);
464    _mat[3][1] = INNER_PRODUCT(lhs, rhs, 3, 1);
465    _mat[3][2] = INNER_PRODUCT(lhs, rhs, 3, 2);
466    _mat[3][3] = INNER_PRODUCT(lhs, rhs, 3, 3);
467}
468
469void Matrix_implementation::preMult( const Matrix_implementation& other )
470{
471    // brute force method requiring a copy
472    //Matrix_implementation tmp(other* *this);
473    // *this = tmp;
474
475    // more efficient method just use a value_type[4] for temporary storage.
476    value_type t[4];
477    for(int col=0; col<4; ++col) {
478        t[0] = INNER_PRODUCT( other, *this, 0, col );
479        t[1] = INNER_PRODUCT( other, *this, 1, col );
480        t[2] = INNER_PRODUCT( other, *this, 2, col );
481        t[3] = INNER_PRODUCT( other, *this, 3, col );
482        _mat[0][col] = t[0];
483        _mat[1][col] = t[1];
484        _mat[2][col] = t[2];
485        _mat[3][col] = t[3];
486    }
487
488}
489
490void Matrix_implementation::postMult( const Matrix_implementation& other )
491{
492    // brute force method requiring a copy
493    //Matrix_implementation tmp(*this * other);
494    // *this = tmp;
495
496    // more efficient method just use a value_type[4] for temporary storage.
497    value_type t[4];
498    for(int row=0; row<4; ++row)
499    {
500        t[0] = INNER_PRODUCT( *this, other, row, 0 );
501        t[1] = INNER_PRODUCT( *this, other, row, 1 );
502        t[2] = INNER_PRODUCT( *this, other, row, 2 );
503        t[3] = INNER_PRODUCT( *this, other, row, 3 );
504        SET_ROW(row, t[0], t[1], t[2], t[3] )
505    }
506}
507
508#undef INNER_PRODUCT
509
510// orthoNormalize the 3x3 rotation matrix
511void Matrix_implementation::orthoNormalize(const Matrix_implementation& rhs)
512{
513    value_type x_colMag = (rhs._mat[0][0] * rhs._mat[0][0]) + (rhs._mat[1][0] * rhs._mat[1][0]) + (rhs._mat[2][0] * rhs._mat[2][0]);
514    value_type y_colMag = (rhs._mat[0][1] * rhs._mat[0][1]) + (rhs._mat[1][1] * rhs._mat[1][1]) + (rhs._mat[2][1] * rhs._mat[2][1]);
515    value_type z_colMag = (rhs._mat[0][2] * rhs._mat[0][2]) + (rhs._mat[1][2] * rhs._mat[1][2]) + (rhs._mat[2][2] * rhs._mat[2][2]);
516   
517    if(!equivalent((double)x_colMag, 1.0) && !equivalent((double)x_colMag, 0.0))
518    {
519      x_colMag = sqrt(x_colMag);
520      _mat[0][0] = rhs._mat[0][0] / x_colMag;
521      _mat[1][0] = rhs._mat[1][0] / x_colMag;
522      _mat[2][0] = rhs._mat[2][0] / x_colMag;
523    }
524    else
525    {
526      _mat[0][0] = rhs._mat[0][0];
527      _mat[1][0] = rhs._mat[1][0];
528      _mat[2][0] = rhs._mat[2][0];
529    }
530
531    if(!equivalent((double)y_colMag, 1.0) && !equivalent((double)y_colMag, 0.0))
532    {
533      y_colMag = sqrt(y_colMag);
534      _mat[0][1] = rhs._mat[0][1] / y_colMag;
535      _mat[1][1] = rhs._mat[1][1] / y_colMag;
536      _mat[2][1] = rhs._mat[2][1] / y_colMag;
537    }
538    else
539    {
540      _mat[0][1] = rhs._mat[0][1];
541      _mat[1][1] = rhs._mat[1][1];
542      _mat[2][1] = rhs._mat[2][1];
543    }
544
545    if(!equivalent((double)z_colMag, 1.0) && !equivalent((double)z_colMag, 0.0))
546    {
547      z_colMag = sqrt(z_colMag);
548      _mat[0][2] = rhs._mat[0][2] / z_colMag;
549      _mat[1][2] = rhs._mat[1][2] / z_colMag;
550      _mat[2][2] = rhs._mat[2][2] / z_colMag;
551    }
552    else
553    {
554      _mat[0][2] = rhs._mat[0][2];
555      _mat[1][2] = rhs._mat[1][2];
556      _mat[2][2] = rhs._mat[2][2];
557    }
558
559    _mat[3][0] = rhs._mat[3][0];
560    _mat[3][1] = rhs._mat[3][1];
561    _mat[3][2] = rhs._mat[3][2];
562
563    _mat[0][3] = rhs._mat[0][3];
564    _mat[1][3] = rhs._mat[1][3];
565    _mat[2][3] = rhs._mat[2][3];
566    _mat[3][3] = rhs._mat[3][3];
567
568}
569
570/******************************************
571  Matrix inversion technique:
572Given a matrix mat, we want to invert it.
573mat = [ r00 r01 r02 a
574        r10 r11 r12 b
575        r20 r21 r22 c
576        tx  ty  tz  d ]
577We note that this matrix can be split into three matrices.
578mat = rot * trans * corr, where rot is rotation part, trans is translation part, and corr is the correction due to perspective (if any).
579rot = [ r00 r01 r02 0
580        r10 r11 r12 0
581        r20 r21 r22 0
582        0   0   0   1 ]
583trans = [ 1  0  0  0
584          0  1  0  0
585          0  0  1  0
586          tx ty tz 1 ]
587corr = [ 1 0 0 px
588         0 1 0 py
589         0 0 1 pz
590         0 0 0 s ]
591where the elements of corr are obtained from linear combinations of the elements of rot, trans, and mat.
592So the inverse is mat' = (trans * corr)' * rot', where rot' must be computed the traditional way, which is easy since it is only a 3x3 matrix.
593This problem is simplified if [px py pz s] = [0 0 0 1], which will happen if mat was composed only of rotations, scales, and translations (which is common).  In this case, we can ignore corr entirely which saves on a lot of computations.
594******************************************/
595
596bool Matrix_implementation::invert_4x3( const Matrix_implementation& mat )
597{
598    if (&mat==this)
599    {
600       Matrix_implementation tm(mat);
601       return invert_4x3(tm);
602    }
603
604    register value_type r00, r01, r02,
605                        r10, r11, r12,
606                        r20, r21, r22;
607      // Copy rotation components directly into registers for speed
608    r00 = mat._mat[0][0]; r01 = mat._mat[0][1]; r02 = mat._mat[0][2];
609    r10 = mat._mat[1][0]; r11 = mat._mat[1][1]; r12 = mat._mat[1][2];
610    r20 = mat._mat[2][0]; r21 = mat._mat[2][1]; r22 = mat._mat[2][2];
611
612        // Partially compute inverse of rot
613    _mat[0][0] = r11*r22 - r12*r21;
614    _mat[0][1] = r02*r21 - r01*r22;
615    _mat[0][2] = r01*r12 - r02*r11;
616
617      // Compute determinant of rot from 3 elements just computed
618    register value_type one_over_det = 1.0/(r00*_mat[0][0] + r10*_mat[0][1] + r20*_mat[0][2]);
619    r00 *= one_over_det; r10 *= one_over_det; r20 *= one_over_det;  // Saves on later computations
620
621      // Finish computing inverse of rot
622    _mat[0][0] *= one_over_det;
623    _mat[0][1] *= one_over_det;
624    _mat[0][2] *= one_over_det;
625    _mat[0][3] = 0.0;
626    _mat[1][0] = r12*r20 - r10*r22; // Have already been divided by det
627    _mat[1][1] = r00*r22 - r02*r20; // same
628    _mat[1][2] = r02*r10 - r00*r12; // same
629    _mat[1][3] = 0.0;
630    _mat[2][0] = r10*r21 - r11*r20; // Have already been divided by det
631    _mat[2][1] = r01*r20 - r00*r21; // same
632    _mat[2][2] = r00*r11 - r01*r10; // same
633    _mat[2][3] = 0.0;
634    _mat[3][3] = 1.0;
635
636// We no longer need the rxx or det variables anymore, so we can reuse them for whatever we want.  But we will still rename them for the sake of clarity.
637
638#define d r22
639    d  = mat._mat[3][3];
640
641    if( osg::square(d-1.0) > 1.0e-6 )  // Involves perspective, so we must
642    {                       // compute the full inverse
643   
644        Matrix_implementation TPinv;
645        _mat[3][0] = _mat[3][1] = _mat[3][2] = 0.0;
646
647#define px r00
648#define py r01
649#define pz r02
650#define one_over_s  one_over_det
651#define a  r10
652#define b  r11
653#define c  r12
654
655        a  = mat._mat[0][3]; b  = mat._mat[1][3]; c  = mat._mat[2][3];
656        px = _mat[0][0]*a + _mat[0][1]*b + _mat[0][2]*c;
657        py = _mat[1][0]*a + _mat[1][1]*b + _mat[1][2]*c;
658        pz = _mat[2][0]*a + _mat[2][1]*b + _mat[2][2]*c;
659
660#undef a
661#undef b
662#undef c
663#define tx r10
664#define ty r11
665#define tz r12
666
667        tx = mat._mat[3][0]; ty = mat._mat[3][1]; tz = mat._mat[3][2];
668        one_over_s  = 1.0/(d - (tx*px + ty*py + tz*pz));
669
670        tx *= one_over_s; ty *= one_over_s; tz *= one_over_s;  // Reduces number of calculations later on
671
672        // Compute inverse of trans*corr
673        TPinv._mat[0][0] = tx*px + 1.0;
674        TPinv._mat[0][1] = ty*px;
675        TPinv._mat[0][2] = tz*px;
676        TPinv._mat[0][3] = -px * one_over_s;
677        TPinv._mat[1][0] = tx*py;
678        TPinv._mat[1][1] = ty*py + 1.0;
679        TPinv._mat[1][2] = tz*py;
680        TPinv._mat[1][3] = -py * one_over_s;
681        TPinv._mat[2][0] = tx*pz;
682        TPinv._mat[2][1] = ty*pz;
683        TPinv._mat[2][2] = tz*pz + 1.0;
684        TPinv._mat[2][3] = -pz * one_over_s;
685        TPinv._mat[3][0] = -tx;
686        TPinv._mat[3][1] = -ty;
687        TPinv._mat[3][2] = -tz;
688        TPinv._mat[3][3] = one_over_s;
689
690        preMult(TPinv); // Finish computing full inverse of mat
691
692#undef px
693#undef py
694#undef pz
695#undef one_over_s
696#undef d
697    }
698    else // Rightmost column is [0; 0; 0; 1] so it can be ignored
699    {
700        tx = mat._mat[3][0]; ty = mat._mat[3][1]; tz = mat._mat[3][2];
701
702        // Compute translation components of mat'
703        _mat[3][0] = -(tx*_mat[0][0] + ty*_mat[1][0] + tz*_mat[2][0]);
704        _mat[3][1] = -(tx*_mat[0][1] + ty*_mat[1][1] + tz*_mat[2][1]);
705        _mat[3][2] = -(tx*_mat[0][2] + ty*_mat[1][2] + tz*_mat[2][2]);
706
707#undef tx
708#undef ty
709#undef tz
710    }
711
712    return true;
713}
714
715
716template <class T>
717inline T SGL_ABS(T a)
718{
719   return (a >= 0 ? a : -a);
720}
721
722#ifndef SGL_SWAP
723#define SGL_SWAP(a,b,temp) ((temp)=(a),(a)=(b),(b)=(temp))
724#endif
725
726bool Matrix_implementation::invert_4x4( const Matrix_implementation& mat )
727{
728    if (&mat==this) {
729       Matrix_implementation tm(mat);
730       return invert_4x4(tm);
731    }
732
733    unsigned int indxc[4], indxr[4], ipiv[4];
734    unsigned int i,j,k,l,ll;
735    unsigned int icol = 0;
736    unsigned int irow = 0;
737    double temp, pivinv, dum, big;
738
739    // copy in place this may be unnecessary
740    *this = mat;
741
742    for (j=0; j<4; j++) ipiv[j]=0;
743
744    for(i=0;i<4;i++)
745    {
746       big=0.0;
747       for (j=0; j<4; j++)
748          if (ipiv[j] != 1)
749             for (k=0; k<4; k++)
750             {
751                if (ipiv[k] == 0)
752                {
753                   if (SGL_ABS(operator()(j,k)) >= big)
754                   {
755                      big = SGL_ABS(operator()(j,k));
756                      irow=j;
757                      icol=k;
758                   }
759                }
760                else if (ipiv[k] > 1)
761                   return false;
762             }
763       ++(ipiv[icol]);
764       if (irow != icol)
765          for (l=0; l<4; l++) SGL_SWAP(operator()(irow,l),
766                                       operator()(icol,l),
767                                       temp);
768
769       indxr[i]=irow;
770       indxc[i]=icol;
771       if (operator()(icol,icol) == 0)
772          return false;
773
774       pivinv = 1.0/operator()(icol,icol);
775       operator()(icol,icol) = 1;
776       for (l=0; l<4; l++) operator()(icol,l) *= pivinv;
777       for (ll=0; ll<4; ll++)
778          if (ll != icol)
779          {
780             dum=operator()(ll,icol);
781             operator()(ll,icol) = 0;
782             for (l=0; l<4; l++) operator()(ll,l) -= operator()(icol,l)*dum;
783          }
784    }
785    for (int lx=4; lx>0; --lx)
786    {
787       if (indxr[lx-1] != indxc[lx-1])
788          for (k=0; k<4; k++) SGL_SWAP(operator()(k,indxr[lx-1]),
789                                       operator()(k,indxc[lx-1]),temp);
790    }
791
792    return true;
793}
794
795void Matrix_implementation::makeOrtho(double left, double right,
796                       double bottom, double top,
797                       double zNear, double zFar)
798{
799    // note transpose of Matrix_implementation wr.t OpenGL documentation, since the OSG use post multiplication rather than pre.
800    double tx = -(right+left)/(right-left);
801    double ty = -(top+bottom)/(top-bottom);
802    double tz = -(zFar+zNear)/(zFar-zNear);
803    SET_ROW(0, 2.0/(right-left),               0.0,               0.0, 0.0 )
804    SET_ROW(1,              0.0,  2.0/(top-bottom),               0.0, 0.0 )
805    SET_ROW(2,              0.0,               0.0,  -2.0/(zFar-zNear), 0.0 )
806    SET_ROW(3,               tx,                ty,                 tz, 1.0 )
807}
808
809bool Matrix_implementation::getOrtho(double& left, double& right,
810                      double& bottom, double& top,
811                      double& zNear, double& zFar) const
812{
813    if (_mat[0][3]!=0.0 || _mat[1][3]!=0.0 || _mat[2][3]!=0.0 || _mat[3][3]!=1.0) return false;
814
815    zNear = (_mat[3][2]+1.0) / _mat[2][2];
816    zFar = (_mat[3][2]-1.0) / _mat[2][2];
817   
818    left = -(1.0+_mat[3][0]) / _mat[0][0];
819    right = (1.0-_mat[3][0]) / _mat[0][0];
820
821    bottom = -(1.0+_mat[3][1]) / _mat[1][1];
822    top = (1.0-_mat[3][1]) / _mat[1][1];
823   
824    return true;
825}           
826
827
828void Matrix_implementation::makeFrustum(double left, double right,
829                         double bottom, double top,
830                         double zNear, double zFar)
831{
832    // note transpose of Matrix_implementation wr.t OpenGL documentation, since the OSG use post multiplication rather than pre.
833    double A = (right+left)/(right-left);
834    double B = (top+bottom)/(top-bottom);
835    double C = -(zFar+zNear)/(zFar-zNear);
836    double D = -2.0*zFar*zNear/(zFar-zNear);
837    SET_ROW(0, 2.0*zNear/(right-left),                    0.0, 0.0,  0.0 )
838    SET_ROW(1,                    0.0, 2.0*zNear/(top-bottom), 0.0,  0.0 )
839    SET_ROW(2,                      A,                      B,   C, -1.0 )
840    SET_ROW(3,                    0.0,                    0.0,   D,  0.0 )
841}
842
843bool Matrix_implementation::getFrustum(double& left, double& right,
844                                       double& bottom, double& top,
845                                       double& zNear, double& zFar) const
846{
847    if (_mat[0][3]!=0.0 || _mat[1][3]!=0.0 || _mat[2][3]!=-1.0 || _mat[3][3]!=0.0) return false;
848
849
850    zNear = _mat[3][2] / (_mat[2][2]-1.0);
851    zFar = _mat[3][2] / (1.0+_mat[2][2]);
852   
853    left = zNear * (_mat[2][0]-1.0) / _mat[0][0];
854    right = zNear * (1.0+_mat[2][0]) / _mat[0][0];
855
856    top = zNear * (1.0+_mat[2][1]) / _mat[1][1];
857    bottom = zNear * (_mat[2][1]-1.0) / _mat[1][1];
858   
859    return true;
860}                 
861
862
863void Matrix_implementation::makePerspective(double fovy,double aspectRatio,
864                                            double zNear, double zFar)
865{
866    // calculate the appropriate left, right etc.
867    double tan_fovy = tan(DegreesToRadians(fovy*0.5));
868    double right  =  tan_fovy * aspectRatio * zNear;
869    double left   = -right;
870    double top    =  tan_fovy * zNear;
871    double bottom =  -top;
872    makeFrustum(left,right,bottom,top,zNear,zFar);
873}
874
875bool Matrix_implementation::getPerspective(double& fovy,double& aspectRatio,
876                                           double& zNear, double& zFar) const
877{
878    double right  =  0.0;
879    double left   =  0.0;
880    double top    =  0.0;
881    double bottom =  0.0;
882    if (getFrustum(left,right,bottom,top,zNear,zFar))
883    {
884        fovy = RadiansToDegrees(atan(top/zNear)-atan(bottom/zNear));
885        aspectRatio = (right-left)/(top-bottom);
886        return true;
887    }
888    return false;
889}
890
891void Matrix_implementation::makeLookAt(const Vec3d& eye,const Vec3d& center,const Vec3d& up)
892{
893    Vec3d f(center-eye);
894    f.normalize();
895    Vec3d s(f^up);
896    s.normalize();
897    Vec3d u(s^f);
898    u.normalize();
899
900    set(
901        s[0],     u[0],     -f[0],     0.0,
902        s[1],     u[1],     -f[1],     0.0,
903        s[2],     u[2],     -f[2],     0.0,
904        0.0,     0.0,     0.0,      1.0);
905
906    preMult(Matrix_implementation::translate(-eye));
907}
908
909
910void Matrix_implementation::getLookAt(Vec3f& eye,Vec3f& center,Vec3f& up,value_type lookDistance) const
911{
912    Matrix_implementation inv;
913    inv.invert(*this);
914    eye = osg::Vec3f(0.0,0.0,0.0)*inv;
915    up = transform3x3(*this,osg::Vec3f(0.0,1.0,0.0));
916    center = transform3x3(*this,osg::Vec3f(0.0,0.0,-1));
917    center.normalize();
918    center = eye + center*lookDistance;
919}
920
921void Matrix_implementation::getLookAt(Vec3d& eye,Vec3d& center,Vec3d& up,value_type lookDistance) const
922{
923    Matrix_implementation inv;
924    inv.invert(*this);
925    eye = osg::Vec3d(0.0,0.0,0.0)*inv;
926    up = transform3x3(*this,osg::Vec3d(0.0,1.0,0.0));
927    center = transform3x3(*this,osg::Vec3d(0.0,0.0,-1));
928    center.normalize();
929    center = eye + center*lookDistance;
930}
931
932#undef SET_ROW
Note: See TracBrowser for help on using the browser.