a2baa75fb6926897e5347d9278966d94b83f952f
[occt.git] / src / gp / gp_Trsf.cxx
1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
3 //
4 // This file is part of Open CASCADE Technology software library.
5 //
6 // This library is free software; you can redistribute it and/or modify it under
7 // the terms of the GNU Lesser General Public License version 2.1 as published
8 // by the Free Software Foundation, with special exception defined in the file
9 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
10 // distribution for complete text of the license and disclaimer of any warranty.
11 //
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
14
15 // JCV 30/08/90 Modif passage version C++ 2.0 sur Sun
16 // JCV 1/10/90 Changement de nom du package vgeom -> gp
17 // JCV 4/10/90 codage sur la forme de la transformation shape,Scaling,negative
18 // JCV 10/12/90 Modif introduction des classes Mat et XYZ dans gp
19
20 #define No_Standard_OutOfRange
21
22 #include <gp_Trsf.ixx>
23 #include <gp.hxx>
24 #include <Standard_ConstructionError.hxx>
25
26
27 //=======================================================================
28 //function : gp_Trsf
29 //purpose  : Constructor from 2d
30 //=======================================================================
31
32 gp_Trsf::gp_Trsf (const gp_Trsf2d& T) : 
33 scale(T.ScaleFactor()),
34 shape(T.Form()),
35 loc(T.TranslationPart().X(),T.TranslationPart().Y(), 0.0)
36 {
37   const gp_Mat2d& M = T.HVectorialPart();
38   matrix(1,1) = M(1,1);
39   matrix(1,2) = M(1,2);
40   matrix(2,1) = M(2,1);
41   matrix(2,2) = M(2,2);
42   matrix(3,3) = 1.;
43   if (shape == gp_Ax1Mirror)
44   {
45     scale = 1;
46     matrix.Multiply(-1);
47   }
48 }
49
50 //=======================================================================
51 //function : SetMirror
52 //purpose  : 
53 //=======================================================================
54
55 void gp_Trsf::SetMirror (const gp_Ax1& A1) 
56 {
57   shape = gp_Ax1Mirror;
58   scale = 1;
59   loc = A1.Location().XYZ();
60   matrix.SetDot(A1.Direction().XYZ());
61   matrix.Multiply(-2);
62   matrix.SetDiagonal (matrix.Value (1,1) + 1,
63                       matrix.Value (2,2) + 1,
64                       matrix.Value (3,3) + 1);
65
66   loc.Multiply (matrix);
67   loc.Add (A1.Location().XYZ());
68   matrix.Multiply(-1);
69 }
70
71 //=======================================================================
72 //function : SetMirror
73 //purpose  : 
74 //=======================================================================
75
76 void gp_Trsf::SetMirror (const gp_Ax2& A2) 
77 {
78   shape = gp_Ax2Mirror;
79   scale = -1;
80   loc = A2.Location().XYZ();
81   matrix.SetDot(A2.Direction().XYZ());
82   matrix.Multiply(2);
83   matrix.SetDiagonal (matrix.Value (1,1) - 1,
84                       matrix.Value (2,2) - 1,
85                       matrix.Value (3,3) - 1);
86
87   loc.Multiply (matrix);
88   loc.Add (A2.Location().XYZ());
89
90
91 //=======================================================================
92 //function : SetRotation
93 //purpose  : 
94 //=======================================================================
95
96 void gp_Trsf::SetRotation (const gp_Ax1& A1,
97                            const Standard_Real Ang)
98 {
99   shape = gp_Rotation;
100   scale = 1.;
101   loc = A1.Location().XYZ();
102   matrix.SetRotation (A1.Direction().XYZ(), Ang);
103   loc.Reverse ();
104   loc.Multiply (matrix);
105   loc.Add (A1.Location().XYZ());
106 }
107
108 //=======================================================================
109 //function : SetRotation
110 //purpose  : 
111 //=======================================================================
112
113 void gp_Trsf::SetRotation (const gp_Quaternion& R)
114 {
115   shape = gp_Rotation;
116   scale = 1.;
117   loc.SetCoord (0., 0., 0.);
118   matrix = R.GetMatrix();
119 }
120
121 //=======================================================================
122 //function : SetScale
123 //purpose  : 
124 //=======================================================================
125
126 void gp_Trsf::SetScale (const gp_Pnt& P, const Standard_Real S)  
127 {
128   shape = gp_Scale;
129   scale = S;
130   loc = P.XYZ();
131   Standard_Real As = scale;
132   if (As < 0) As = - As;
133   Standard_ConstructionError_Raise_if
134     (As <= gp::Resolution(),"gp_Trsf::SetScaleFactor");
135   matrix.SetIdentity ();
136   loc.Multiply (1-S);
137 }
138
139 //=======================================================================
140 //function : SetTransformation
141 //purpose  : 
142 //=======================================================================
143
144 void gp_Trsf::SetTransformation (const gp_Ax3& FromA1,
145                                  const gp_Ax3& ToA2)
146 {
147   shape = gp_CompoundTrsf;
148   scale = 1.0;
149   // matrix from XOY  ToA2 :
150   matrix.SetCol (1, ToA2.XDirection().XYZ());
151   matrix.SetCol (2, ToA2.YDirection().XYZ());
152   matrix.SetCol (3, ToA2.Direction().XYZ());
153   loc = ToA2.Location().XYZ();
154   matrix.Transpose();
155   loc.Multiply (matrix);
156   loc.Reverse ();
157
158   // matrix FromA1 to XOY :
159   const gp_XYZ& xDir = FromA1.XDirection().XYZ();
160   const gp_XYZ& yDir = FromA1.YDirection().XYZ();
161   const gp_XYZ& zDir = FromA1.Direction().XYZ();
162
163   gp_Mat MA1 (xDir, yDir, zDir);
164   gp_XYZ MA1loc = FromA1.Location().XYZ();
165
166   // matrix * MA1 => FromA1 ToA2 :
167   MA1loc.Multiply (matrix);
168   loc.Add (MA1loc);
169   matrix.Multiply (MA1);
170 }
171
172 void gp_Trsf::SetTransformation (const gp_Ax3& A3) 
173 {
174   shape = gp_CompoundTrsf;
175   scale = 1.0;
176   loc = A3.Location().XYZ();
177   matrix.SetCols (A3.XDirection().XYZ(),
178                   A3.YDirection().XYZ(),
179                   A3. Direction().XYZ());
180   matrix.Transpose();
181   loc.Multiply (matrix);
182   loc.Reverse ();
183 }
184
185 //=======================================================================
186 //function : SetTransformation
187 //purpose  : 
188 //=======================================================================
189
190 void gp_Trsf::SetTransformation (const gp_Quaternion& R, const gp_Vec& T)
191 {
192   shape = gp_CompoundTrsf;
193   scale = 1.;
194   loc = T.XYZ();
195   matrix = R.GetMatrix();
196 }
197
198 //=======================================================================
199 //function : SetDisplacement
200 //purpose  : 
201 //=======================================================================
202
203 void gp_Trsf::SetDisplacement (const gp_Ax3& FromA1,
204                                const gp_Ax3& ToA2)
205 {
206   shape = gp_CompoundTrsf;
207   scale = 1.0;
208   // matrix from ToA2 to XOY :
209   matrix.SetCol (1, ToA2.XDirection().XYZ());
210   matrix.SetCol (2, ToA2.YDirection().XYZ());
211   matrix.SetCol (3, ToA2.Direction().XYZ());
212   loc = ToA2.Location().XYZ();
213   // matrix XOY to FromA1 :
214   const gp_XYZ& xDir = FromA1.XDirection().XYZ();
215   const gp_XYZ& yDir = FromA1.YDirection().XYZ();
216   const gp_XYZ& zDir = FromA1.Direction().XYZ();
217   gp_Mat MA1 (xDir, yDir, zDir);
218   MA1.Transpose();
219   gp_XYZ MA1loc = FromA1.Location().XYZ();
220   MA1loc.Multiply (MA1);
221   MA1loc.Reverse();
222   // matrix * MA1 
223   MA1loc.Multiply (matrix);
224   loc.Add (MA1loc);
225   matrix.Multiply (MA1);
226 }
227
228 //=======================================================================
229 //function : SetTranslationPart
230 //purpose  : 
231 //=======================================================================
232
233 void gp_Trsf::SetTranslationPart (const gp_Vec& V) {   
234
235   loc = V.XYZ();
236   Standard_Real X = loc.X();
237   if (X < 0) X = - X;
238   Standard_Real Y = loc.Y();
239   if (Y < 0) Y = - Y;
240   Standard_Real Z = loc.Z();
241   if (Z < 0) Z = - Z;
242   Standard_Boolean locnull =
243     (X <= gp::Resolution() && 
244      Y <= gp::Resolution() && 
245      Z <= gp::Resolution());
246
247   switch (shape) {
248
249   case gp_Identity :
250     if (!locnull) shape = gp_Translation;
251     break;
252
253   case gp_Translation :
254     if (locnull) shape = gp_Identity;
255     break;
256
257   case gp_Rotation :
258   case gp_PntMirror :
259   case gp_Ax1Mirror :
260   case gp_Ax2Mirror :
261   case gp_Scale :
262   case gp_CompoundTrsf :
263   case gp_Other :
264     break;
265   }
266 }
267
268 //=======================================================================
269 //function : SetScaleFactor
270 //purpose  : 
271 //=======================================================================
272
273 void gp_Trsf::SetScaleFactor (const Standard_Real S) 
274 {   
275   Standard_Real As = S;
276   if (As < 0) As = - As;
277   Standard_ConstructionError_Raise_if
278     (As <= gp::Resolution(),"gp_Trsf::SetScaleFactor");
279   scale = S;
280   As = scale - 1.;
281   if (As < 0) As = - As;
282   Standard_Boolean unit  = As <= gp::Resolution();
283   As = scale + 1.;
284   if (As < 0) As = - As;
285   Standard_Boolean munit = As <= gp::Resolution();
286   
287   switch (shape) {
288   case gp_Identity :
289   case gp_Translation :
290     if (!unit) shape = gp_Scale;
291     if (munit) shape = gp_PntMirror;
292     break;
293   case gp_Rotation :
294     if (!unit) shape = gp_CompoundTrsf;
295     break;
296   case gp_PntMirror :
297   case gp_Ax1Mirror :
298   case gp_Ax2Mirror :
299     if (!munit) shape = gp_Scale;
300     if (unit)   shape = gp_Identity;
301     break;
302   case gp_Scale :
303     if (unit)  shape = gp_Identity;
304     if (munit) shape = gp_PntMirror;
305     break;
306   case gp_CompoundTrsf :
307     break;
308   case gp_Other :
309     break;
310   }
311 }
312
313 //=======================================================================
314 //function : SetValues
315 //purpose  : 
316 // 06-01-1998 modified by PMN : On utilise TolDist pour evaluer si les coeffs 
317 //  sont nuls : c'est toujours mieux que gp::Resolution !
318 //=======================================================================
319
320 void gp_Trsf::SetValues(const Standard_Real a11, 
321                         const Standard_Real a12, 
322                         const Standard_Real a13, 
323                         const Standard_Real a14, 
324                         const Standard_Real a21, 
325                         const Standard_Real a22, 
326                         const Standard_Real a23, 
327                         const Standard_Real a24, 
328                         const Standard_Real a31, 
329                         const Standard_Real a32,
330                         const Standard_Real a33, 
331                         const Standard_Real a34, 
332 //                      const Standard_Real Tolang, 
333                         const Standard_Real , 
334                         const Standard_Real
335 #ifndef No_Exception
336                                             TolDist
337 #endif
338                        )
339 {
340   gp_XYZ col1(a11,a21,a31);
341   gp_XYZ col2(a12,a22,a32);
342   gp_XYZ col3(a13,a23,a33);
343   gp_XYZ col4(a14,a24,a34);
344   // compute the determinant
345   gp_Mat M(col1,col2,col3);
346   Standard_Real s = M.Determinant();
347   Standard_Real As = s;
348   if (As < 0) As = - As;
349   Standard_ConstructionError_Raise_if
350     (As < gp::Resolution(),"gp_Trsf::SeValues, null determinant");
351   if (s > 0)
352     s = Pow(s,1./3.);
353   else
354     s = -Pow(-s,1./3.);
355   M.Divide(s);
356   
357   // check if the matrix is a rotation matrix
358   // the transposition should be the invert.
359   gp_Mat TM(M);
360   TM.Transpose();
361   TM.Multiply(M);
362   //
363   // don t trust the initial values !
364   //
365   gp_Mat anIdentity ;
366   anIdentity.SetIdentity() ;
367   TM.Subtract(anIdentity);
368   As = TM.Value(1,1);
369   if (As < 0) As = - As;
370   Standard_ConstructionError_Raise_if
371     (As > TolDist,"gp_Trsf::SeValues, non uniform");
372   As = TM.Value(1,2);
373   if (As < 0) As = - As;
374   Standard_ConstructionError_Raise_if
375     (As > TolDist,"gp_Trsf::SeValues, non uniform");
376   As = TM.Value(1,3);
377   if (As < 0) As = - As;
378   Standard_ConstructionError_Raise_if
379     (As > TolDist,"gp_Trsf::SeValues, non uniform");
380   As = TM.Value(2,1);
381   if (As < 0) As = - As;
382   Standard_ConstructionError_Raise_if
383     (As > TolDist,"gp_Trsf::SeValues, non uniform");
384   As = TM.Value(2,2);
385   if (As < 0) As = - As;
386   Standard_ConstructionError_Raise_if
387     (As > TolDist,"gp_Trsf::SeValues, non uniform");
388   As = TM.Value(2,3);
389   if (As < 0) As = - As;
390   Standard_ConstructionError_Raise_if
391     (As > TolDist,"gp_Trsf::SeValues, non uniform");
392   As = TM.Value(3,1);
393   if (As < 0) As = - As;
394   Standard_ConstructionError_Raise_if
395     (As > TolDist,"gp_Trsf::SeValues, non uniform");
396   As = TM.Value(3,2);
397   if (As < 0) As = - As;
398   Standard_ConstructionError_Raise_if
399     (As > TolDist,"gp_Trsf::SeValues, non uniform");
400   As = TM.Value(3,3);
401   if (As < 0) As = - As;
402   Standard_ConstructionError_Raise_if
403     (As > TolDist,"gp_Trsf::SeValues, non uniform");
404   scale = s;
405   shape = gp_CompoundTrsf;
406   matrix = M;
407   loc = col4;
408 }
409
410 //=======================================================================
411 //function : GetRotation
412 //purpose  : 
413 //=======================================================================
414
415 gp_Quaternion gp_Trsf::GetRotation () const
416
417   return gp_Quaternion (matrix); 
418 }
419
420 //=======================================================================
421 //function : VectorialPart
422 //purpose  : 
423 //=======================================================================
424
425 gp_Mat gp_Trsf::VectorialPart () const
426
427   if (scale == 1.0)  return matrix; 
428   gp_Mat M = matrix;
429   if (shape == gp_Scale || shape == gp_PntMirror)
430     M.SetDiagonal(scale*M.Value(1,1),
431                   scale*M.Value(2,2),
432                   scale*M.Value(3,3));
433   else
434     M.Multiply (scale);
435   return M;
436 }
437
438 //=======================================================================
439 //function : Invert
440 //purpose  : 
441 //=======================================================================
442
443 void gp_Trsf::Invert()
444
445   //                                    -1
446   //  X' = scale * R * X + T  =>  X = (R  / scale)  * ( X' - T)
447   //
448   // Pour les gp_Trsf puisque le scale est extrait de la gp_Matrice R
449   // on a toujours determinant (R) = 1 et R-1 = R transposee.
450   if (shape == gp_Identity) { }
451   else if (shape == gp_Translation || shape == gp_PntMirror) loc.Reverse();
452   else if (shape == gp_Scale) {
453     Standard_Real As = scale;
454     if (As < 0) As = - As;
455     Standard_ConstructionError_Raise_if
456       (As <= gp::Resolution(),"");
457     scale = 1.0 / scale;
458     loc.Multiply (-scale);
459   }
460   else {
461     Standard_Real As = scale;
462     if (As < 0) As = - As;
463     Standard_ConstructionError_Raise_if
464       (As <= gp::Resolution(),"");
465     scale = 1.0 / scale;
466     matrix.Transpose ();
467     loc.Multiply (matrix);
468     loc.Multiply (-scale);
469   }
470 }
471
472 //=======================================================================
473 //function : Multiply
474 //purpose  : 
475 //=======================================================================
476
477 void gp_Trsf::Multiply(const gp_Trsf& T)
478 {
479   if (T.shape == gp_Identity) { }
480   else if (shape == gp_Identity) {
481     shape = T.shape;
482     scale = T.scale;
483     loc = T.loc;
484     matrix = T.matrix;
485   } 
486   else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
487     if (T.loc.X() != 0.0 || T.loc.Y() != 0.0 || T.loc.Z() != 0.0) {
488       loc.Add (T.loc.Multiplied (matrix));
489     }
490     matrix.Multiply(T.matrix);
491   }
492   else if (shape == gp_Translation && T.shape == gp_Translation) {
493     loc.Add (T.loc);
494   }
495   else if (shape == gp_Scale && T.shape == gp_Scale) {
496     loc.Add (T.loc.Multiplied(scale));
497     scale = scale * T.scale;
498   }
499   else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
500     scale = 1.0;
501     shape = gp_Translation;
502     loc.Add (T.loc.Reversed());
503   }
504   else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
505     shape = gp_Rotation;
506     loc.Add (T.loc.Multiplied (matrix));
507     matrix.Multiply(T.matrix);
508   }
509   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
510             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
511            && T.shape == gp_Translation) {
512     gp_XYZ Tloc(T.loc);
513     Tloc.Multiply(matrix);
514     if (scale != 1.0) { Tloc.Multiply(scale); }
515     loc.Add (Tloc);
516   }
517   else if ((shape == gp_Scale || shape == gp_PntMirror)
518            && T.shape == gp_Translation) {
519     gp_XYZ Tloc(T.loc);
520     Tloc.Multiply (scale);
521     loc.Add (Tloc);
522   }
523   else if (shape == gp_Translation && 
524            (T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
525             T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)) {
526     shape = gp_CompoundTrsf;
527     scale = T.scale;
528     loc.Add (T.loc);
529     matrix = T.matrix;
530   }
531   else if (shape == gp_Translation && 
532            (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
533     shape = T.shape;
534     loc.Add (T.loc);
535     scale = T.scale;
536   }
537   else if ((shape == gp_PntMirror || shape == gp_Scale) &&
538            (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
539     shape = gp_CompoundTrsf;
540     gp_XYZ Tloc(T.loc);
541     Tloc.Multiply (scale);
542     loc.Add (Tloc);
543     scale = scale * T.scale;
544   }
545   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
546             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
547            && (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
548     shape = gp_CompoundTrsf;
549     gp_XYZ Tloc(T.loc);
550     if (scale == 1.0) {
551       scale = T.scale;
552       Tloc.Multiply(matrix);
553     }
554     else {
555       Tloc.Multiply (matrix);
556       Tloc.Multiply (scale);
557       scale = scale * T.scale;
558     }
559     loc.Add (Tloc);
560   }
561   else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
562             T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)
563            && (shape == gp_Scale || shape == gp_PntMirror)) {
564     shape = gp_CompoundTrsf;
565     gp_XYZ Tloc(T.loc);
566     Tloc.Multiply(scale);
567     loc.Add (Tloc);
568     scale = scale * T.scale;
569     matrix = T.matrix;
570   }
571   else {
572     shape = gp_CompoundTrsf;
573     gp_XYZ Tloc(T.loc);
574     Tloc.Multiply (matrix);
575     if (scale != 1.0) { 
576       Tloc.Multiply (scale);
577       scale = scale * T.scale;
578     }
579     else { scale = T.scale; }
580     loc.Add (Tloc);
581     matrix.Multiply(T.matrix);
582   }
583 }
584
585 //=======================================================================
586 //function : Power
587 //purpose  : 
588 //=======================================================================
589
590 void gp_Trsf::Power (const Standard_Integer N)
591 {
592   if (shape == gp_Identity) { }
593   else {
594     if (N == 0)  {
595       scale = 1.0;
596       shape = gp_Identity;
597       matrix.SetIdentity();
598       loc = gp_XYZ (0.0, 0.0, 0.0);
599     }
600     else if (N == 1)  { }
601     else if (N == -1) { Invert(); }
602     else {
603       if (N < 0) { Invert(); }
604       if (shape == gp_Translation) {
605         Standard_Integer Npower = N;
606         if (Npower < 0) Npower = - Npower;
607         Npower--;
608         gp_XYZ Temploc = loc;
609         for(;;) {
610           if (IsOdd(Npower))  loc.Add (Temploc);
611           if (Npower == 1) break;
612           Temploc.Add (Temploc);
613           Npower = Npower/2;
614         }
615       }
616       else if (shape == gp_Scale) {
617         Standard_Integer Npower = N;
618         if (Npower < 0) Npower = - Npower;
619         Npower--;
620         gp_XYZ Temploc = loc;
621         Standard_Real Tempscale = scale;
622         for(;;) {
623           if (IsOdd(Npower)) {
624             loc.Add (Temploc.Multiplied(scale));
625             scale = scale * Tempscale;
626           }
627           if (Npower == 1) break;
628           Temploc.Add (Temploc.Multiplied(Tempscale));
629           Tempscale = Tempscale * Tempscale;
630           Npower = Npower/2;
631         }
632       }
633       else if (shape == gp_Rotation) {
634         Standard_Integer Npower = N;
635         if (Npower < 0) Npower = - Npower;
636         Npower--;
637         gp_Mat Tempmatrix (matrix);
638         if (loc.X() == 0.0 && loc.Y() == 0.0 && loc.Z() == 0.0) {
639           for(;;) {
640             if (IsOdd(Npower)) matrix.Multiply (Tempmatrix);
641             if (Npower == 1)   break;
642             Tempmatrix.Multiply (Tempmatrix);
643             Npower = Npower/2;
644           }
645         }
646         else {
647           gp_XYZ Temploc = loc;
648           for(;;) {
649             if (IsOdd(Npower)) {
650               loc.Add (Temploc.Multiplied (matrix));
651               matrix.Multiply (Tempmatrix);
652             }
653             if (Npower == 1) break;
654             Temploc.Add (Temploc.Multiplied (Tempmatrix));
655             Tempmatrix.Multiply (Tempmatrix);
656             Npower = Npower/2;
657           }
658         }
659       }
660       else if (shape == gp_PntMirror || shape == gp_Ax1Mirror ||
661                shape == gp_Ax2Mirror) {
662         if (IsEven (N)) {
663           shape = gp_Identity;
664           scale = 1.0;
665           matrix.SetIdentity ();
666           loc.SetX(0);
667           loc.SetY(0);
668           loc.SetZ(0);
669         }
670       }
671       else {
672         shape = gp_CompoundTrsf;
673         Standard_Integer Npower = N;
674         if (Npower < 0) Npower = - Npower;
675         Npower--;
676         gp_XYZ Temploc = loc;
677         Standard_Real Tempscale = scale;
678         gp_Mat Tempmatrix (matrix);
679         for(;;) {
680           if (IsOdd(Npower)) {
681             loc.Add ((Temploc.Multiplied (matrix)).Multiplied (scale));
682             scale = scale * Tempscale;
683             matrix.Multiply (Tempmatrix);
684           }
685           if (Npower == 1) break;
686           Tempscale = Tempscale * Tempscale;
687           Temploc.Add ( (Temploc.Multiplied (Tempmatrix)).Multiplied 
688                         (Tempscale)
689                         );
690           Tempmatrix.Multiply (Tempmatrix);
691           Npower = Npower/2;
692         }
693       }
694     }
695   }
696 }
697
698 //=======================================================================
699 //function : PreMultiply
700 //purpose  : 
701 //=======================================================================
702
703 void gp_Trsf::PreMultiply (const gp_Trsf& T)
704 {
705   if (T.shape == gp_Identity) { }
706   else if (shape == gp_Identity) {
707     shape = T.shape;
708     scale = T.scale;
709     loc = T.loc;
710     matrix = T.matrix;
711   } 
712   else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
713     loc.Multiply (T.matrix);
714     loc.Add (T.loc);
715     matrix.PreMultiply(T.matrix);
716   }
717   else if (shape == gp_Translation && T.shape == gp_Translation) {
718     loc.Add (T.loc);
719   }
720   else if (shape == gp_Scale && T.shape == gp_Scale) {
721     loc.Multiply (T.scale);
722     loc.Add (T.loc);
723     scale = scale * T.scale;
724   }
725   else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
726     scale = 1.0;
727     shape = gp_Translation;
728     loc.Reverse();
729     loc.Add (T.loc);
730   }
731   else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
732     shape = gp_Rotation;
733     loc.Multiply (T.matrix);
734     loc.Add (T.loc);
735     matrix.PreMultiply(T.matrix);
736   }
737   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
738             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
739            && T.shape == gp_Translation) {
740     loc.Add (T.loc);
741   }
742   else if ((shape == gp_Scale || shape == gp_PntMirror)
743            && T.shape == gp_Translation) {
744     loc.Add (T.loc);
745   }
746   else if (shape == gp_Translation && 
747            (T.shape == gp_CompoundTrsf || T.shape == gp_Rotation
748             || T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)) {
749     shape = gp_CompoundTrsf;
750     matrix = T.matrix;
751     if (T.scale == 1.0)  loc.Multiply (T.matrix);
752     else {
753       scale = T.scale;
754       loc.Multiply (matrix);
755       loc.Multiply (scale);
756     }
757     loc.Add (T.loc);
758   }
759   else if ((T.shape == gp_Scale || T.shape == gp_PntMirror)
760            && shape == gp_Translation) {
761     loc.Multiply (T.scale);
762     loc.Add (T.loc);
763     scale = T.scale;
764     shape = T.shape;
765   }
766   else if ((shape == gp_PntMirror || shape == gp_Scale) &&
767            (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
768     shape = gp_CompoundTrsf;
769     loc.Multiply (T.scale);
770     loc.Add (T.loc);
771     scale = scale * T.scale;
772   }
773   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
774             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror) 
775            && (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
776     shape = gp_CompoundTrsf;
777     loc.Multiply (T.scale);
778     loc.Add (T.loc);
779     scale = scale * T.scale;
780   } 
781   else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
782             T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror) 
783            && (shape == gp_Scale || shape == gp_PntMirror)) {
784     shape = gp_CompoundTrsf;
785     matrix = T.matrix;
786     if (T.scale == 1.0)  loc.Multiply (T.matrix);
787     else {
788       loc.Multiply (matrix);
789       loc.Multiply (T.scale);
790       scale = T.scale * scale;
791     }
792     loc.Add (T.loc);
793   } 
794   else {
795     shape = gp_CompoundTrsf;
796     loc.Multiply (T.matrix);
797     if (T.scale != 1.0) {
798       loc.Multiply (T.scale);    scale = scale * T.scale;
799     }
800     loc.Add (T.loc);
801     matrix.PreMultiply(T.matrix);
802   }
803 }
804
805 //=======================================================================
806 //function : GetRotation
807 //purpose  : algorithm from A.Korn, M.Korn, "Mathematical Handbook for
808 //           scientists and Engineers" McGraw-Hill, 1961, ch.14.10-2.
809 //=======================================================================
810
811 Standard_Boolean gp_Trsf::GetRotation (gp_XYZ&        theAxis,
812                                        Standard_Real& theAngle) const
813 {
814   gp_Quaternion Q = GetRotation();
815   gp_Vec aVec;
816   Q.GetVectorAndAngle (aVec, theAngle);
817   theAxis = aVec.XYZ();
818   return Standard_True;
819 }