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

Revision 8868, 27.7 kB (checked in by robert, 6 years ago)

From Mathias Froehlich, "This is a generic optimization that does not depend on any cpu or instruction
set.

The optimization is based on the observation that matrix matrix multiplication
with a dense matrix 4x4 is 43 Operations whereas multiplication with a
transform, or scale matrix is only 4
2 operations. Which is a gain of a
*FACTOR*4* for these special cases.
The change implements these special cases, provides a unit test for these
implementation and converts uses of the expensiver dense matrix matrix
routine with the specialized versions.

Depending on the transform nodes in the scenegraph this change gives a
noticable improovement.
For example the osgforest code using the MatrixTransform? is about 20% slower
than the same codepath using the PositionAttitudeTransform? instead of the
MatrixTransform? with this patch applied.

If I remember right, the sse type optimizations did *not* provide a factor 4
improovement. Also these changes are totally independent of any cpu or
instruction set architecture. So I would prefer to have this current kind of
change instead of some hand coded and cpu dependent assembly stuff. If we
need that hand tuned stuff, these can go on top of this changes which must
provide than hand optimized additional variants for the specialized versions
to give a even better result in the end.

An other change included here is a change to rotation matrix from quaterion
code. There is a sqrt call which couold be optimized away. Since we divide in
effect by sqrt(length)*sqrt(length) which is just length ...
"

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