root/OpenSceneGraph/trunk/include/osg/Matrixd @ 8868

Revision 8868, 29.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-2004 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#ifndef OSG_MATRIXD
15#define OSG_MATRIXD 1
16
17#include <osg/Object>
18#include <osg/Vec3d>
19#include <osg/Vec4d>
20#include <osg/Quat>
21
22namespace osg {
23
24class Matrixf;
25
26class OSG_EXPORT Matrixd
27{
28    public:
29   
30        typedef double value_type;
31
32        inline Matrixd() { makeIdentity(); }
33        inline Matrixd( const Matrixd& mat) { set(mat.ptr()); }
34        Matrixd( const Matrixf& mat );
35        inline explicit Matrixd( float const * const ptr ) { set(ptr); }
36        inline explicit Matrixd( double const * const ptr ) { set(ptr); }
37        inline explicit Matrixd( const Quat& quat ) { makeRotate(quat); }
38
39        Matrixd(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        ~Matrixd() {}
45
46        int compare(const Matrixd& m) const;
47
48        bool operator < (const Matrixd& m) const { return compare(m)<0; }
49        bool operator == (const Matrixd& m) const { return compare(m)==0; }
50        bool operator != (const Matrixd& m) const { return compare(m)!=0; }
51
52        inline value_type& operator()(int row, int col) { return _mat[row][col]; }
53        inline value_type operator()(int row, int col) const { return _mat[row][col]; }
54
55        inline bool valid() const { return !isNaN(); }
56        inline bool isNaN() const { return osg::isNaN(_mat[0][0]) || osg::isNaN(_mat[0][1]) || osg::isNaN(_mat[0][2]) || osg::isNaN(_mat[0][3]) ||
57                                                 osg::isNaN(_mat[1][0]) || osg::isNaN(_mat[1][1]) || osg::isNaN(_mat[1][2]) || osg::isNaN(_mat[1][3]) ||
58                                                 osg::isNaN(_mat[2][0]) || osg::isNaN(_mat[2][1]) || osg::isNaN(_mat[2][2]) || osg::isNaN(_mat[2][3]) ||
59                                                 osg::isNaN(_mat[3][0]) || osg::isNaN(_mat[3][1]) || osg::isNaN(_mat[3][2]) || osg::isNaN(_mat[3][3]); }
60
61        inline Matrixd& operator = (const Matrixd& rhs)
62        {
63            if( &rhs == this ) return *this;
64            set(rhs.ptr());
65            return *this;
66        }
67       
68        Matrixd& operator = (const Matrixf& other);
69
70        inline void set(const Matrixd& rhs) { set(rhs.ptr()); }
71
72        void set(const Matrixf& rhs);
73
74        inline void set(float const * const ptr)
75        {
76            value_type* local_ptr = (value_type*)_mat;
77            for(int i=0;i<16;++i) local_ptr[i]=(value_type)ptr[i];
78        }
79       
80        inline void set(double const * const ptr)
81        {
82            value_type* local_ptr = (value_type*)_mat;
83            for(int i=0;i<16;++i) local_ptr[i]=(value_type)ptr[i];
84        }
85
86        void set(value_type a00, value_type a01, value_type a02,value_type a03,
87                 value_type a10, value_type a11, value_type a12,value_type a13,
88                 value_type a20, value_type a21, value_type a22,value_type a23,
89                 value_type a30, value_type a31, value_type a32,value_type a33);
90                 
91        value_type * ptr() { return (value_type*)_mat; }
92        const value_type * ptr() const { return (const value_type *)_mat; }
93
94        bool isIdentity() const
95        {
96            return _mat[0][0]==1.0 && _mat[0][1]==0.0 && _mat[0][2]==0.0 &&  _mat[0][3]==0.0 &&
97                   _mat[1][0]==0.0 && _mat[1][1]==1.0 && _mat[1][2]==0.0 &&  _mat[1][3]==0.0 &&
98                   _mat[2][0]==0.0 && _mat[2][1]==0.0 && _mat[2][2]==1.0 &&  _mat[2][3]==0.0 &&
99                   _mat[3][0]==0.0 && _mat[3][1]==0.0 && _mat[3][2]==0.0 &&  _mat[3][3]==1.0;
100        }
101
102        void makeIdentity();
103       
104        void makeScale( const Vec3f& );
105        void makeScale( const Vec3d& );
106        void makeScale( value_type, value_type, value_type );
107       
108        void makeTranslate( const Vec3f& );
109        void makeTranslate( const Vec3d& );
110        void makeTranslate( value_type, value_type, value_type );
111       
112        void makeRotate( const Vec3f& from, const Vec3f& to );
113        void makeRotate( const Vec3d& from, const Vec3d& to );
114        void makeRotate( value_type angle, const Vec3f& axis );
115        void makeRotate( value_type angle, const Vec3d& axis );
116        void makeRotate( value_type angle, value_type x, value_type y, value_type z );
117        void makeRotate( const Quat& );
118        void makeRotate( value_type angle1, const Vec3f& axis1,
119                         value_type angle2, const Vec3f& axis2,
120                         value_type angle3, const Vec3f& axis3);
121        void makeRotate( value_type angle1, const Vec3d& axis1,
122                         value_type angle2, const Vec3d& axis2,
123                         value_type angle3, const Vec3d& axis3);
124
125
126        /** decompose the matrix into translation, rotation, scale and scale orientation.*/       
127        void decompose( osg::Vec3f& translation,
128                        osg::Quat& rotation,
129                        osg::Vec3f& scale,
130                        osg::Quat& so ) const;
131
132        /** decompose the matrix into translation, rotation, scale and scale orientation.*/       
133        void decompose( osg::Vec3d& translation,
134                        osg::Quat& rotation,
135                        osg::Vec3d& scale,
136                        osg::Quat& so ) const;
137
138
139        /** Set to an orthographic projection.
140         * See glOrtho for further details.
141        */
142        void makeOrtho(double left,   double right,
143                       double bottom, double top,
144                       double zNear,  double zFar);
145
146        /** Get the orthographic settings of the orthographic projection matrix.
147          * Note, if matrix is not an orthographic matrix then invalid values
148          * will be returned.
149        */
150        bool getOrtho(double& left,   double& right,
151                      double& bottom, double& top,
152                      double& zNear,  double& zFar) const;
153
154        /** Set to a 2D orthographic projection.
155          * See glOrtho2D for further details.
156        */
157        inline void makeOrtho2D(double left,   double right,
158                                double bottom, double top)
159        {
160            makeOrtho(left,right,bottom,top,-1.0,1.0);
161        }
162
163
164        /** Set to a perspective projection.
165          * See glFrustum for further details.
166        */
167        void makeFrustum(double left,   double right,
168                         double bottom, double top,
169                         double zNear,  double zFar);
170
171        /** Get the frustum settings of a perspective projection matrix.
172          * Note, if matrix is not a perspective matrix then invalid values
173          * will be returned.
174        */
175        bool getFrustum(double& left,   double& right,
176                        double& bottom, double& top,
177                        double& zNear,  double& zFar) const;
178
179        /** Set to a symmetrical perspective projection.
180          * See gluPerspective for further details.
181          * Aspect ratio is defined as width/height.
182        */
183        void makePerspective(double fovy,  double aspectRatio,
184                             double zNear, double zFar);
185
186        /** Get the frustum settings of a symmetric perspective projection
187          * matrix.
188          * Return false if matrix is not a perspective matrix,
189          * where parameter values are undefined.
190          * Note, if matrix is not a symmetric perspective matrix then the
191          * shear will be lost.
192          * Asymmetric matrices occur when stereo, power walls, caves and
193          * reality center display are used.
194          * In these configuration one should use the AsFrustum method instead.
195        */
196        bool getPerspective(double& fovy,  double& aspectRatio,
197                            double& zNear, double& zFar) const;
198
199        /** Set the position and orientation to be a view matrix,
200          * using the same convention as gluLookAt.
201        */
202        void makeLookAt(const Vec3d& eye,const Vec3d& center,const Vec3d& up);
203
204        /** Get to the position and orientation of a modelview matrix,
205          * using the same convention as gluLookAt.
206        */
207        void getLookAt(Vec3f& eye,Vec3f& center,Vec3f& up,
208                       value_type lookDistance=1.0f) const;
209
210        /** Get to the position and orientation of a modelview matrix,
211          * using the same convention as gluLookAt.
212        */
213        void getLookAt(Vec3d& eye,Vec3d& center,Vec3d& up,
214                       value_type lookDistance=1.0f) const;
215
216        /** invert the matrix rhs, automatically select invert_4x3 or invert_4x4. */
217        inline bool invert( const Matrixd& rhs)
218        {
219            bool is_4x3 = (rhs._mat[0][3]==0.0 && rhs._mat[1][3]==0.0 &&  rhs._mat[2][3]==0.0 && rhs._mat[3][3]==1.0);
220            return is_4x3 ? invert_4x3(rhs) :  invert_4x4(rhs);
221        }
222
223        /** 4x3 matrix invert, not right hand column is assumed to be 0,0,0,1. */
224        bool invert_4x3( const Matrixd& rhs);
225
226        /** full 4x4 matrix invert. */
227        bool invert_4x4( const Matrixd& rhs);
228
229        /** ortho-normalize the 3x3 rotation & scale matrix */
230        void orthoNormalize(const Matrixd& rhs);
231
232        // basic utility functions to create new matrices
233        inline static Matrixd identity( void );
234        inline static Matrixd scale( const Vec3f& sv);
235        inline static Matrixd scale( const Vec3d& sv);
236        inline static Matrixd scale( value_type sx, value_type sy, value_type sz);
237        inline static Matrixd translate( const Vec3f& dv);
238        inline static Matrixd translate( const Vec3d& dv);
239        inline static Matrixd translate( value_type x, value_type y, value_type z);
240        inline static Matrixd rotate( const Vec3f& from, const Vec3f& to);
241        inline static Matrixd rotate( const Vec3d& from, const Vec3d& to);
242        inline static Matrixd rotate( value_type angle, value_type x, value_type y, value_type z);
243        inline static Matrixd rotate( value_type angle, const Vec3f& axis);
244        inline static Matrixd rotate( value_type angle, const Vec3d& axis);
245        inline static Matrixd rotate( value_type angle1, const Vec3f& axis1,
246                                      value_type angle2, const Vec3f& axis2,
247                                      value_type angle3, const Vec3f& axis3);
248        inline static Matrixd rotate( value_type angle1, const Vec3d& axis1,
249                                      value_type angle2, const Vec3d& axis2,
250                                      value_type angle3, const Vec3d& axis3);
251        inline static Matrixd rotate( const Quat& quat);
252        inline static Matrixd inverse( const Matrixd& matrix);
253        inline static Matrixd orthoNormal(const Matrixd& matrix);
254        /** Create an orthographic projection matrix.
255          * See glOrtho for further details.
256        */
257        inline static Matrixd ortho(double left,   double right,
258                                    double bottom, double top,
259                                    double zNear,  double zFar);
260
261        /** Create a 2D orthographic projection.
262          * See glOrtho for further details.
263        */
264        inline static Matrixd ortho2D(double left,   double right,
265                                      double bottom, double top);
266
267        /** Create a perspective projection.
268          * See glFrustum for further details.
269        */
270        inline static Matrixd frustum(double left,   double right,
271                                      double bottom, double top,
272                                      double zNear,  double zFar);
273
274        /** Create a symmetrical perspective projection.
275          * See gluPerspective for further details.
276          * Aspect ratio is defined as width/height.
277        */
278        inline static Matrixd perspective(double fovy,  double aspectRatio,
279                                          double zNear, double zFar);
280
281        /** Create the position and orientation as per a camera,
282          * using the same convention as gluLookAt.
283        */
284        inline static Matrixd lookAt(const Vec3f& eye,
285                                     const Vec3f& center,
286                                     const Vec3f& up);
287
288        /** Create the position and orientation as per a camera,
289          * using the same convention as gluLookAt.
290        */
291        inline static Matrixd lookAt(const Vec3d& eye,
292                                     const Vec3d& center,
293                                     const Vec3d& up);
294
295        inline Vec3f preMult( const Vec3f& v ) const;
296        inline Vec3d preMult( const Vec3d& v ) const;
297        inline Vec3f postMult( const Vec3f& v ) const;
298        inline Vec3d postMult( const Vec3d& v ) const;
299        inline Vec3f operator* ( const Vec3f& v ) const;
300        inline Vec3d operator* ( const Vec3d& v ) const;
301        inline Vec4f preMult( const Vec4f& v ) const;
302        inline Vec4d preMult( const Vec4d& v ) const;
303        inline Vec4f postMult( const Vec4f& v ) const;
304        inline Vec4d postMult( const Vec4d& v ) const;
305        inline Vec4f operator* ( const Vec4f& v ) const;
306        inline Vec4d operator* ( const Vec4d& v ) const;
307
308#ifdef USE_DEPRECATED_API
309        inline void set(const Quat& q) { makeRotate(q); }
310        inline void get(Quat& q) const { q = getRotate(); }
311#endif
312
313        void setRotate(const Quat& q);
314        /** Get the matrix rotation as a Quat. Note that this function
315          * assumes a non-scaled matrix and will return incorrect results
316          * for scaled matrixces. Consider decompose() instead.
317          */
318        Quat getRotate() const;
319
320        void setTrans( value_type tx, value_type ty, value_type tz );
321        void setTrans( const Vec3f& v );
322        void setTrans( const Vec3d& v );
323       
324        inline Vec3d getTrans() const { return Vec3d(_mat[3][0],_mat[3][1],_mat[3][2]); }
325       
326        inline Vec3d getScale() const {
327          Vec3d x_vec(_mat[0][0],_mat[1][0],_mat[2][0]);
328          Vec3d y_vec(_mat[0][1],_mat[1][1],_mat[2][1]);
329          Vec3d z_vec(_mat[0][2],_mat[1][2],_mat[2][2]);
330          return Vec3d(x_vec.length(), y_vec.length(), z_vec.length());
331        }
332       
333        /** apply a 3x3 transform of v*M[0..2,0..2]. */
334        inline static Vec3f transform3x3(const Vec3f& v,const Matrixd& m);
335
336        /** apply a 3x3 transform of v*M[0..2,0..2]. */
337        inline static Vec3d transform3x3(const Vec3d& v,const Matrixd& m);
338
339        /** apply a 3x3 transform of M[0..2,0..2]*v. */
340        inline static Vec3f transform3x3(const Matrixd& m,const Vec3f& v);
341
342        /** apply a 3x3 transform of M[0..2,0..2]*v. */
343        inline static Vec3d transform3x3(const Matrixd& m,const Vec3d& v);
344
345        // basic Matrixd multiplication, our workhorse methods.
346        void mult( const Matrixd&, const Matrixd& );
347        void preMult( const Matrixd& );
348        void postMult( const Matrixd& );
349
350        /** Optimized version of preMult(translate(v)); */
351        inline void preMultTranslate( const Vec3d& v );
352        inline void preMultTranslate( const Vec3f& v );
353        /** Optimized version of postMult(translate(v)); */
354        inline void postMultTranslate( const Vec3d& v );
355        inline void postMultTranslate( const Vec3f& v );
356
357        /** Optimized version of preMult(scale(v)); */
358        inline void preMultScale( const Vec3d& v );
359        inline void preMultScale( const Vec3f& v );
360        /** Optimized version of postMult(scale(v)); */
361        inline void postMultScale( const Vec3d& v );
362        inline void postMultScale( const Vec3f& v );
363
364        /** Optimized version of preMult(rotate(q)); */
365        inline void preMultRotate( const Quat& q );
366        /** Optimized version of postMult(rotate(q)); */
367        inline void postMultRotate( const Quat& q );
368
369        inline void operator *= ( const Matrixd& other )
370        {    if( this == &other ) {
371                Matrixd temp(other);
372                postMult( temp );
373            }
374            else postMult( other );
375        }
376
377        inline Matrixd operator * ( const Matrixd &m ) const
378        {
379            osg::Matrixd r;
380            r.mult(*this,m);
381            return  r;
382        }
383
384    protected:
385        value_type _mat[4][4];
386
387};
388
389class RefMatrixd : public Object, public Matrixd
390{
391    public:
392   
393        RefMatrixd():Object(false), Matrixd() {}
394        RefMatrixd( const Matrixd& other) : Object(false), Matrixd(other) {}
395        RefMatrixd( const Matrixf& other) : Object(false), Matrixd(other) {}
396        RefMatrixd( const RefMatrixd& other) : Object(other), Matrixd(other) {}
397        explicit RefMatrixd( Matrixd::value_type const * const def ):Object(false), Matrixd(def) {}
398        RefMatrixd( Matrixd::value_type a00, Matrixd::value_type a01, Matrixd::value_type a02, Matrixd::value_type a03,
399            Matrixd::value_type a10, Matrixd::value_type a11, Matrixd::value_type a12, Matrixd::value_type a13,
400            Matrixd::value_type a20, Matrixd::value_type a21, Matrixd::value_type a22, Matrixd::value_type a23,
401            Matrixd::value_type a30, Matrixd::value_type a31, Matrixd::value_type a32, Matrixd::value_type a33):
402            Object(false),
403            Matrixd(a00, a01, a02, a03,
404                    a10, a11, a12, a13,
405                    a20, a21, a22, a23,
406                    a30, a31, a32, a33) {}
407
408        virtual Object* cloneType() const { return new RefMatrixd(); }
409        virtual Object* clone(const CopyOp&) const { return new RefMatrixd(*this); }
410        virtual bool isSameKindAs(const Object* obj) const { return dynamic_cast<const RefMatrixd*>(obj)!=NULL; }
411        virtual const char* libraryName() const { return "osg"; }
412        virtual const char* className() const { return "Matrix"; }
413       
414       
415    protected:
416   
417        virtual ~RefMatrixd() {}
418};
419
420
421// static utility methods
422inline Matrixd Matrixd::identity(void)
423{
424    Matrixd m;
425    m.makeIdentity();
426    return m;
427}
428
429inline Matrixd Matrixd::scale(value_type sx, value_type sy, value_type sz)
430{
431    Matrixd m;
432    m.makeScale(sx,sy,sz);
433    return m;
434}
435
436inline Matrixd Matrixd::scale(const Vec3f& v )
437{
438    return scale(v.x(), v.y(), v.z() );
439}
440
441inline Matrixd Matrixd::scale(const Vec3d& v )
442{
443    return scale(v.x(), v.y(), v.z() );
444}
445
446inline Matrixd Matrixd::translate(value_type tx, value_type ty, value_type tz)
447{
448    Matrixd m;
449    m.makeTranslate(tx,ty,tz);
450    return m;
451}
452
453inline Matrixd Matrixd::translate(const Vec3f& v )
454{
455    return translate(v.x(), v.y(), v.z() );
456}
457
458inline Matrixd Matrixd::translate(const Vec3d& v )
459{
460    return translate(v.x(), v.y(), v.z() );
461}
462
463inline Matrixd Matrixd::rotate( const Quat& q )
464{
465    return Matrixd(q);
466}
467inline Matrixd Matrixd::rotate(value_type angle, value_type x, value_type y, value_type z )
468{
469    Matrixd m;
470    m.makeRotate(angle,x,y,z);
471    return m;
472}
473inline Matrixd Matrixd::rotate(value_type angle, const Vec3f& axis )
474{
475    Matrixd m;
476    m.makeRotate(angle,axis);
477    return m;
478}
479inline Matrixd Matrixd::rotate(value_type angle, const Vec3d& axis )
480{
481    Matrixd m;
482    m.makeRotate(angle,axis);
483    return m;
484}
485inline Matrixd Matrixd::rotate( value_type angle1, const Vec3f& axis1,
486                                value_type angle2, const Vec3f& axis2,
487                                value_type angle3, const Vec3f& axis3)
488{
489    Matrixd m;
490    m.makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
491    return m;
492}
493inline Matrixd Matrixd::rotate( value_type angle1, const Vec3d& axis1,
494                                value_type angle2, const Vec3d& axis2,
495                                value_type angle3, const Vec3d& axis3)
496{
497    Matrixd m;
498    m.makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
499    return m;
500}
501inline Matrixd Matrixd::rotate(const Vec3f& from, const Vec3f& to )
502{
503    Matrixd m;
504    m.makeRotate(from,to);
505    return m;
506}
507inline Matrixd Matrixd::rotate(const Vec3d& from, const Vec3d& to )
508{
509    Matrixd m;
510    m.makeRotate(from,to);
511    return m;
512}
513
514inline Matrixd Matrixd::inverse( const Matrixd& matrix)
515{
516    Matrixd m;
517    m.invert(matrix);
518    return m;
519}
520
521inline Matrixd Matrixd::orthoNormal(const Matrixd& matrix)
522{
523  Matrixd m;
524  m.orthoNormalize(matrix);
525  return m;
526}
527
528inline Matrixd Matrixd::ortho(double left,   double right,
529                              double bottom, double top,
530                              double zNear,  double zFar)
531{
532    Matrixd m;
533    m.makeOrtho(left,right,bottom,top,zNear,zFar);
534    return m;
535}
536
537inline Matrixd Matrixd::ortho2D(double left,   double right,
538                                double bottom, double top)
539{
540    Matrixd m;
541    m.makeOrtho2D(left,right,bottom,top);
542    return m;
543}
544
545inline Matrixd Matrixd::frustum(double left,   double right,
546                                double bottom, double top,
547                                double zNear,  double zFar)
548{
549    Matrixd m;
550    m.makeFrustum(left,right,bottom,top,zNear,zFar);
551    return m;
552}
553
554inline Matrixd Matrixd::perspective(double fovy,  double aspectRatio,
555                                    double zNear, double zFar)
556{
557    Matrixd m;
558    m.makePerspective(fovy,aspectRatio,zNear,zFar);
559    return m;
560}
561
562inline Matrixd Matrixd::lookAt(const Vec3f& eye,
563                               const Vec3f& center,
564                               const Vec3f& up)
565{
566    Matrixd m;
567    m.makeLookAt(eye,center,up);
568    return m;
569}
570
571inline Matrixd Matrixd::lookAt(const Vec3d& eye,
572                               const Vec3d& center,
573                               const Vec3d& up)
574{
575    Matrixd m;
576    m.makeLookAt(eye,center,up);
577    return m;
578}
579
580inline Vec3f Matrixd::postMult( const Vec3f& v ) const
581{
582    value_type d = 1.0f/(_mat[3][0]*v.x()+_mat[3][1]*v.y()+_mat[3][2]*v.z()+_mat[3][3]) ;
583    return Vec3f( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3])*d,
584        (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3])*d,
585        (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3])*d) ;
586}
587
588inline Vec3d Matrixd::postMult( const Vec3d& v ) const
589{
590    value_type d = 1.0f/(_mat[3][0]*v.x()+_mat[3][1]*v.y()+_mat[3][2]*v.z()+_mat[3][3]) ;
591    return Vec3d( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3])*d,
592        (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3])*d,
593        (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3])*d) ;
594}
595
596inline Vec3f Matrixd::preMult( const Vec3f& v ) const
597{
598    value_type d = 1.0f/(_mat[0][3]*v.x()+_mat[1][3]*v.y()+_mat[2][3]*v.z()+_mat[3][3]) ;
599    return Vec3f( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0])*d,
600        (_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1])*d,
601        (_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2])*d);
602}
603
604inline Vec3d Matrixd::preMult( const Vec3d& v ) const
605{
606    value_type d = 1.0f/(_mat[0][3]*v.x()+_mat[1][3]*v.y()+_mat[2][3]*v.z()+_mat[3][3]) ;
607    return Vec3d( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0])*d,
608        (_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1])*d,
609        (_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2])*d);
610}
611
612inline Vec4f Matrixd::postMult( const Vec4f& v ) const
613{
614    return Vec4f( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3]*v.w()),
615        (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3]*v.w()),
616        (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3]*v.w()),
617        (_mat[3][0]*v.x() + _mat[3][1]*v.y() + _mat[3][2]*v.z() + _mat[3][3]*v.w())) ;
618}
619inline Vec4d Matrixd::postMult( const Vec4d& v ) const
620{
621    return Vec4d( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3]*v.w()),
622        (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3]*v.w()),
623        (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3]*v.w()),
624        (_mat[3][0]*v.x() + _mat[3][1]*v.y() + _mat[3][2]*v.z() + _mat[3][3]*v.w())) ;
625}
626
627inline Vec4f Matrixd::preMult( const Vec4f& v ) const
628{
629    return Vec4f( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0]*v.w()),
630        (_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1]*v.w()),
631        (_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2]*v.w()),
632        (_mat[0][3]*v.x() + _mat[1][3]*v.y() + _mat[2][3]*v.z() + _mat[3][3]*v.w()));
633}
634
635inline Vec4d Matrixd::preMult( const Vec4d& v ) const
636{
637    return Vec4d( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0]*v.w()),
638        (_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1]*v.w()),
639        (_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2]*v.w()),
640        (_mat[0][3]*v.x() + _mat[1][3]*v.y() + _mat[2][3]*v.z() + _mat[3][3]*v.w()));
641}
642
643inline Vec3f Matrixd::transform3x3(const Vec3f& v,const Matrixd& m)
644{
645    return Vec3f( (m._mat[0][0]*v.x() + m._mat[1][0]*v.y() + m._mat[2][0]*v.z()),
646                 (m._mat[0][1]*v.x() + m._mat[1][1]*v.y() + m._mat[2][1]*v.z()),
647                 (m._mat[0][2]*v.x() + m._mat[1][2]*v.y() + m._mat[2][2]*v.z()));
648}
649inline Vec3d Matrixd::transform3x3(const Vec3d& v,const Matrixd& m)
650{
651    return Vec3d( (m._mat[0][0]*v.x() + m._mat[1][0]*v.y() + m._mat[2][0]*v.z()),
652                 (m._mat[0][1]*v.x() + m._mat[1][1]*v.y() + m._mat[2][1]*v.z()),
653                 (m._mat[0][2]*v.x() + m._mat[1][2]*v.y() + m._mat[2][2]*v.z()));
654}
655
656inline Vec3f Matrixd::transform3x3(const Matrixd& m,const Vec3f& v)
657{
658    return Vec3f( (m._mat[0][0]*v.x() + m._mat[0][1]*v.y() + m._mat[0][2]*v.z()),
659                 (m._mat[1][0]*v.x() + m._mat[1][1]*v.y() + m._mat[1][2]*v.z()),
660                 (m._mat[2][0]*v.x() + m._mat[2][1]*v.y() + m._mat[2][2]*v.z()) ) ;
661}
662inline Vec3d Matrixd::transform3x3(const Matrixd& m,const Vec3d& v)
663{
664    return Vec3d( (m._mat[0][0]*v.x() + m._mat[0][1]*v.y() + m._mat[0][2]*v.z()),
665                 (m._mat[1][0]*v.x() + m._mat[1][1]*v.y() + m._mat[1][2]*v.z()),
666                 (m._mat[2][0]*v.x() + m._mat[2][1]*v.y() + m._mat[2][2]*v.z()) ) ;
667}
668
669inline void Matrixd::preMultTranslate( const Vec3d& v )
670{
671    for (unsigned i = 0; i < 3; ++i)
672    {
673        double tmp = v[i];
674        if (tmp == 0)
675            continue;
676        _mat[3][0] += tmp*_mat[i][0];
677        _mat[3][1] += tmp*_mat[i][1];
678        _mat[3][2] += tmp*_mat[i][2];
679        _mat[3][3] += tmp*_mat[i][3];
680    }
681}
682
683inline void Matrixd::preMultTranslate( const Vec3f& v )
684{
685    for (unsigned i = 0; i < 3; ++i)
686    {
687        float tmp = v[i];
688        if (tmp == 0)
689            continue;
690        _mat[3][0] += tmp*_mat[i][0];
691        _mat[3][1] += tmp*_mat[i][1];
692        _mat[3][2] += tmp*_mat[i][2];
693        _mat[3][3] += tmp*_mat[i][3];
694    }
695}
696
697inline void Matrixd::postMultTranslate( const Vec3d& v )
698{
699    for (unsigned i = 0; i < 3; ++i)
700    {
701        double tmp = v[i];
702        if (tmp == 0)
703            continue;
704        _mat[0][i] += tmp*_mat[0][3];
705        _mat[1][i] += tmp*_mat[1][3];
706        _mat[2][i] += tmp*_mat[2][3];
707        _mat[3][i] += tmp*_mat[3][3];
708    }
709}
710
711inline void Matrixd::postMultTranslate( const Vec3f& v )
712{
713    for (unsigned i = 0; i < 3; ++i)
714    {
715        float tmp = v[i];
716        if (tmp == 0)
717            continue;
718        _mat[0][i] += tmp*_mat[0][3];
719        _mat[1][i] += tmp*_mat[1][3];
720        _mat[2][i] += tmp*_mat[2][3];
721        _mat[3][i] += tmp*_mat[3][3];
722    }
723}
724
725inline void Matrixd::preMultScale( const Vec3d& v )
726{
727    _mat[0][0] *= v[0]; _mat[0][1] *= v[0]; _mat[0][2] *= v[0]; _mat[0][3] *= v[0];
728    _mat[1][0] *= v[1]; _mat[1][1] *= v[1]; _mat[1][2] *= v[1]; _mat[1][3] *= v[1];
729    _mat[2][0] *= v[2]; _mat[2][1] *= v[2]; _mat[2][2] *= v[2]; _mat[2][3] *= v[2];
730}
731
732inline void Matrixd::preMultScale( const Vec3f& v )
733{
734    _mat[0][0] *= v[0]; _mat[0][1] *= v[0]; _mat[0][2] *= v[0]; _mat[0][3] *= v[0];
735    _mat[1][0] *= v[1]; _mat[1][1] *= v[1]; _mat[1][2] *= v[1]; _mat[1][3] *= v[1];
736    _mat[2][0] *= v[2]; _mat[2][1] *= v[2]; _mat[2][2] *= v[2]; _mat[2][3] *= v[2];
737}
738
739inline void Matrixd::postMultScale( const Vec3d& v )
740{
741    _mat[0][0] *= v[0]; _mat[1][0] *= v[0]; _mat[2][0] *= v[0]; _mat[3][0] *= v[0];
742    _mat[0][1] *= v[1]; _mat[1][1] *= v[1]; _mat[2][1] *= v[1]; _mat[3][1] *= v[1];
743    _mat[0][2] *= v[2]; _mat[1][2] *= v[2]; _mat[2][2] *= v[2]; _mat[3][2] *= v[2];
744}
745
746inline void Matrixd::postMultScale( const Vec3f& v )
747{
748    _mat[0][0] *= v[0]; _mat[1][0] *= v[0]; _mat[2][0] *= v[0]; _mat[3][0] *= v[0];
749    _mat[0][1] *= v[1]; _mat[1][1] *= v[1]; _mat[2][1] *= v[1]; _mat[3][1] *= v[1];
750    _mat[0][2] *= v[2]; _mat[1][2] *= v[2]; _mat[2][2] *= v[2]; _mat[3][2] *= v[2];
751}
752
753inline void Matrixd::preMultRotate( const Quat& q )
754{
755    if (q.zeroRotation())
756        return;
757    Matrixd r;
758    r.setRotate(q);
759    preMult(r);
760}
761
762inline void Matrixd::postMultRotate( const Quat& q )
763{
764    if (q.zeroRotation())
765        return;
766    Matrixd r;
767    r.setRotate(q);
768    postMult(r);
769}
770
771inline Vec3f operator* (const Vec3f& v, const Matrixd& m )
772{
773    return m.preMult(v);
774}
775
776inline Vec3d operator* (const Vec3d& v, const Matrixd& m )
777{
778    return m.preMult(v);
779}
780
781inline Vec4f operator* (const Vec4f& v, const Matrixd& m )
782{
783    return m.preMult(v);
784}
785
786inline Vec4d operator* (const Vec4d& v, const Matrixd& m )
787{
788    return m.preMult(v);
789}
790
791inline Vec3f Matrixd::operator* (const Vec3f& v) const
792{
793    return postMult(v);
794}
795
796inline Vec3d Matrixd::operator* (const Vec3d& v) const
797{
798    return postMult(v);
799}
800
801inline Vec4f Matrixd::operator* (const Vec4f& v) const
802{
803    return postMult(v);
804}
805
806inline Vec4d Matrixd::operator* (const Vec4d& v) const
807{
808    return postMult(v);
809}
810
811
812} //namespace osg
813
814
815#endif
Note: See TracBrowser for help on using the browser.