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