0025253: gp_trsf code cleaning
[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   const Standard_Boolean locnull = (loc.SquareModulus() < gp::Resolution());
237
238   switch (shape) {
239
240   case gp_Identity :
241     if (!locnull) shape = gp_Translation;
242     break;
243
244   case gp_Translation :
245     if (locnull) shape = gp_Identity;
246     break;
247
248   case gp_Rotation :
249   case gp_PntMirror :
250   case gp_Ax1Mirror :
251   case gp_Ax2Mirror :
252   case gp_Scale :
253   case gp_CompoundTrsf :
254   case gp_Other :
255     if (!locnull) {
256       shape = gp_CompoundTrsf;
257     }
258     break;
259   }
260 }
261
262 //=======================================================================
263 //function : SetScaleFactor
264 //purpose  : 
265 //=======================================================================
266
267 void gp_Trsf::SetScaleFactor (const Standard_Real S) 
268 {   
269   Standard_Real As = S;
270   if (As < 0) As = - As;
271   Standard_ConstructionError_Raise_if
272     (As <= gp::Resolution(),"gp_Trsf::SetScaleFactor");
273   scale = S;
274   As = scale - 1.;
275   if (As < 0) As = - As;
276   Standard_Boolean unit  = As <= gp::Resolution();
277   As = scale + 1.;
278   if (As < 0) As = - As;
279   Standard_Boolean munit = As <= gp::Resolution();
280   
281   switch (shape) {
282   case gp_Identity :
283   case gp_Translation :
284     if (!unit) shape = gp_Scale;
285     if (munit) shape = gp_PntMirror;
286     break;
287   case gp_Rotation :
288     if (!unit) shape = gp_CompoundTrsf;
289     break;
290   case gp_PntMirror :
291   case gp_Ax1Mirror :
292   case gp_Ax2Mirror :
293     if (!munit) shape = gp_Scale;
294     if (unit)   shape = gp_Identity;
295     break;
296   case gp_Scale :
297     if (unit)  shape = gp_Identity;
298     if (munit) shape = gp_PntMirror;
299     break;
300   case gp_CompoundTrsf :
301     break;
302   case gp_Other :
303     break;
304   }
305 }
306
307 //=======================================================================
308 //function : SetValues
309 //purpose  : 
310 // 06-01-1998 modified by PMN : On utilise TolDist pour evaluer si les coeffs 
311 //  sont nuls : c'est toujours mieux que gp::Resolution !
312 //=======================================================================
313
314 void gp_Trsf::SetValues(const Standard_Real a11, 
315                         const Standard_Real a12, 
316                         const Standard_Real a13, 
317                         const Standard_Real a14, 
318                         const Standard_Real a21, 
319                         const Standard_Real a22, 
320                         const Standard_Real a23, 
321                         const Standard_Real a24, 
322                         const Standard_Real a31, 
323                         const Standard_Real a32,
324                         const Standard_Real a33, 
325                         const Standard_Real a34, 
326 //                      const Standard_Real Tolang, 
327                         const Standard_Real , 
328                         const Standard_Real
329 #ifndef No_Exception
330                                             TolDist
331 #endif
332                        )
333 {
334   gp_XYZ col1(a11,a21,a31);
335   gp_XYZ col2(a12,a22,a32);
336   gp_XYZ col3(a13,a23,a33);
337   gp_XYZ col4(a14,a24,a34);
338   // compute the determinant
339   gp_Mat M(col1,col2,col3);
340   Standard_Real s = M.Determinant();
341   Standard_Real As = s;
342   if (As < 0) As = - As;
343   Standard_ConstructionError_Raise_if
344     (As < gp::Resolution(),"gp_Trsf::SeValues, null determinant");
345   if (s > 0)
346     s = Pow(s,1./3.);
347   else
348     s = -Pow(-s,1./3.);
349   M.Divide(s);
350   
351   // check if the matrix is a rotation matrix
352   // the transposition should be the invert.
353   gp_Mat TM(M);
354   TM.Transpose();
355   TM.Multiply(M);
356   //
357   // don t trust the initial values !
358   //
359   gp_Mat anIdentity ;
360   anIdentity.SetIdentity() ;
361   TM.Subtract(anIdentity);
362   As = TM.Value(1,1);
363   if (As < 0) As = - As;
364   Standard_ConstructionError_Raise_if
365     (As > TolDist,"gp_Trsf::SeValues, non uniform");
366   As = TM.Value(1,2);
367   if (As < 0) As = - As;
368   Standard_ConstructionError_Raise_if
369     (As > TolDist,"gp_Trsf::SeValues, non uniform");
370   As = TM.Value(1,3);
371   if (As < 0) As = - As;
372   Standard_ConstructionError_Raise_if
373     (As > TolDist,"gp_Trsf::SeValues, non uniform");
374   As = TM.Value(2,1);
375   if (As < 0) As = - As;
376   Standard_ConstructionError_Raise_if
377     (As > TolDist,"gp_Trsf::SeValues, non uniform");
378   As = TM.Value(2,2);
379   if (As < 0) As = - As;
380   Standard_ConstructionError_Raise_if
381     (As > TolDist,"gp_Trsf::SeValues, non uniform");
382   As = TM.Value(2,3);
383   if (As < 0) As = - As;
384   Standard_ConstructionError_Raise_if
385     (As > TolDist,"gp_Trsf::SeValues, non uniform");
386   As = TM.Value(3,1);
387   if (As < 0) As = - As;
388   Standard_ConstructionError_Raise_if
389     (As > TolDist,"gp_Trsf::SeValues, non uniform");
390   As = TM.Value(3,2);
391   if (As < 0) As = - As;
392   Standard_ConstructionError_Raise_if
393     (As > TolDist,"gp_Trsf::SeValues, non uniform");
394   As = TM.Value(3,3);
395   if (As < 0) As = - As;
396   Standard_ConstructionError_Raise_if
397     (As > TolDist,"gp_Trsf::SeValues, non uniform");
398   scale = s;
399   shape = gp_CompoundTrsf;
400   matrix = M;
401   loc = col4;
402 }
403
404 //=======================================================================
405 //function : GetRotation
406 //purpose  : 
407 //=======================================================================
408
409 gp_Quaternion gp_Trsf::GetRotation () const
410
411   return gp_Quaternion (matrix); 
412 }
413
414 //=======================================================================
415 //function : VectorialPart
416 //purpose  : 
417 //=======================================================================
418
419 gp_Mat gp_Trsf::VectorialPart () const
420
421   if (scale == 1.0)  return matrix; 
422   gp_Mat M = matrix;
423   if (shape == gp_Scale || shape == gp_PntMirror)
424     M.SetDiagonal(scale*M.Value(1,1),
425                   scale*M.Value(2,2),
426                   scale*M.Value(3,3));
427   else
428     M.Multiply (scale);
429   return M;
430 }
431
432 //=======================================================================
433 //function : Invert
434 //purpose  : 
435 //=======================================================================
436
437 void gp_Trsf::Invert()
438
439   //                                    -1
440   //  X' = scale * R * X + T  =>  X = (R  / scale)  * ( X' - T)
441   //
442   // Pour les gp_Trsf puisque le scale est extrait de la gp_Matrice R
443   // on a toujours determinant (R) = 1 et R-1 = R transposee.
444   if (shape == gp_Identity) { }
445   else if (shape == gp_Translation || shape == gp_PntMirror) loc.Reverse();
446   else if (shape == gp_Scale) {
447     Standard_Real As = scale;
448     if (As < 0) As = - As;
449     Standard_ConstructionError_Raise_if
450       (As <= gp::Resolution(),"");
451     scale = 1.0 / scale;
452     loc.Multiply (-scale);
453   }
454   else {
455     Standard_Real As = scale;
456     if (As < 0) As = - As;
457     Standard_ConstructionError_Raise_if
458       (As <= gp::Resolution(),"");
459     scale = 1.0 / scale;
460     matrix.Transpose ();
461     loc.Multiply (matrix);
462     loc.Multiply (-scale);
463   }
464 }
465
466 //=======================================================================
467 //function : Multiply
468 //purpose  : 
469 //=======================================================================
470
471 void gp_Trsf::Multiply(const gp_Trsf& T)
472 {
473   if (T.shape == gp_Identity) { }
474   else if (shape == gp_Identity) {
475     shape = T.shape;
476     scale = T.scale;
477     loc = T.loc;
478     matrix = T.matrix;
479   } 
480   else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
481     if (T.loc.X() != 0.0 || T.loc.Y() != 0.0 || T.loc.Z() != 0.0) {
482       loc.Add (T.loc.Multiplied (matrix));
483     }
484     matrix.Multiply(T.matrix);
485   }
486   else if (shape == gp_Translation && T.shape == gp_Translation) {
487     loc.Add (T.loc);
488   }
489   else if (shape == gp_Scale && T.shape == gp_Scale) {
490     loc.Add (T.loc.Multiplied(scale));
491     scale = scale * T.scale;
492   }
493   else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
494     scale = 1.0;
495     shape = gp_Translation;
496     loc.Add (T.loc.Reversed());
497   }
498   else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
499     shape = gp_Rotation;
500     loc.Add (T.loc.Multiplied (matrix));
501     matrix.Multiply(T.matrix);
502   }
503   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
504             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
505            && T.shape == gp_Translation) {
506     gp_XYZ Tloc(T.loc);
507     Tloc.Multiply(matrix);
508     if (scale != 1.0) { Tloc.Multiply(scale); }
509     loc.Add (Tloc);
510   }
511   else if ((shape == gp_Scale || shape == gp_PntMirror)
512            && T.shape == gp_Translation) {
513     gp_XYZ Tloc(T.loc);
514     Tloc.Multiply (scale);
515     loc.Add (Tloc);
516   }
517   else if (shape == gp_Translation && 
518            (T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
519             T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)) {
520     shape = gp_CompoundTrsf;
521     scale = T.scale;
522     loc.Add (T.loc);
523     matrix = T.matrix;
524   }
525   else if (shape == gp_Translation && 
526            (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
527     shape = T.shape;
528     loc.Add (T.loc);
529     scale = T.scale;
530   }
531   else if ((shape == gp_PntMirror || shape == gp_Scale) &&
532            (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
533     shape = gp_CompoundTrsf;
534     gp_XYZ Tloc(T.loc);
535     Tloc.Multiply (scale);
536     loc.Add (Tloc);
537     scale = scale * T.scale;
538   }
539   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
540             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
541            && (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
542     shape = gp_CompoundTrsf;
543     gp_XYZ Tloc(T.loc);
544     if (scale == 1.0) {
545       scale = T.scale;
546       Tloc.Multiply(matrix);
547     }
548     else {
549       Tloc.Multiply (matrix);
550       Tloc.Multiply (scale);
551       scale = scale * T.scale;
552     }
553     loc.Add (Tloc);
554   }
555   else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
556             T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)
557            && (shape == gp_Scale || shape == gp_PntMirror)) {
558     shape = gp_CompoundTrsf;
559     gp_XYZ Tloc(T.loc);
560     Tloc.Multiply(scale);
561     loc.Add (Tloc);
562     scale = scale * T.scale;
563     matrix = T.matrix;
564   }
565   else {
566     shape = gp_CompoundTrsf;
567     gp_XYZ Tloc(T.loc);
568     Tloc.Multiply (matrix);
569     if (scale != 1.0) { 
570       Tloc.Multiply (scale);
571       scale = scale * T.scale;
572     }
573     else { scale = T.scale; }
574     loc.Add (Tloc);
575     matrix.Multiply(T.matrix);
576   }
577 }
578
579 //=======================================================================
580 //function : Power
581 //purpose  : 
582 //=======================================================================
583
584 void gp_Trsf::Power (const Standard_Integer N)
585 {
586   if (shape == gp_Identity) { }
587   else {
588     if (N == 0)  {
589       scale = 1.0;
590       shape = gp_Identity;
591       matrix.SetIdentity();
592       loc = gp_XYZ (0.0, 0.0, 0.0);
593     }
594     else if (N == 1)  { }
595     else if (N == -1) { Invert(); }
596     else {
597       if (N < 0) { Invert(); }
598       if (shape == gp_Translation) {
599         Standard_Integer Npower = N;
600         if (Npower < 0) Npower = - Npower;
601         Npower--;
602         gp_XYZ Temploc = loc;
603         for(;;) {
604           if (IsOdd(Npower))  loc.Add (Temploc);
605           if (Npower == 1) break;
606           Temploc.Add (Temploc);
607           Npower = Npower/2;
608         }
609       }
610       else if (shape == gp_Scale) {
611         Standard_Integer Npower = N;
612         if (Npower < 0) Npower = - Npower;
613         Npower--;
614         gp_XYZ Temploc = loc;
615         Standard_Real Tempscale = scale;
616         for(;;) {
617           if (IsOdd(Npower)) {
618             loc.Add (Temploc.Multiplied(scale));
619             scale = scale * Tempscale;
620           }
621           if (Npower == 1) break;
622           Temploc.Add (Temploc.Multiplied(Tempscale));
623           Tempscale = Tempscale * Tempscale;
624           Npower = Npower/2;
625         }
626       }
627       else if (shape == gp_Rotation) {
628         Standard_Integer Npower = N;
629         if (Npower < 0) Npower = - Npower;
630         Npower--;
631         gp_Mat Tempmatrix (matrix);
632         if (loc.X() == 0.0 && loc.Y() == 0.0 && loc.Z() == 0.0) {
633           for(;;) {
634             if (IsOdd(Npower)) matrix.Multiply (Tempmatrix);
635             if (Npower == 1)   break;
636             Tempmatrix.Multiply (Tempmatrix);
637             Npower = Npower/2;
638           }
639         }
640         else {
641           gp_XYZ Temploc = loc;
642           for(;;) {
643             if (IsOdd(Npower)) {
644               loc.Add (Temploc.Multiplied (matrix));
645               matrix.Multiply (Tempmatrix);
646             }
647             if (Npower == 1) break;
648             Temploc.Add (Temploc.Multiplied (Tempmatrix));
649             Tempmatrix.Multiply (Tempmatrix);
650             Npower = Npower/2;
651           }
652         }
653       }
654       else if (shape == gp_PntMirror || shape == gp_Ax1Mirror ||
655                shape == gp_Ax2Mirror) {
656         if (IsEven (N)) {
657           shape = gp_Identity;
658           scale = 1.0;
659           matrix.SetIdentity ();
660           loc.SetX(0);
661           loc.SetY(0);
662           loc.SetZ(0);
663         }
664       }
665       else {
666         shape = gp_CompoundTrsf;
667         Standard_Integer Npower = N;
668         if (Npower < 0) Npower = - Npower;
669         Npower--;
670         gp_XYZ Temploc = loc;
671         Standard_Real Tempscale = scale;
672         gp_Mat Tempmatrix (matrix);
673         for(;;) {
674           if (IsOdd(Npower)) {
675             loc.Add ((Temploc.Multiplied (matrix)).Multiplied (scale));
676             scale = scale * Tempscale;
677             matrix.Multiply (Tempmatrix);
678           }
679           if (Npower == 1) break;
680           Tempscale = Tempscale * Tempscale;
681           Temploc.Add ( (Temploc.Multiplied (Tempmatrix)).Multiplied 
682                         (Tempscale)
683                         );
684           Tempmatrix.Multiply (Tempmatrix);
685           Npower = Npower/2;
686         }
687       }
688     }
689   }
690 }
691
692 //=======================================================================
693 //function : PreMultiply
694 //purpose  : 
695 //=======================================================================
696
697 void gp_Trsf::PreMultiply (const gp_Trsf& T)
698 {
699   if (T.shape == gp_Identity) { }
700   else if (shape == gp_Identity) {
701     shape = T.shape;
702     scale = T.scale;
703     loc = T.loc;
704     matrix = T.matrix;
705   } 
706   else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
707     loc.Multiply (T.matrix);
708     loc.Add (T.loc);
709     matrix.PreMultiply(T.matrix);
710   }
711   else if (shape == gp_Translation && T.shape == gp_Translation) {
712     loc.Add (T.loc);
713   }
714   else if (shape == gp_Scale && T.shape == gp_Scale) {
715     loc.Multiply (T.scale);
716     loc.Add (T.loc);
717     scale = scale * T.scale;
718   }
719   else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
720     scale = 1.0;
721     shape = gp_Translation;
722     loc.Reverse();
723     loc.Add (T.loc);
724   }
725   else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
726     shape = gp_Rotation;
727     loc.Multiply (T.matrix);
728     loc.Add (T.loc);
729     matrix.PreMultiply(T.matrix);
730   }
731   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
732             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
733            && T.shape == gp_Translation) {
734     loc.Add (T.loc);
735   }
736   else if ((shape == gp_Scale || shape == gp_PntMirror)
737            && T.shape == gp_Translation) {
738     loc.Add (T.loc);
739   }
740   else if (shape == gp_Translation && 
741            (T.shape == gp_CompoundTrsf || T.shape == gp_Rotation
742             || T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)) {
743     shape = gp_CompoundTrsf;
744     matrix = T.matrix;
745     if (T.scale == 1.0)  loc.Multiply (T.matrix);
746     else {
747       scale = T.scale;
748       loc.Multiply (matrix);
749       loc.Multiply (scale);
750     }
751     loc.Add (T.loc);
752   }
753   else if ((T.shape == gp_Scale || T.shape == gp_PntMirror)
754            && shape == gp_Translation) {
755     loc.Multiply (T.scale);
756     loc.Add (T.loc);
757     scale = T.scale;
758     shape = T.shape;
759   }
760   else if ((shape == gp_PntMirror || shape == gp_Scale) &&
761            (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
762     shape = gp_CompoundTrsf;
763     loc.Multiply (T.scale);
764     loc.Add (T.loc);
765     scale = scale * T.scale;
766   }
767   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
768             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror) 
769            && (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
770     shape = gp_CompoundTrsf;
771     loc.Multiply (T.scale);
772     loc.Add (T.loc);
773     scale = scale * T.scale;
774   } 
775   else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
776             T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror) 
777            && (shape == gp_Scale || shape == gp_PntMirror)) {
778     shape = gp_CompoundTrsf;
779     matrix = T.matrix;
780     if (T.scale == 1.0)  loc.Multiply (T.matrix);
781     else {
782       loc.Multiply (matrix);
783       loc.Multiply (T.scale);
784       scale = T.scale * scale;
785     }
786     loc.Add (T.loc);
787   } 
788   else {
789     shape = gp_CompoundTrsf;
790     loc.Multiply (T.matrix);
791     if (T.scale != 1.0) {
792       loc.Multiply (T.scale);    scale = scale * T.scale;
793     }
794     loc.Add (T.loc);
795     matrix.PreMultiply(T.matrix);
796   }
797 }
798
799 //=======================================================================
800 //function : GetRotation
801 //purpose  : algorithm from A.Korn, M.Korn, "Mathematical Handbook for
802 //           scientists and Engineers" McGraw-Hill, 1961, ch.14.10-2.
803 //=======================================================================
804
805 Standard_Boolean gp_Trsf::GetRotation (gp_XYZ&        theAxis,
806                                        Standard_Real& theAngle) const
807 {
808   gp_Quaternion Q = GetRotation();
809   gp_Vec aVec;
810   Q.GetVectorAndAngle (aVec, theAngle);
811   theAxis = aVec.XYZ();
812   return Standard_True;
813 }