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

Revision 13041, 31.5 kB (checked in by robert, 3 years ago)

Ran script to remove trailing spaces and tabs

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