Elbeem: fix memory leak and add guarded allocator directives
[blender.git] / intern / elbeem / intern / ntl_matrices.h
1 /** \file elbeem/intern/ntl_matrices.h
2  *  \ingroup elbeem
3  */
4
5 /******************************************************************************
6  *
7  * El'Beem - Free Surface Fluid Simulation with the Lattice Boltzmann Method
8  * Copyright 2003-2006 Nils Thuerey
9  *
10  * Basic matrix utility include file
11  *
12  *****************************************************************************/
13 #ifndef NTL_MATRICES_H
14
15 #include "ntl_vector3dim.h"
16
17 #ifdef WITH_CXX_GUARDEDALLOC
18 #  include "MEM_guardedalloc.h"
19 #endif
20
21 // The basic vector class
22 template<class Scalar>
23 class ntlMatrix4x4
24 {
25 public:
26   // Constructor
27   inline ntlMatrix4x4(void );
28   // Copy-Constructor
29   inline ntlMatrix4x4(const ntlMatrix4x4<Scalar> &v );
30   // construct a vector from one Scalar
31   inline ntlMatrix4x4(Scalar);
32   // construct a vector from three Scalars
33   inline ntlMatrix4x4(Scalar, Scalar, Scalar);
34
35   // Assignment operator
36   inline const ntlMatrix4x4<Scalar>& operator=  (const ntlMatrix4x4<Scalar>& v);
37   // Assignment operator
38   inline const ntlMatrix4x4<Scalar>& operator=  (Scalar s);
39   // Assign and add operator
40   inline const ntlMatrix4x4<Scalar>& operator+= (const ntlMatrix4x4<Scalar>& v);
41   // Assign and add operator
42   inline const ntlMatrix4x4<Scalar>& operator+= (Scalar s);
43   // Assign and sub operator
44   inline const ntlMatrix4x4<Scalar>& operator-= (const ntlMatrix4x4<Scalar>& v);
45   // Assign and sub operator
46   inline const ntlMatrix4x4<Scalar>& operator-= (Scalar s);
47   // Assign and mult operator
48   inline const ntlMatrix4x4<Scalar>& operator*= (const ntlMatrix4x4<Scalar>& v);
49   // Assign and mult operator
50   inline const ntlMatrix4x4<Scalar>& operator*= (Scalar s);
51   // Assign and div operator
52   inline const ntlMatrix4x4<Scalar>& operator/= (const ntlMatrix4x4<Scalar>& v);
53   // Assign and div operator
54   inline const ntlMatrix4x4<Scalar>& operator/= (Scalar s);
55
56   
57   // unary operator
58   inline ntlMatrix4x4<Scalar> operator- () const;
59
60   // binary operator add
61   inline ntlMatrix4x4<Scalar> operator+ (const ntlMatrix4x4<Scalar>&) const;
62   // binary operator add
63   inline ntlMatrix4x4<Scalar> operator+ (Scalar) const;
64   // binary operator sub
65   inline ntlMatrix4x4<Scalar> operator- (const ntlMatrix4x4<Scalar>&) const;
66   // binary operator sub
67   inline ntlMatrix4x4<Scalar> operator- (Scalar) const;
68   // binary operator mult
69   inline ntlMatrix4x4<Scalar> operator* (const ntlMatrix4x4<Scalar>&) const;
70   // binary operator mult
71   inline ntlVector3Dim<Scalar> operator* (const ntlVector3Dim<Scalar>&) const;
72   // binary operator mult
73   inline ntlMatrix4x4<Scalar> operator* (Scalar) const;
74   // binary operator div
75   inline ntlMatrix4x4<Scalar> operator/ (Scalar) const;
76
77         // init function
78         //! init identity matrix
79         inline void initId();
80         //! init rotation matrix
81         inline void initTranslation(Scalar x, Scalar y, Scalar z);
82         //! init rotation matrix
83         inline void initRotationX(Scalar rot);
84         inline void initRotationY(Scalar rot);
85         inline void initRotationZ(Scalar rot);
86         inline void initRotationXYZ(Scalar rotx,Scalar roty, Scalar rotz);
87         //! init scaling matrix
88         inline void initScaling(Scalar scale);
89         inline void initScaling(Scalar x, Scalar y, Scalar z);
90         //! from 16 value array (init id if all 0)
91         inline void initArrayCheck(Scalar *array);
92
93         //! decompose matrix
94         void decompose(ntlVector3Dim<Scalar> &trans,ntlVector3Dim<Scalar> &scale,ntlVector3Dim<Scalar> &rot,ntlVector3Dim<Scalar> &shear);
95
96         //! public to avoid [][] operators
97   Scalar value[4][4];  //< Storage of vector values
98
99
100 protected:
101
102 private:
103 #ifdef WITH_CXX_GUARDEDALLOC
104         MEM_CXX_CLASS_ALLOC_FUNCS("ELBEEM:ntlMatrix4x4")
105 #endif
106 };
107
108
109
110 //------------------------------------------------------------------------------
111 // TYPEDEFS
112 //------------------------------------------------------------------------------
113
114 // a 3D vector for graphics output, typically float?
115 //typedef ntlMatrix4x4<float>  ntlVec3Gfx; 
116
117 //typedef ntlMatrix4x4<double>  ntlMat4d; 
118 typedef ntlMatrix4x4<double>  ntlMat4d; 
119
120 // a 3D vector with single precision
121 typedef ntlMatrix4x4<float>   ntlMat4f; 
122
123 // a 3D vector with grafix precision
124 typedef ntlMatrix4x4<gfxReal>   ntlMat4Gfx;
125
126 // a 3D integer vector
127 typedef ntlMatrix4x4<int>     ntlMat4i; 
128
129
130
131
132
133 //------------------------------------------------------------------------------
134 // STREAM FUNCTIONS
135 //------------------------------------------------------------------------------
136
137
138
139 /*************************************************************************
140   Outputs the object in human readable form using the format
141   [x,y,z]
142   */
143 template<class Scalar>
144 std::ostream&
145 operator<<( std::ostream& os, const ntlMatrix4x4<Scalar>& m )
146 {
147         for(int i=0; i<4; i++) {
148         os << '|' << m.value[i][0] << ", " << m.value[i][1] << ", " << m.value[i][2] << ", " << m.value[i][3] << '|';
149         }
150   return os;
151 }
152
153
154
155 /*************************************************************************
156   Reads the contents of the object from a stream using the same format
157   as the output operator.
158   */
159 template<class Scalar>
160 std::istream&
161 operator>>( std::istream& is, ntlMatrix4x4<Scalar>& m )
162 {
163   char c;
164   char dummy[3];
165   
166         for(int i=0; i<4; i++) {
167         is >> c >> m.value[i][0] >> dummy >> m.value[i][1] >> dummy >> m.value[i][2] >> dummy >> m.value[i][3] >> c;
168         }
169   return is;
170 }
171
172
173 //------------------------------------------------------------------------------
174 // VECTOR inline FUNCTIONS
175 //------------------------------------------------------------------------------
176
177
178
179 /*************************************************************************
180   Constructor.
181   */
182 template<class Scalar>
183 inline ntlMatrix4x4<Scalar>::ntlMatrix4x4( void )
184 {
185 #ifdef MATRIX_INIT_ZERO
186         for(int i=0; i<4; i++) {
187                 for(int j=0; j<4; j++) {
188                 value[i][j] = 0.0;
189                 }
190         }
191 #endif
192 }
193
194
195
196 /*************************************************************************
197   Copy-Constructor.
198   */
199 template<class Scalar>
200 inline ntlMatrix4x4<Scalar>::ntlMatrix4x4( const ntlMatrix4x4<Scalar> &v )
201 {
202   value[0][0] = v.value[0][0]; value[0][1] = v.value[0][1]; value[0][2] = v.value[0][2]; value[0][3] = v.value[0][3];
203   value[1][0] = v.value[1][0]; value[1][1] = v.value[1][1]; value[1][2] = v.value[1][2]; value[1][3] = v.value[1][3];
204   value[2][0] = v.value[2][0]; value[2][1] = v.value[2][1]; value[2][2] = v.value[2][2]; value[2][3] = v.value[2][3];
205   value[3][0] = v.value[3][0]; value[3][1] = v.value[3][1]; value[3][2] = v.value[3][2]; value[3][3] = v.value[3][3];
206 }
207
208
209
210 /*************************************************************************
211   Constructor for a vector from a single Scalar. All components of
212   the vector get the same value.
213   \param s The value to set
214   \return The new vector
215   */
216 template<class Scalar>
217 inline ntlMatrix4x4<Scalar>::ntlMatrix4x4(Scalar s )
218 {
219         for(int i=0; i<4; i++) {
220                 for(int j=0; j<4; j++) {
221                 value[i][j] = s;
222                 }
223         }
224 }
225
226
227
228 /*************************************************************************
229   Copy a ntlMatrix4x4 componentwise.
230   \param v vector with values to be copied
231   \return Reference to self
232   */
233 template<class Scalar>
234 inline const ntlMatrix4x4<Scalar>&
235 ntlMatrix4x4<Scalar>::operator=( const ntlMatrix4x4<Scalar> &v )
236 {
237   value[0][0] = v.value[0][0]; value[0][1] = v.value[0][1]; value[0][2] = v.value[0][2]; value[0][3] = v.value[0][3];
238   value[1][0] = v.value[1][0]; value[1][1] = v.value[1][1]; value[1][2] = v.value[1][2]; value[1][3] = v.value[1][3];
239   value[2][0] = v.value[2][0]; value[2][1] = v.value[2][1]; value[2][2] = v.value[2][2]; value[2][3] = v.value[2][3];
240   value[3][0] = v.value[3][0]; value[3][1] = v.value[3][1]; value[3][2] = v.value[3][2]; value[3][3] = v.value[3][3];
241   return *this;
242 }
243
244
245 /*************************************************************************
246   Copy a Scalar to each component.
247   \param s The value to copy
248   \return Reference to self
249   */
250 template<class Scalar>
251 inline const ntlMatrix4x4<Scalar>&
252 ntlMatrix4x4<Scalar>::operator=(Scalar s)
253 {
254         for(int i=0; i<4; i++) {
255                 for(int j=0; j<4; j++) {
256                 value[i][j] = s;
257                 }
258         }
259   return *this;
260 }
261
262
263 /*************************************************************************
264   Add another ntlMatrix4x4 componentwise.
265   \param v vector with values to be added
266   \return Reference to self
267   */
268 template<class Scalar>
269 inline const ntlMatrix4x4<Scalar>&
270 ntlMatrix4x4<Scalar>::operator+=( const ntlMatrix4x4<Scalar> &v )
271 {
272   value[0][0] += v.value[0][0]; value[0][1] += v.value[0][1]; value[0][2] += v.value[0][2]; value[0][3] += v.value[0][3];
273   value[1][0] += v.value[1][0]; value[1][1] += v.value[1][1]; value[1][2] += v.value[1][2]; value[1][3] += v.value[1][3];
274   value[2][0] += v.value[2][0]; value[2][1] += v.value[2][1]; value[2][2] += v.value[2][2]; value[2][3] += v.value[2][3];
275   value[3][0] += v.value[3][0]; value[3][1] += v.value[3][1]; value[3][2] += v.value[3][2]; value[3][3] += v.value[3][3];
276   return *this;
277 }
278
279
280 /*************************************************************************
281   Add a Scalar value to each component.
282   \param s Value to add
283   \return Reference to self
284   */
285 template<class Scalar>
286 inline const ntlMatrix4x4<Scalar>&
287 ntlMatrix4x4<Scalar>::operator+=(Scalar s)
288 {
289         for(int i=0; i<4; i++) {
290                 for(int j=0; j<4; j++) {
291                 value[i][j] += s;
292                 }
293         }
294   return *this;
295 }
296
297
298 /*************************************************************************
299   Subtract another vector componentwise.
300   \param v vector of values to subtract
301   \return Reference to self
302   */
303 template<class Scalar>
304 inline const ntlMatrix4x4<Scalar>&
305 ntlMatrix4x4<Scalar>::operator-=( const ntlMatrix4x4<Scalar> &v )
306 {
307   value[0][0] -= v.value[0][0]; value[0][1] -= v.value[0][1]; value[0][2] -= v.value[0][2]; value[0][3] -= v.value[0][3];
308   value[1][0] -= v.value[1][0]; value[1][1] -= v.value[1][1]; value[1][2] -= v.value[1][2]; value[1][3] -= v.value[1][3];
309   value[2][0] -= v.value[2][0]; value[2][1] -= v.value[2][1]; value[2][2] -= v.value[2][2]; value[2][3] -= v.value[2][3];
310   value[3][0] -= v.value[3][0]; value[3][1] -= v.value[3][1]; value[3][2] -= v.value[3][2]; value[3][3] -= v.value[3][3];
311   return *this;
312 }
313
314
315 /*************************************************************************
316   Subtract a Scalar value from each component.
317   \param s Value to subtract
318   \return Reference to self
319   */
320 template<class Scalar>
321 inline const ntlMatrix4x4<Scalar>&
322 ntlMatrix4x4<Scalar>::operator-=(Scalar s)
323 {
324         for(int i=0; i<4; i++) {
325                 for(int j=0; j<4; j++) {
326                 value[i][j] -= s;
327                 }
328         }
329   return *this;
330 }
331
332
333 /*************************************************************************
334   Multiply with another vector componentwise.
335   \param v vector of values to multiply with
336   \return Reference to self
337   */
338 template<class Scalar>
339 inline const ntlMatrix4x4<Scalar>&
340 ntlMatrix4x4<Scalar>::operator*=( const ntlMatrix4x4<Scalar> &v )
341 {
342         ntlMatrix4x4<Scalar> nv(0.0);
343         for(int i=0; i<4; i++) {
344                 for(int j=0; j<4; j++) {
345
346                         for(int k=0;k<4;k++)
347                                 nv.value[i][j] += (value[i][k] * v.value[k][j]);
348                 }
349         }
350   *this = nv;
351   return *this;
352 }
353
354
355 /*************************************************************************
356   Multiply each component with a Scalar value.
357   \param s Value to multiply with
358   \return Reference to self
359   */
360 template<class Scalar>
361 inline const ntlMatrix4x4<Scalar>&
362 ntlMatrix4x4<Scalar>::operator*=(Scalar s)
363 {
364         for(int i=0; i<4; i++) {
365                 for(int j=0; j<4; j++) {
366                 value[i][j] *= s;
367                 }
368         }
369   return *this;
370 }
371
372
373
374 /*************************************************************************
375   Divide each component by a Scalar value.
376   \param s Value to divide by
377   \return Reference to self
378   */
379 template<class Scalar>
380 inline const ntlMatrix4x4<Scalar>&
381 ntlMatrix4x4<Scalar>::operator/=(Scalar s)
382 {
383         for(int i=0; i<4; i++) {
384                 for(int j=0; j<4; j++) {
385                 value[i][j] /= s;
386                 }
387         }
388   return *this;
389 }
390
391
392 //------------------------------------------------------------------------------
393 // unary operators
394 //------------------------------------------------------------------------------
395
396
397 /*************************************************************************
398   Build componentwise the negative this vector.
399   \return The new (negative) vector
400   */
401 template<class Scalar>
402 inline ntlMatrix4x4<Scalar>
403 ntlMatrix4x4<Scalar>::operator-() const
404 {
405         ntlMatrix4x4<Scalar> nv;
406         for(int i=0; i<4; i++) {
407                 for(int j=0; j<4; j++) {
408                 nv[i][j] = -value[i][j];
409                 }
410         }
411   return nv;
412 }
413
414
415
416 //------------------------------------------------------------------------------
417 // binary operators
418 //------------------------------------------------------------------------------
419
420
421 /*************************************************************************
422   Build a vector with another vector added componentwise.
423   \param v The second vector to add
424   \return The sum vector
425   */
426 template<class Scalar>
427 inline ntlMatrix4x4<Scalar>
428 ntlMatrix4x4<Scalar>::operator+( const ntlMatrix4x4<Scalar> &v ) const
429 {
430         ntlMatrix4x4<Scalar> nv;
431         for(int i=0; i<4; i++) {
432                 for(int j=0; j<4; j++) {
433                 nv[i][j] = value[i][j] + v.value[i][j];
434                 }
435         }
436   return nv;
437 }
438
439
440 /*************************************************************************
441   Build a vector with a Scalar value added to each component.
442   \param s The Scalar value to add
443   \return The sum vector
444   */
445 template<class Scalar>
446 inline ntlMatrix4x4<Scalar>
447 ntlMatrix4x4<Scalar>::operator+(Scalar s) const
448 {
449         ntlMatrix4x4<Scalar> nv;
450         for(int i=0; i<4; i++) {
451                 for(int j=0; j<4; j++) {
452                 nv[i][j] = value[i][j] + s;
453                 }
454         }
455   return nv;
456 }
457
458
459 /*************************************************************************
460   Build a vector with another vector subtracted componentwise.
461   \param v The second vector to subtract
462   \return The difference vector
463   */
464 template<class Scalar>
465 inline ntlMatrix4x4<Scalar>
466 ntlMatrix4x4<Scalar>::operator-( const ntlMatrix4x4<Scalar> &v ) const
467 {
468         ntlMatrix4x4<Scalar> nv;
469         for(int i=0; i<4; i++) {
470                 for(int j=0; j<4; j++) {
471                 nv[i][j] = value[i][j] - v.value[i][j];
472                 }
473         }
474   return nv;
475 }
476
477
478 /*************************************************************************
479   Build a vector with a Scalar value subtracted componentwise.
480   \param s The Scalar value to subtract
481   \return The difference vector
482   */
483 template<class Scalar>
484 inline ntlMatrix4x4<Scalar>
485 ntlMatrix4x4<Scalar>::operator-(Scalar s ) const
486 {
487         ntlMatrix4x4<Scalar> nv;
488         for(int i=0; i<4; i++) {
489                 for(int j=0; j<4; j++) {
490                 nv[i][j] = value[i][j] - s;
491                 }
492         }
493   return nv;
494 }
495
496
497
498 /*************************************************************************
499   Build a ntlMatrix4x4 with a Scalar value multiplied to each component.
500   \param s The Scalar value to multiply with
501   \return The product vector
502   */
503 template<class Scalar>
504 inline ntlMatrix4x4<Scalar>
505 ntlMatrix4x4<Scalar>::operator*(Scalar s) const
506 {
507         ntlMatrix4x4<Scalar> nv;
508         for(int i=0; i<4; i++) {
509                 for(int j=0; j<4; j++) {
510                 nv[i][j] = value[i][j] * s;
511                 }
512         }
513   return nv;
514 }
515
516
517
518
519 /*************************************************************************
520   Build a vector divided componentwise by a Scalar value.
521   \param s The Scalar value to divide by
522   \return The ratio vector
523   */
524 template<class Scalar>
525 inline ntlMatrix4x4<Scalar>
526 ntlMatrix4x4<Scalar>::operator/(Scalar s) const
527 {
528         ntlMatrix4x4<Scalar> nv;
529         for(int i=0; i<4; i++) {
530                 for(int j=0; j<4; j++) {
531                 nv[i][j] = value[i][j] / s;
532                 }
533         }
534   return nv;
535 }
536
537
538
539
540
541 /*************************************************************************
542   Build a vector with another vector multiplied by componentwise.
543   \param v The second vector to muliply with
544   \return The product vector
545   */
546 template<class Scalar>
547 inline ntlMatrix4x4<Scalar>
548 ntlMatrix4x4<Scalar>::operator*( const ntlMatrix4x4<Scalar>& v) const
549 {
550         ntlMatrix4x4<Scalar> nv(0.0);
551         for(int i=0; i<4; i++) {
552                 for(int j=0; j<4; j++) {
553
554                         for(int k=0;k<4;k++)
555                                 nv.value[i][j] += (value[i][k] * v.value[k][j]);
556                 }
557         }
558   return nv;
559 }
560
561
562 template<class Scalar>
563 inline ntlVector3Dim<Scalar>
564 ntlMatrix4x4<Scalar>::operator*( const ntlVector3Dim<Scalar>& v) const
565 {
566         ntlVector3Dim<Scalar> nvec(0.0);
567         for(int i=0; i<3; i++) {
568                 for(int j=0; j<3; j++) {
569                         nvec[i] += (v[j] * value[i][j]);
570                 }
571         }
572         // assume normalized w coord
573         for(int i=0; i<3; i++) {
574                 nvec[i] += (1.0 * value[i][3]);
575         }
576   return nvec;
577 }
578
579
580
581 //------------------------------------------------------------------------------
582 // Other helper functions
583 //------------------------------------------------------------------------------
584
585 //! init identity matrix
586 template<class Scalar>
587 inline void ntlMatrix4x4<Scalar>::initId()
588 {
589         (*this) = (Scalar)(0.0);
590         value[0][0] = 
591         value[1][1] = 
592         value[2][2] = 
593         value[3][3] = (Scalar)(1.0);
594 }
595
596 //! init rotation matrix
597 template<class Scalar>
598 inline void ntlMatrix4x4<Scalar>::initTranslation(Scalar x, Scalar y, Scalar z)
599 {
600         //(*this) = (Scalar)(0.0);
601         this->initId();
602         value[0][3] = x;
603         value[1][3] = y;
604         value[2][3] = z;
605 }
606
607 //! init rotation matrix
608 template<class Scalar>
609 inline void 
610 ntlMatrix4x4<Scalar>::initRotationX(Scalar rot)
611 {
612         double drot = (double)(rot/360.0*2.0*M_PI);
613         //? while(drot < 0.0) drot += (M_PI*2.0);
614
615         this->initId();
616         value[1][1] = (Scalar)  cos(drot);
617         value[1][2] = (Scalar)  sin(drot);
618         value[2][1] = (Scalar)(-sin(drot));
619         value[2][2] = (Scalar)  cos(drot);
620 }
621 template<class Scalar>
622 inline void 
623 ntlMatrix4x4<Scalar>::initRotationY(Scalar rot)
624 {
625         double drot = (double)(rot/360.0*2.0*M_PI);
626         //? while(drot < 0.0) drot += (M_PI*2.0);
627
628         this->initId();
629         value[0][0] = (Scalar)  cos(drot);
630         value[0][2] = (Scalar)(-sin(drot));
631         value[2][0] = (Scalar)  sin(drot);
632         value[2][2] = (Scalar)  cos(drot);
633 }
634 template<class Scalar>
635 inline void 
636 ntlMatrix4x4<Scalar>::initRotationZ(Scalar rot)
637 {
638         double drot = (double)(rot/360.0*2.0*M_PI);
639         //? while(drot < 0.0) drot += (M_PI*2.0);
640
641         this->initId();
642         value[0][0] = (Scalar)  cos(drot);
643         value[0][1] = (Scalar)  sin(drot);
644         value[1][0] = (Scalar)(-sin(drot));
645         value[1][1] = (Scalar)  cos(drot);
646 }
647 template<class Scalar>
648 inline void 
649 ntlMatrix4x4<Scalar>::initRotationXYZ( Scalar rotx, Scalar roty, Scalar rotz)
650 {
651         ntlMatrix4x4<Scalar> val;
652         ntlMatrix4x4<Scalar> rot;
653         this->initId();
654
655         // org
656         /*rot.initRotationX(rotx);
657         (*this) *= rot;
658         rot.initRotationY(roty);
659         (*this) *= rot;
660         rot.initRotationZ(rotz);
661         (*this) *= rot;
662         // org */
663
664         // blender
665         rot.initRotationZ(rotz);
666         (*this) *= rot;
667         rot.initRotationY(roty);
668         (*this) *= rot;
669         rot.initRotationX(rotx);
670         (*this) *= rot;
671         // blender */
672 }
673
674 //! init scaling matrix
675 template<class Scalar>
676 inline void 
677 ntlMatrix4x4<Scalar>::initScaling(Scalar scale)
678 {
679         this->initId();
680         value[0][0] = scale;
681         value[1][1] = scale;
682         value[2][2] = scale;
683 }
684 //! init scaling matrix
685 template<class Scalar>
686 inline void 
687 ntlMatrix4x4<Scalar>::initScaling(Scalar x, Scalar y, Scalar z)
688 {
689         this->initId();
690         value[0][0] = x;
691         value[1][1] = y;
692         value[2][2] = z;
693 }
694
695
696 //! from 16 value array (init id if all 0)
697 template<class Scalar>
698 inline void 
699 ntlMatrix4x4<Scalar>::initArrayCheck(Scalar *array)
700 {
701         bool allZero = true;
702         for(int i=0; i<4; i++) {
703                 for(int j=0; j<4; j++) {
704                         value[i][j] = array[i*4+j];
705                         if(array[i*4+j]!=0.0) allZero=false;
706                 }
707         }
708         if(allZero) this->initId();
709 }
710
711 //! decompose matrix
712 template<class Scalar>
713 void 
714 ntlMatrix4x4<Scalar>::decompose(ntlVector3Dim<Scalar> &trans,ntlVector3Dim<Scalar> &scale,ntlVector3Dim<Scalar> &rot,ntlVector3Dim<Scalar> &shear) {
715         ntlVec3Gfx row[3],temp;
716
717         for(int i = 0; i < 3; i++) {
718                 trans[i] = this->value[3][i];
719         }
720
721         for(int i = 0; i < 3; i++) {
722                 row[i][0] = this->value[i][0];
723                 row[i][1] = this->value[i][1];
724                 row[i][2] = this->value[i][2];
725         }
726
727         scale[0] = norm(row[0]);
728         normalize (row[0]);
729
730         shear[0] = dot(row[0], row[1]);
731         row[1][0] = row[1][0] - shear[0]*row[0][0];
732         row[1][1] = row[1][1] - shear[0]*row[0][1];
733         row[1][2] = row[1][2] - shear[0]*row[0][2];
734
735         scale[1] = norm(row[1]);
736         normalize (row[1]);
737
738         if(scale[1] != 0.0)
739                 shear[0] /= scale[1];
740
741         shear[1] = dot(row[0], row[2]);
742         row[2][0] = row[2][0] - shear[1]*row[0][0];
743         row[2][1] = row[2][1] - shear[1]*row[0][1];
744         row[2][2] = row[2][2] - shear[1]*row[0][2];
745
746         shear[2] = dot(row[1], row[2]);
747         row[2][0] = row[2][0] - shear[2]*row[1][0];
748         row[2][1] = row[2][1] - shear[2]*row[1][1];
749         row[2][2] = row[2][2] - shear[2]*row[1][2];
750
751         scale[2] = norm(row[2]);
752         normalize (row[2]);
753
754         if(scale[2] != 0.0) {
755                 shear[1] /= scale[2];
756                 shear[2] /= scale[2];
757         }
758
759         temp = cross(row[1], row[2]);
760         if(dot(row[0], temp) < 0.0) {
761                 for(int i = 0; i < 3; i++) {
762                         scale[i]  *= -1.0;
763                         row[i][0] *= -1.0;
764                         row[i][1] *= -1.0;
765                         row[i][2] *= -1.0;
766                 }
767         }
768
769         if(row[0][2] < -1.0) row[0][2] = -1.0;
770         if(row[0][2] > +1.0) row[0][2] = +1.0;
771
772         rot[1] = asin(-row[0][2]);
773
774         if(fabs(cos(rot[1])) > VECTOR_EPSILON) {
775                 rot[0] = atan2 (row[1][2], row[2][2]);
776                 rot[2] = atan2 (row[0][1], row[0][0]);
777         }
778         else {
779                 rot[0] = atan2 (row[1][0], row[1][1]);
780                 rot[2] = 0.0;
781         }
782
783         rot[0] = (180.0/M_PI)*rot[0];
784         rot[1] = (180.0/M_PI)*rot[1];
785         rot[2] = (180.0/M_PI)*rot[2];
786
787
788 #define NTL_MATRICES_H
789 #endif
790