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

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

Ran script to remove trailing spaces and tabs

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