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

Revision 8868, 29.6 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_MATRIXF
15#define OSG_MATRIXF 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 Matrixf
27{
28    public:
29   
30        typedef float value_type;
31
32        inline Matrixf() { makeIdentity(); }
33        inline Matrixf( const Matrixf& mat) { set(mat.ptr()); }
34        Matrixf( const Matrixd& mat );
35        inline explicit Matrixf( float const * const ptr ) { set(ptr); }
36        inline explicit Matrixf( double const * const ptr ) { set(ptr); }
37        inline explicit Matrixf( const Quat& quat ) { makeRotate(quat); }
38
39        Matrixf( 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        ~Matrixf() {}
45
46        int compare(const Matrixf& m) const;
47
48        bool operator < (const Matrixf& m) const { return compare(m)<0; }
49        bool operator == (const Matrixf& m) const { return compare(m)==0; }
50        bool operator != (const Matrixf& 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 Matrixf& operator = (const Matrixf& rhs)
62        {
63            if( &rhs == this ) return *this;
64            set(rhs.ptr());
65            return *this;
66        }
67       
68        Matrixf& operator = (const Matrixd& other);
69
70        inline void set(const Matrixf& rhs) { set(rhs.ptr()); }
71
72        void set(const Matrixd& 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.0f && _mat[0][1]==0.0f && _mat[0][2]==0.0f &&  _mat[0][3]==0.0f &&
97                   _mat[1][0]==0.0f && _mat[1][1]==1.0f && _mat[1][2]==0.0f &&  _mat[1][3]==0.0f &&
98                   _mat[2][0]==0.0f && _mat[2][1]==0.0f && _mat[2][2]==1.0f &&  _mat[2][3]==0.0f &&
99                   _mat[3][0]==0.0f && _mat[3][1]==0.0f && _mat[3][2]==0.0f &&  _mat[3][3]==1.0f;
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 Matrixf& rhs)
218        {
219            bool is_4x3 = (rhs._mat[0][3]==0.0f && rhs._mat[1][3]==0.0f &&  rhs._mat[2][3]==0.0f && rhs._mat[3][3]==1.0f);
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 Matrixf& rhs);
225
226        /** full 4x4 matrix invert. */
227        bool invert_4x4( const Matrixf& rhs);
228
229        /** ortho-normalize the 3x3 rotation & scale matrix */
230        void orthoNormalize(const Matrixf& rhs);
231
232        //basic utility functions to create new matrices
233        inline static Matrixf identity( void );
234        inline static Matrixf scale( const Vec3f& sv);
235        inline static Matrixf scale( const Vec3d& sv);
236        inline static Matrixf scale( value_type sx, value_type sy, value_type sz);
237        inline static Matrixf translate( const Vec3f& dv);
238        inline static Matrixf translate( const Vec3d& dv);
239        inline static Matrixf translate( value_type x, value_type y, value_type z);
240        inline static Matrixf rotate( const Vec3f& from, const Vec3f& to);
241        inline static Matrixf rotate( const Vec3d& from, const Vec3d& to);
242        inline static Matrixf rotate( value_type angle, value_type x, value_type y, value_type z);
243        inline static Matrixf rotate( value_type angle, const Vec3f& axis);
244        inline static Matrixf rotate( value_type angle, const Vec3d& axis);
245        inline static Matrixf rotate( value_type angle1, const Vec3f& axis1,
246                                      value_type angle2, const Vec3f& axis2,
247                                      value_type angle3, const Vec3f& axis3);
248        inline static Matrixf rotate( value_type angle1, const Vec3d& axis1,
249                                      value_type angle2, const Vec3d& axis2,
250                                      value_type angle3, const Vec3d& axis3);
251        inline static Matrixf rotate( const Quat& quat);
252        inline static Matrixf inverse( const Matrixf& matrix);
253        inline static Matrixf orthoNormal(const Matrixf& matrix);
254       
255        /** Create an orthographic projection matrix.
256          * See glOrtho for further details.
257        */
258        inline static Matrixf ortho(double left,   double right,
259                                    double bottom, double top,
260                                    double zNear,  double zFar);
261
262        /** Create a 2D orthographic projection.
263          * See glOrtho for further details.
264        */
265        inline static Matrixf ortho2D(double left,   double right,
266                                      double bottom, double top);
267
268        /** Create a perspective projection.
269          * See glFrustum for further details.
270        */
271        inline static Matrixf frustum(double left,   double right,
272                                      double bottom, double top,
273                                      double zNear,  double zFar);
274
275        /** Create a symmetrical perspective projection.
276          * See gluPerspective for further details.
277          * Aspect ratio is defined as width/height.
278        */
279        inline static Matrixf perspective(double fovy,  double aspectRatio,
280                                          double zNear, double zFar);
281
282        /** Create the position and orientation as per a camera,
283          * using the same convention as gluLookAt.
284        */
285        inline static Matrixf lookAt(const Vec3f& eye,
286                                     const Vec3f& center,
287                                     const Vec3f& up);
288
289        /** Create the position and orientation as per a camera,
290          * using the same convention as gluLookAt.
291        */
292        inline static Matrixf lookAt(const Vec3d& eye,
293                                     const Vec3d& center,
294                                     const Vec3d& up);
295
296        inline Vec3f preMult( const Vec3f& v ) const;
297        inline Vec3d preMult( const Vec3d& v ) const;
298        inline Vec3f postMult( const Vec3f& v ) const;
299        inline Vec3d postMult( const Vec3d& v ) const;
300        inline Vec3f operator* ( const Vec3f& v ) const;
301        inline Vec3d operator* ( const Vec3d& v ) const;
302        inline Vec4f preMult( const Vec4f& v ) const;
303        inline Vec4d preMult( const Vec4d& v ) const;
304        inline Vec4f postMult( const Vec4f& v ) const;
305        inline Vec4d postMult( const Vec4d& v ) const;
306        inline Vec4f operator* ( const Vec4f& v ) const;
307        inline Vec4d operator* ( const Vec4d& v ) const;
308
309#ifdef USE_DEPRECATED_API
310        inline void set(const Quat& q) { makeRotate(q); }
311        inline void get(Quat& q) const { q = getRotate(); }
312#endif
313
314        void setRotate(const Quat& q);
315        /** Get the matrix rotation as a Quat. Note that this function
316          * assumes a non-scaled matrix and will return incorrect results
317          * for scaled matrixces. Consider decompose() instead.
318          */
319        Quat getRotate() const;
320
321
322        void setTrans( value_type tx, value_type ty, value_type tz );
323        void setTrans( const Vec3f& v );
324        void setTrans( const Vec3d& v );
325       
326        inline Vec3d getTrans() const { return Vec3d(_mat[3][0],_mat[3][1],_mat[3][2]); }
327       
328        inline Vec3d getScale() const {
329          Vec3d x_vec(_mat[0][0],_mat[1][0],_mat[2][0]);
330          Vec3d y_vec(_mat[0][1],_mat[1][1],_mat[2][1]);
331          Vec3d z_vec(_mat[0][2],_mat[1][2],_mat[2][2]);
332          return Vec3d(x_vec.length(), y_vec.length(), z_vec.length());
333        }
334       
335        /** apply a 3x3 transform of v*M[0..2,0..2]. */
336        inline static Vec3f transform3x3(const Vec3f& v,const Matrixf& m);
337
338        /** apply a 3x3 transform of v*M[0..2,0..2]. */
339        inline static Vec3d transform3x3(const Vec3d& v,const Matrixf& m);
340
341        /** apply a 3x3 transform of M[0..2,0..2]*v. */
342        inline static Vec3f transform3x3(const Matrixf& m,const Vec3f& v);
343
344        /** apply a 3x3 transform of M[0..2,0..2]*v. */
345        inline static Vec3d transform3x3(const Matrixf& m,const Vec3d& v);
346
347        // basic Matrixf multiplication, our workhorse methods.
348        void mult( const Matrixf&, const Matrixf& );
349        void preMult( const Matrixf& );
350        void postMult( const Matrixf& );
351
352        /** Optimized version of preMult(translate(v)); */
353        inline void preMultTranslate( const Vec3d& v );
354        inline void preMultTranslate( const Vec3f& v );
355        /** Optimized version of postMult(translate(v)); */
356        inline void postMultTranslate( const Vec3d& v );
357        inline void postMultTranslate( const Vec3f& v );
358
359        /** Optimized version of preMult(scale(v)); */
360        inline void preMultScale( const Vec3d& v );
361        inline void preMultScale( const Vec3f& v );
362        /** Optimized version of postMult(scale(v)); */
363        inline void postMultScale( const Vec3d& v );
364        inline void postMultScale( const Vec3f& v );
365
366        /** Optimized version of preMult(rotate(q)); */
367        inline void preMultRotate( const Quat& q );
368        /** Optimized version of postMult(rotate(q)); */
369        inline void postMultRotate( const Quat& q );
370
371        inline void operator *= ( const Matrixf& other )
372        {    if( this == &other ) {
373                Matrixf temp(other);
374                postMult( temp );
375            }
376            else postMult( other );
377        }
378
379        inline Matrixf operator * ( const Matrixf &m ) const
380        {
381            osg::Matrixf r;
382            r.mult(*this,m);
383            return  r;
384        }
385
386    protected:
387        value_type _mat[4][4];
388
389};
390
391class RefMatrixf : public Object, public Matrixf
392{
393    public:
394   
395        RefMatrixf():Object(false), Matrixf() {}
396        RefMatrixf( const Matrixf& other) : Object(false), Matrixf(other) {}
397        RefMatrixf( const Matrixd& other) : Object(false), Matrixf(other) {}
398        RefMatrixf( const RefMatrixf& other) : Object(other), Matrixf(other) {}
399        explicit RefMatrixf( Matrixf::value_type const * const def ):Object(false), Matrixf(def) {}
400        RefMatrixf( Matrixf::value_type a00, Matrixf::value_type a01, Matrixf::value_type a02, Matrixf::value_type a03,
401            Matrixf::value_type a10, Matrixf::value_type a11, Matrixf::value_type a12, Matrixf::value_type a13,
402            Matrixf::value_type a20, Matrixf::value_type a21, Matrixf::value_type a22, Matrixf::value_type a23,
403            Matrixf::value_type a30, Matrixf::value_type a31, Matrixf::value_type a32, Matrixf::value_type a33):
404            Object(false),
405            Matrixf(a00, a01, a02, a03,
406                   a10, a11, a12, a13,
407                   a20, a21, a22, a23,
408                   a30, a31, a32, a33) {}
409
410        virtual Object* cloneType() const { return new RefMatrixf(); }
411        virtual Object* clone(const CopyOp&) const { return new RefMatrixf(*this); }
412        virtual bool isSameKindAs(const Object* obj) const { return dynamic_cast<const RefMatrixf*>(obj)!=NULL; }
413        virtual const char* libraryName() const { return "osg"; }
414        virtual const char* className() const { return "Matrix"; }
415       
416       
417    protected:
418   
419        virtual ~RefMatrixf() {}
420};
421
422
423//static utility methods
424inline Matrixf Matrixf::identity(void)
425{
426    Matrixf m;
427    m.makeIdentity();
428    return m;
429}
430
431inline Matrixf Matrixf::scale(value_type sx, value_type sy, value_type sz)
432{
433    Matrixf m;
434    m.makeScale(sx,sy,sz);
435    return m;
436}
437
438inline Matrixf Matrixf::scale(const Vec3f& v )
439{
440    return scale(v.x(), v.y(), v.z() );
441}
442
443inline Matrixf Matrixf::scale(const Vec3d& v )
444{
445    return scale(v.x(), v.y(), v.z() );
446}
447
448inline Matrixf Matrixf::translate(value_type tx, value_type ty, value_type tz)
449{
450    Matrixf m;
451    m.makeTranslate(tx,ty,tz);
452    return m;
453}
454
455inline Matrixf Matrixf::translate(const Vec3f& v )
456{
457    return translate(v.x(), v.y(), v.z() );
458}
459
460inline Matrixf Matrixf::translate(const Vec3d& v )
461{
462    return translate(v.x(), v.y(), v.z() );
463}
464
465inline Matrixf Matrixf::rotate( const Quat& q )
466{
467    return Matrixf(q);
468}
469inline Matrixf Matrixf::rotate(value_type angle, value_type x, value_type y, value_type z )
470{
471    Matrixf m;
472    m.makeRotate(angle,x,y,z);
473    return m;
474}
475inline Matrixf Matrixf::rotate(value_type angle, const Vec3f& axis )
476{
477    Matrixf m;
478    m.makeRotate(angle,axis);
479    return m;
480}
481inline Matrixf Matrixf::rotate(value_type angle, const Vec3d& axis )
482{
483    Matrixf m;
484    m.makeRotate(angle,axis);
485    return m;
486}
487inline Matrixf Matrixf::rotate( value_type angle1, const Vec3f& axis1,
488                              value_type angle2, const Vec3f& axis2,
489                              value_type angle3, const Vec3f& axis3)
490{
491    Matrixf m;
492    m.makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
493    return m;
494}
495inline Matrixf Matrixf::rotate( value_type angle1, const Vec3d& axis1,
496                              value_type angle2, const Vec3d& axis2,
497                              value_type angle3, const Vec3d& axis3)
498{
499    Matrixf m;
500    m.makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
501    return m;
502}
503inline Matrixf Matrixf::rotate(const Vec3f& from, const Vec3f& to )
504{
505    Matrixf m;
506    m.makeRotate(from,to);
507    return m;
508}
509inline Matrixf Matrixf::rotate(const Vec3d& from, const Vec3d& to )
510{
511    Matrixf m;
512    m.makeRotate(from,to);
513    return m;
514}
515
516inline Matrixf Matrixf::inverse( const Matrixf& matrix)
517{
518    Matrixf m;
519    m.invert(matrix);
520    return m;
521}
522
523inline Matrixf Matrixf::orthoNormal(const Matrixf& matrix)
524{
525  Matrixf m;
526  m.orthoNormalize(matrix);
527  return m;
528}
529
530inline Matrixf Matrixf::ortho(double left, double right,
531                            double bottom, double top,
532                            double zNear, double zFar)
533{
534    Matrixf m;
535    m.makeOrtho(left,right,bottom,top,zNear,zFar);
536    return m;
537}
538
539inline Matrixf Matrixf::ortho2D(double left, double right,
540                              double bottom, double top)
541{
542    Matrixf m;
543    m.makeOrtho2D(left,right,bottom,top);
544    return m;
545}
546
547inline Matrixf Matrixf::frustum(double left, double right,
548                              double bottom, double top,
549                              double zNear, double zFar)
550{
551    Matrixf m;
552    m.makeFrustum(left,right,bottom,top,zNear,zFar);
553    return m;
554}
555
556inline Matrixf Matrixf::perspective(double fovy,double aspectRatio,
557                                  double zNear, double zFar)
558{
559    Matrixf m;
560    m.makePerspective(fovy,aspectRatio,zNear,zFar);
561    return m;
562}
563
564inline Matrixf Matrixf::lookAt(const Vec3f& eye,const Vec3f& center,const Vec3f& up)
565{
566    Matrixf m;
567    m.makeLookAt(eye,center,up);
568    return m;
569}
570
571inline Matrixf Matrixf::lookAt(const Vec3d& eye,const Vec3d& center,const Vec3d& up)
572{
573    Matrixf m;
574    m.makeLookAt(eye,center,up);
575    return m;
576}
577
578inline Vec3f Matrixf::postMult( const Vec3f& v ) const
579{
580    value_type d = 1.0f/(_mat[3][0]*v.x()+_mat[3][1]*v.y()+_mat[3][2]*v.z()+_mat[3][3]) ;
581    return Vec3f( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3])*d,
582        (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3])*d,
583        (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3])*d) ;
584}
585inline Vec3d Matrixf::postMult( const Vec3d& v ) const
586{
587    value_type d = 1.0f/(_mat[3][0]*v.x()+_mat[3][1]*v.y()+_mat[3][2]*v.z()+_mat[3][3]) ;
588    return Vec3d( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3])*d,
589        (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3])*d,
590        (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3])*d) ;
591}
592
593inline Vec3f Matrixf::preMult( const Vec3f& v ) const
594{
595    value_type d = 1.0f/(_mat[0][3]*v.x()+_mat[1][3]*v.y()+_mat[2][3]*v.z()+_mat[3][3]) ;
596    return Vec3f( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0])*d,
597        (_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1])*d,
598        (_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2])*d);
599}
600inline Vec3d Matrixf::preMult( const Vec3d& v ) const
601{
602    value_type d = 1.0f/(_mat[0][3]*v.x()+_mat[1][3]*v.y()+_mat[2][3]*v.z()+_mat[3][3]) ;
603    return Vec3d( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0])*d,
604        (_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1])*d,
605        (_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2])*d);
606}
607
608inline Vec4f Matrixf::postMult( const Vec4f& v ) const
609{
610    return Vec4f( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3]*v.w()),
611        (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3]*v.w()),
612        (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3]*v.w()),
613        (_mat[3][0]*v.x() + _mat[3][1]*v.y() + _mat[3][2]*v.z() + _mat[3][3]*v.w())) ;
614}
615inline Vec4d Matrixf::postMult( const Vec4d& v ) const
616{
617    return Vec4d( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3]*v.w()),
618        (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3]*v.w()),
619        (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3]*v.w()),
620        (_mat[3][0]*v.x() + _mat[3][1]*v.y() + _mat[3][2]*v.z() + _mat[3][3]*v.w())) ;
621}
622
623inline Vec4f Matrixf::preMult( const Vec4f& v ) const
624{
625    return Vec4f( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0]*v.w()),
626        (_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1]*v.w()),
627        (_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2]*v.w()),
628        (_mat[0][3]*v.x() + _mat[1][3]*v.y() + _mat[2][3]*v.z() + _mat[3][3]*v.w()));
629}
630inline Vec4d Matrixf::preMult( const Vec4d& v ) const
631{
632    return Vec4d( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0]*v.w()),
633        (_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1]*v.w()),
634        (_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2]*v.w()),
635        (_mat[0][3]*v.x() + _mat[1][3]*v.y() + _mat[2][3]*v.z() + _mat[3][3]*v.w()));
636}
637inline Vec3f Matrixf::transform3x3(const Vec3f& v,const Matrixf& m)
638{
639    return Vec3f( (m._mat[0][0]*v.x() + m._mat[1][0]*v.y() + m._mat[2][0]*v.z()),
640                 (m._mat[0][1]*v.x() + m._mat[1][1]*v.y() + m._mat[2][1]*v.z()),
641                 (m._mat[0][2]*v.x() + m._mat[1][2]*v.y() + m._mat[2][2]*v.z()));
642}
643inline Vec3d Matrixf::transform3x3(const Vec3d& v,const Matrixf& m)
644{
645    return Vec3d( (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}
649
650inline Vec3f Matrixf::transform3x3(const Matrixf& m,const Vec3f& v)
651{
652    return Vec3f( (m._mat[0][0]*v.x() + m._mat[0][1]*v.y() + m._mat[0][2]*v.z()),
653                 (m._mat[1][0]*v.x() + m._mat[1][1]*v.y() + m._mat[1][2]*v.z()),
654                 (m._mat[2][0]*v.x() + m._mat[2][1]*v.y() + m._mat[2][2]*v.z()) ) ;
655}
656inline Vec3d Matrixf::transform3x3(const Matrixf& m,const Vec3d& v)
657{
658    return Vec3d( (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}
662
663inline void Matrixf::preMultTranslate( const Vec3d& v )
664{
665    for (unsigned i = 0; i < 3; ++i)
666    {
667        double tmp = v[i];
668        if (tmp == 0)
669            continue;
670        _mat[3][0] += tmp*_mat[i][0];
671        _mat[3][1] += tmp*_mat[i][1];
672        _mat[3][2] += tmp*_mat[i][2];
673        _mat[3][3] += tmp*_mat[i][3];
674    }
675}
676
677inline void Matrixf::preMultTranslate( const Vec3f& v )
678{
679    for (unsigned i = 0; i < 3; ++i)
680    {
681        float tmp = v[i];
682        if (tmp == 0)
683            continue;
684        _mat[3][0] += tmp*_mat[i][0];
685        _mat[3][1] += tmp*_mat[i][1];
686        _mat[3][2] += tmp*_mat[i][2];
687        _mat[3][3] += tmp*_mat[i][3];
688    }
689}
690
691inline void Matrixf::postMultTranslate( const Vec3d& v )
692{
693    for (unsigned i = 0; i < 3; ++i)
694    {
695        double tmp = v[i];
696        if (tmp == 0)
697            continue;
698        _mat[0][i] += tmp*_mat[0][3];
699        _mat[1][i] += tmp*_mat[1][3];
700        _mat[2][i] += tmp*_mat[2][3];
701        _mat[3][i] += tmp*_mat[3][3];
702    }
703}
704
705inline void Matrixf::postMultTranslate( const Vec3f& v )
706{
707    for (unsigned i = 0; i < 3; ++i)
708    {
709        float tmp = v[i];
710        if (tmp == 0)
711            continue;
712        _mat[0][i] += tmp*_mat[0][3];
713        _mat[1][i] += tmp*_mat[1][3];
714        _mat[2][i] += tmp*_mat[2][3];
715        _mat[3][i] += tmp*_mat[3][3];
716    }
717}
718
719inline void Matrixf::preMultScale( const Vec3d& v )
720{
721    _mat[0][0] *= v[0]; _mat[0][1] *= v[0]; _mat[0][2] *= v[0]; _mat[0][3] *= v[0];
722    _mat[1][0] *= v[1]; _mat[1][1] *= v[1]; _mat[1][2] *= v[1]; _mat[1][3] *= v[1];
723    _mat[2][0] *= v[2]; _mat[2][1] *= v[2]; _mat[2][2] *= v[2]; _mat[2][3] *= v[2];
724}
725
726inline void Matrixf::preMultScale( const Vec3f& v )
727{
728    _mat[0][0] *= v[0]; _mat[0][1] *= v[0]; _mat[0][2] *= v[0]; _mat[0][3] *= v[0];
729    _mat[1][0] *= v[1]; _mat[1][1] *= v[1]; _mat[1][2] *= v[1]; _mat[1][3] *= v[1];
730    _mat[2][0] *= v[2]; _mat[2][1] *= v[2]; _mat[2][2] *= v[2]; _mat[2][3] *= v[2];
731}
732
733inline void Matrixf::postMultScale( const Vec3d& v )
734{
735    _mat[0][0] *= v[0]; _mat[1][0] *= v[0]; _mat[2][0] *= v[0]; _mat[3][0] *= v[0];
736    _mat[0][1] *= v[1]; _mat[1][1] *= v[1]; _mat[2][1] *= v[1]; _mat[3][1] *= v[1];
737    _mat[0][2] *= v[2]; _mat[1][2] *= v[2]; _mat[2][2] *= v[2]; _mat[3][2] *= v[2];
738}
739
740inline void Matrixf::postMultScale( const Vec3f& v )
741{
742    _mat[0][0] *= v[0]; _mat[1][0] *= v[0]; _mat[2][0] *= v[0]; _mat[3][0] *= v[0];
743    _mat[0][1] *= v[1]; _mat[1][1] *= v[1]; _mat[2][1] *= v[1]; _mat[3][1] *= v[1];
744    _mat[0][2] *= v[2]; _mat[1][2] *= v[2]; _mat[2][2] *= v[2]; _mat[3][2] *= v[2];
745}
746
747
748inline void Matrixf::preMultRotate( const Quat& q )
749{
750    if (q.zeroRotation())
751        return;
752    Matrixf r;
753    r.setRotate(q);
754    preMult(r);
755}
756
757inline void Matrixf::postMultRotate( const Quat& q )
758{
759    if (q.zeroRotation())
760        return;
761    Matrixf r;
762    r.setRotate(q);
763    postMult(r);
764}
765
766inline Vec3f operator* (const Vec3f& v, const Matrixf& m )
767{
768    return m.preMult(v);
769}
770inline Vec3d operator* (const Vec3d& v, const Matrixf& m )
771{
772    return m.preMult(v);
773}
774inline Vec4f operator* (const Vec4f& v, const Matrixf& m )
775{
776    return m.preMult(v);
777}
778inline Vec4d operator* (const Vec4d& v, const Matrixf& m )
779{
780    return m.preMult(v);
781}
782
783inline Vec3f Matrixf::operator* (const Vec3f& v) const
784{
785    return postMult(v);
786}
787inline Vec3d Matrixf::operator* (const Vec3d& v) const
788{
789    return postMult(v);
790}
791inline Vec4f Matrixf::operator* (const Vec4f& v) const
792{
793    return postMult(v);
794}
795inline Vec4d Matrixf::operator* (const Vec4d& v) const
796{
797    return postMult(v);
798}
799
800
801} //namespace osg
802
803
804#endif
Note: See TracBrowser for help on using the browser.