0031673: Draw Harness, ViewerTest - command vlocation applies transformation in oppos...
[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
23 #include <gp.hxx>
24 #include <gp_Ax1.hxx>
25 #include <gp_Ax2.hxx>
26 #include <gp_Ax3.hxx>
27 #include <gp_GTrsf.hxx>
28 #include <gp_Mat.hxx>
29 #include <gp_Pnt.hxx>
30 #include <gp_Quaternion.hxx>
31 #include <gp_Trsf.hxx>
32 #include <gp_Trsf2d.hxx>
33 #include <gp_Vec.hxx>
34 #include <gp_XYZ.hxx>
35 #include <Standard_ConstructionError.hxx>
36 #include <Standard_OutOfRange.hxx>
37 #include <Standard_Dump.hxx>
38
39 //=======================================================================
40 //function : gp_Trsf
41 //purpose  : Constructor from 2d
42 //=======================================================================
43 gp_Trsf::gp_Trsf (const gp_Trsf2d& T) : 
44 scale(T.ScaleFactor()),
45 shape(T.Form()),
46 loc(T.TranslationPart().X(),T.TranslationPart().Y(), 0.0)
47 {
48   const gp_Mat2d& M = T.HVectorialPart();
49   matrix(1,1) = M(1,1);
50   matrix(1,2) = M(1,2);
51   matrix(2,1) = M(2,1);
52   matrix(2,2) = M(2,2);
53   matrix(3,3) = 1.;
54   if (shape == gp_Ax1Mirror)
55   {
56     scale = 1;
57     matrix.Multiply(-1);
58   }
59 }
60
61 //=======================================================================
62 //function : SetMirror
63 //purpose  : 
64 //=======================================================================
65
66 void gp_Trsf::SetMirror (const gp_Ax1& A1) 
67 {
68   shape = gp_Ax1Mirror;
69   scale = 1;
70   loc = A1.Location().XYZ();
71   matrix.SetDot(A1.Direction().XYZ());
72   matrix.Multiply(-2);
73   matrix.SetDiagonal (matrix.Value (1,1) + 1,
74                       matrix.Value (2,2) + 1,
75                       matrix.Value (3,3) + 1);
76
77   loc.Multiply (matrix);
78   loc.Add (A1.Location().XYZ());
79   matrix.Multiply(-1);
80 }
81
82 //=======================================================================
83 //function : SetMirror
84 //purpose  : 
85 //=======================================================================
86
87 void gp_Trsf::SetMirror (const gp_Ax2& A2) 
88 {
89   shape = gp_Ax2Mirror;
90   scale = -1;
91   loc = A2.Location().XYZ();
92   matrix.SetDot(A2.Direction().XYZ());
93   matrix.Multiply(2);
94   matrix.SetDiagonal (matrix.Value (1,1) - 1,
95                       matrix.Value (2,2) - 1,
96                       matrix.Value (3,3) - 1);
97
98   loc.Multiply (matrix);
99   loc.Add (A2.Location().XYZ());
100
101
102 //=======================================================================
103 //function : SetRotation
104 //purpose  : 
105 //=======================================================================
106
107 void gp_Trsf::SetRotation (const gp_Ax1& A1,
108                            const Standard_Real Ang)
109 {
110   shape = gp_Rotation;
111   scale = 1.;
112   loc = A1.Location().XYZ();
113   matrix.SetRotation (A1.Direction().XYZ(), Ang);
114   loc.Reverse ();
115   loc.Multiply (matrix);
116   loc.Add (A1.Location().XYZ());
117 }
118
119 //=======================================================================
120 //function : SetRotation
121 //purpose  : 
122 //=======================================================================
123
124 void gp_Trsf::SetRotation (const gp_Quaternion& R)
125 {
126   shape = gp_Rotation;
127   scale = 1.;
128   loc.SetCoord (0., 0., 0.);
129   matrix = R.GetMatrix();
130 }
131
132 //=======================================================================
133 //function : SetRotationPart
134 //purpose  :
135 //=======================================================================
136 void gp_Trsf::SetRotationPart (const gp_Quaternion& theR)
137 {
138   const bool hasRotation = !theR.IsEqual (gp_Quaternion());
139   if (hasRotation)
140   {
141     matrix = theR.GetMatrix();
142   }
143   else
144   {
145     matrix.SetIdentity();
146   }
147
148   switch (shape)
149   {
150     case gp_Identity:
151     {
152       if (hasRotation)
153       {
154         shape = gp_Rotation;
155       }
156       break;
157     }
158     case gp_Rotation:
159     {
160       if (!hasRotation)
161       {
162         shape = gp_Identity;
163       }
164       break;
165     }
166     case gp_Translation:
167     case gp_PntMirror:
168     case gp_Ax1Mirror:
169     case gp_Ax2Mirror:
170     case gp_Scale:
171     case gp_CompoundTrsf:
172     case gp_Other:
173     {
174       if (hasRotation)
175       {
176         shape = gp_CompoundTrsf;
177       }
178       break;
179     }
180   }
181 }
182
183 //=======================================================================
184 //function : SetScale
185 //purpose  : 
186 //=======================================================================
187
188 void gp_Trsf::SetScale (const gp_Pnt& P, const Standard_Real S)  
189 {
190   shape = gp_Scale;
191   scale = S;
192   loc = P.XYZ();
193   Standard_Real As = scale;
194   if (As < 0) As = - As;
195   Standard_ConstructionError_Raise_if
196     (As <= gp::Resolution(),"gp_Trsf::SetScaleFactor");
197   matrix.SetIdentity ();
198   loc.Multiply (1-S);
199 }
200
201 //=======================================================================
202 //function : SetTransformation
203 //purpose  : 
204 //=======================================================================
205
206 void gp_Trsf::SetTransformation (const gp_Ax3& FromA1,
207                                  const gp_Ax3& ToA2)
208 {
209   shape = gp_CompoundTrsf;
210   scale = 1.0;
211   // matrix from XOY  ToA2 :
212   matrix.SetRows (ToA2.XDirection().XYZ(),
213                   ToA2.YDirection().XYZ(),
214                   ToA2. Direction().XYZ());
215   loc = ToA2.Location().XYZ();
216   loc.Multiply (matrix);
217   loc.Reverse ();
218
219   // matrix FromA1 to XOY :
220   const gp_XYZ& xDir = FromA1.XDirection().XYZ();
221   const gp_XYZ& yDir = FromA1.YDirection().XYZ();
222   const gp_XYZ& zDir = FromA1.Direction().XYZ();
223
224   gp_Mat MA1 (xDir, yDir, zDir);
225   gp_XYZ MA1loc = FromA1.Location().XYZ();
226
227   // matrix * MA1 => FromA1 ToA2 :
228   MA1loc.Multiply (matrix);
229   loc.Add (MA1loc);
230   matrix.Multiply (MA1);
231 }
232
233 void gp_Trsf::SetTransformation (const gp_Ax3& A3) 
234 {
235   shape = gp_CompoundTrsf;
236   scale = 1.0;
237   matrix.SetRows (A3.XDirection().XYZ(),
238                   A3.YDirection().XYZ(),
239                   A3. Direction().XYZ());
240   loc = A3.Location().XYZ();
241   loc.Multiply (matrix);
242   loc.Reverse ();
243 }
244
245 //=======================================================================
246 //function : SetTransformation
247 //purpose  : 
248 //=======================================================================
249
250 void gp_Trsf::SetTransformation (const gp_Quaternion& R, const gp_Vec& T)
251 {
252   shape = gp_CompoundTrsf;
253   scale = 1.;
254   loc = T.XYZ();
255   matrix = R.GetMatrix();
256 }
257
258 //=======================================================================
259 //function : SetDisplacement
260 //purpose  : 
261 //=======================================================================
262
263 void gp_Trsf::SetDisplacement (const gp_Ax3& FromA1,
264                                const gp_Ax3& ToA2)
265 {
266   shape = gp_CompoundTrsf;
267   scale = 1.0;
268   // matrix from ToA2 to XOY :
269   matrix.SetCol (1, ToA2.XDirection().XYZ());
270   matrix.SetCol (2, ToA2.YDirection().XYZ());
271   matrix.SetCol (3, ToA2.Direction().XYZ());
272   loc = ToA2.Location().XYZ();
273   // matrix XOY to FromA1 :
274   const gp_XYZ& xDir = FromA1.XDirection().XYZ();
275   const gp_XYZ& yDir = FromA1.YDirection().XYZ();
276   const gp_XYZ& zDir = FromA1.Direction().XYZ();
277   gp_Mat MA1 (xDir, yDir, zDir);
278   MA1.Transpose();
279   gp_XYZ MA1loc = FromA1.Location().XYZ();
280   MA1loc.Multiply (MA1);
281   MA1loc.Reverse();
282   // matrix * MA1 
283   MA1loc.Multiply (matrix);
284   loc.Add (MA1loc);
285   matrix.Multiply (MA1);
286 }
287
288 //=======================================================================
289 //function : SetTranslationPart
290 //purpose  : 
291 //=======================================================================
292
293 void gp_Trsf::SetTranslationPart (const gp_Vec& V) {   
294
295   loc = V.XYZ();
296   const Standard_Boolean locnull = (loc.SquareModulus() < gp::Resolution());
297
298   switch (shape) {
299
300   case gp_Identity :
301     if (!locnull) shape = gp_Translation;
302     break;
303
304   case gp_Translation :
305     if (locnull) shape = gp_Identity;
306     break;
307
308   case gp_Rotation :
309   case gp_PntMirror :
310   case gp_Ax1Mirror :
311   case gp_Ax2Mirror :
312   case gp_Scale :
313   case gp_CompoundTrsf :
314   case gp_Other :
315     if (!locnull) {
316       shape = gp_CompoundTrsf;
317     }
318     break;
319   }
320 }
321
322 //=======================================================================
323 //function : SetScaleFactor
324 //purpose  : 
325 //=======================================================================
326
327 void gp_Trsf::SetScaleFactor (const Standard_Real S) 
328 {   
329   Standard_Real As = S;
330   if (As < 0) As = - As;
331   Standard_ConstructionError_Raise_if
332     (As <= gp::Resolution(),"gp_Trsf::SetScaleFactor");
333   scale = S;
334   As = scale - 1.;
335   if (As < 0) As = - As;
336   Standard_Boolean unit  = As <= gp::Resolution(); // = (scale == 1)
337   As = scale + 1.;
338   if (As < 0) As = - As;
339   Standard_Boolean munit = As <= gp::Resolution(); // = (scale == -1)
340   
341   switch (shape) {
342   case gp_Identity :
343   case gp_Translation :
344     if (!unit) shape = gp_Scale;
345     if (munit) shape = gp_PntMirror;
346     break;
347   case gp_Rotation :
348     if (!unit) shape = gp_CompoundTrsf;
349     break;
350   case gp_PntMirror :
351   case gp_Ax1Mirror :
352   case gp_Ax2Mirror :
353     if (!munit) shape = gp_Scale;
354     if (unit)   shape = gp_Identity;
355     break;
356   case gp_Scale :
357     if (unit)  shape = gp_Identity;
358     if (munit) shape = gp_PntMirror;
359     break;
360   case gp_CompoundTrsf :
361     break;
362   case gp_Other :
363     break;
364   }
365 }
366
367 //=======================================================================
368 //function : SetValues
369 //purpose  : 
370 // 06-01-1998 modified by PMN : On utilise TolDist pour evaluer si les coeffs 
371 //  sont nuls : c'est toujours mieux que gp::Resolution !
372 //=======================================================================
373
374 void gp_Trsf::SetValues(const Standard_Real a11, 
375                         const Standard_Real a12, 
376                         const Standard_Real a13, 
377                         const Standard_Real a14, 
378                         const Standard_Real a21, 
379                         const Standard_Real a22, 
380                         const Standard_Real a23, 
381                         const Standard_Real a24, 
382                         const Standard_Real a31, 
383                         const Standard_Real a32,
384                         const Standard_Real a33, 
385                         const Standard_Real a34)
386 {
387   gp_XYZ col1(a11,a21,a31);
388   gp_XYZ col2(a12,a22,a32);
389   gp_XYZ col3(a13,a23,a33);
390   gp_XYZ col4(a14,a24,a34);
391   // compute the determinant
392   gp_Mat M(col1,col2,col3);
393   Standard_Real s = M.Determinant();
394   Standard_Real As = s;
395   if (As < 0) As = - As;
396   Standard_ConstructionError_Raise_if
397     (As < gp::Resolution(),"gp_Trsf::SetValues, null determinant");
398   if (s > 0)
399     s = Pow(s,1./3.);
400   else
401     s = -Pow(-s,1./3.);
402   M.Divide(s);
403   
404   scale = s;
405   shape = gp_CompoundTrsf;
406
407   matrix = M;
408   Orthogonalize();
409   
410   loc = col4;
411 }
412
413 //=======================================================================
414 //function : GetRotation
415 //purpose  : 
416 //=======================================================================
417
418 gp_Quaternion gp_Trsf::GetRotation () const
419
420   return gp_Quaternion (matrix); 
421 }
422
423 //=======================================================================
424 //function : VectorialPart
425 //purpose  : 
426 //=======================================================================
427
428 gp_Mat gp_Trsf::VectorialPart () const
429
430   if (scale == 1.0)  return matrix; 
431   gp_Mat M = matrix;
432   if (shape == gp_Scale || shape == gp_PntMirror)
433     M.SetDiagonal(scale*M.Value(1,1),
434                   scale*M.Value(2,2),
435                   scale*M.Value(3,3));
436   else
437     M.Multiply (scale);
438   return M;
439 }
440
441 //=======================================================================
442 //function : Invert
443 //purpose  : 
444 //=======================================================================
445
446 void gp_Trsf::Invert()
447
448   //                                    -1
449   //  X' = scale * R * X + T  =>  X = (R  / scale)  * ( X' - T)
450   //
451   // Pour les gp_Trsf puisque le scale est extrait de la gp_Matrice R
452   // on a toujours determinant (R) = 1 et R-1 = R transposee.
453   if (shape == gp_Identity) { }
454   else if (shape == gp_Translation || shape == gp_PntMirror) loc.Reverse();
455   else if (shape == gp_Scale) {
456     Standard_ConstructionError_Raise_if (Abs(scale) <= gp::Resolution(), "gp_Trsf::Invert() - transformation has zero scale");
457     scale = 1.0 / scale;
458     loc.Multiply (-scale);
459   }
460   else {
461     Standard_ConstructionError_Raise_if (Abs(scale) <= gp::Resolution(), "gp_Trsf::Invert() - transformation has zero scale");
462     scale = 1.0 / scale;
463     matrix.Transpose ();
464     loc.Multiply (matrix);
465     loc.Multiply (-scale);
466   }
467 }
468
469 //=======================================================================
470 //function : Multiply
471 //purpose  : 
472 //=======================================================================
473
474 void gp_Trsf::Multiply(const gp_Trsf& T)
475 {
476   if (T.shape == gp_Identity) { }
477   else if (shape == gp_Identity) {
478     shape = T.shape;
479     scale = T.scale;
480     loc = T.loc;
481     matrix = T.matrix;
482   } 
483   else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
484     if (T.loc.X() != 0.0 || T.loc.Y() != 0.0 || T.loc.Z() != 0.0) {
485       loc.Add (T.loc.Multiplied (matrix));
486     }
487     matrix.Multiply(T.matrix);
488   }
489   else if (shape == gp_Translation && T.shape == gp_Translation) {
490     loc.Add (T.loc);
491   }
492   else if (shape == gp_Scale && T.shape == gp_Scale) {
493     loc.Add (T.loc.Multiplied(scale));
494     scale = scale * T.scale;
495   }
496   else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
497     scale = 1.0;
498     shape = gp_Translation;
499     loc.Add (T.loc.Reversed());
500   }
501   else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
502     shape = gp_Rotation;
503     loc.Add (T.loc.Multiplied (matrix));
504     matrix.Multiply(T.matrix);
505   }
506   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
507             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
508            && T.shape == gp_Translation) {
509     gp_XYZ Tloc(T.loc);
510     Tloc.Multiply(matrix);
511     if (scale != 1.0) { Tloc.Multiply(scale); }
512     loc.Add (Tloc);
513   }
514   else if ((shape == gp_Scale || shape == gp_PntMirror)
515            && T.shape == gp_Translation) {
516     gp_XYZ Tloc(T.loc);
517     Tloc.Multiply (scale);
518     loc.Add (Tloc);
519   }
520   else if (shape == gp_Translation && 
521            (T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
522             T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)) {
523     shape = gp_CompoundTrsf;
524     scale = T.scale;
525     loc.Add (T.loc);
526     matrix = T.matrix;
527   }
528   else if (shape == gp_Translation && 
529            (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
530     shape = T.shape;
531     loc.Add (T.loc);
532     scale = T.scale;
533   }
534   else if ((shape == gp_PntMirror || shape == gp_Scale) &&
535            (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
536     shape = gp_CompoundTrsf;
537     gp_XYZ Tloc(T.loc);
538     Tloc.Multiply (scale);
539     loc.Add (Tloc);
540     scale = scale * T.scale;
541   }
542   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
543             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
544            && (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
545     shape = gp_CompoundTrsf;
546     gp_XYZ Tloc(T.loc);
547     if (scale == 1.0) {
548       scale = T.scale;
549       Tloc.Multiply(matrix);
550     }
551     else {
552       Tloc.Multiply (matrix);
553       Tloc.Multiply (scale);
554       scale = scale * T.scale;
555     }
556     loc.Add (Tloc);
557   }
558   else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
559             T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)
560            && (shape == gp_Scale || shape == gp_PntMirror)) {
561     shape = gp_CompoundTrsf;
562     gp_XYZ Tloc(T.loc);
563     Tloc.Multiply(scale);
564     loc.Add (Tloc);
565     scale = scale * T.scale;
566     matrix = T.matrix;
567   }
568   else {
569     shape = gp_CompoundTrsf;
570     gp_XYZ Tloc(T.loc);
571     Tloc.Multiply (matrix);
572     if (scale != 1.0) { 
573       Tloc.Multiply (scale);
574       scale = scale * T.scale;
575     }
576     else { scale = T.scale; }
577     loc.Add (Tloc);
578     matrix.Multiply(T.matrix);
579   }
580 }
581
582 //=======================================================================
583 //function : Power
584 //purpose  : 
585 //=======================================================================
586
587 void gp_Trsf::Power (const Standard_Integer N)
588 {
589   if (shape == gp_Identity) { }
590   else {
591     if (N == 0)  {
592       scale = 1.0;
593       shape = gp_Identity;
594       matrix.SetIdentity();
595       loc = gp_XYZ (0.0, 0.0, 0.0);
596     }
597     else if (N == 1)  { }
598     else if (N == -1) { Invert(); }
599     else {
600       if (N < 0) { Invert(); }
601       if (shape == gp_Translation) {
602         Standard_Integer Npower = N;
603         if (Npower < 0) Npower = - Npower;
604         Npower--;
605         gp_XYZ Temploc = loc;
606         for(;;) {
607           if (IsOdd(Npower))  loc.Add (Temploc);
608           if (Npower == 1) break;
609           Temploc.Add (Temploc);
610           Npower = Npower/2;
611         }
612       }
613       else if (shape == gp_Scale) {
614         Standard_Integer Npower = N;
615         if (Npower < 0) Npower = - Npower;
616         Npower--;
617         gp_XYZ Temploc = loc;
618         Standard_Real Tempscale = scale;
619         for(;;) {
620           if (IsOdd(Npower)) {
621             loc.Add (Temploc.Multiplied(scale));
622             scale = scale * Tempscale;
623           }
624           if (Npower == 1) break;
625           Temploc.Add (Temploc.Multiplied(Tempscale));
626           Tempscale = Tempscale * Tempscale;
627           Npower = Npower/2;
628         }
629       }
630       else if (shape == gp_Rotation) {
631         Standard_Integer Npower = N;
632         if (Npower < 0) Npower = - Npower;
633         Npower--;
634         gp_Mat Tempmatrix (matrix);
635         if (loc.X() == 0.0 && loc.Y() == 0.0 && loc.Z() == 0.0) {
636           for(;;) {
637             if (IsOdd(Npower)) matrix.Multiply (Tempmatrix);
638             if (Npower == 1)   break;
639             Tempmatrix.Multiply (Tempmatrix);
640             Npower = Npower/2;
641           }
642         }
643         else {
644           gp_XYZ Temploc = loc;
645           for(;;) {
646             if (IsOdd(Npower)) {
647               loc.Add (Temploc.Multiplied (matrix));
648               matrix.Multiply (Tempmatrix);
649             }
650             if (Npower == 1) break;
651             Temploc.Add (Temploc.Multiplied (Tempmatrix));
652             Tempmatrix.Multiply (Tempmatrix);
653             Npower = Npower/2;
654           }
655         }
656       }
657       else if (shape == gp_PntMirror || shape == gp_Ax1Mirror ||
658                shape == gp_Ax2Mirror) {
659         if (IsEven (N)) {
660           shape = gp_Identity;
661           scale = 1.0;
662           matrix.SetIdentity ();
663           loc.SetX(0);
664           loc.SetY(0);
665           loc.SetZ(0);
666         }
667       }
668       else {
669         shape = gp_CompoundTrsf;
670         Standard_Integer Npower = N;
671         if (Npower < 0) Npower = - Npower;
672         Npower--;
673         gp_XYZ Temploc = loc;
674         Standard_Real Tempscale = scale;
675         gp_Mat Tempmatrix (matrix);
676         for(;;) {
677           if (IsOdd(Npower)) {
678             loc.Add ((Temploc.Multiplied (matrix)).Multiplied (scale));
679             scale = scale * Tempscale;
680             matrix.Multiply (Tempmatrix);
681           }
682           if (Npower == 1) break;
683           Tempscale = Tempscale * Tempscale;
684           Temploc.Add ( (Temploc.Multiplied (Tempmatrix)).Multiplied 
685                         (Tempscale)
686                         );
687           Tempmatrix.Multiply (Tempmatrix);
688           Npower = Npower/2;
689         }
690       }
691     }
692   }
693 }
694
695 //=======================================================================
696 //function : PreMultiply
697 //purpose  : 
698 //=======================================================================
699
700 void gp_Trsf::PreMultiply (const gp_Trsf& T)
701 {
702   if (T.shape == gp_Identity) { }
703   else if (shape == gp_Identity) {
704     shape = T.shape;
705     scale = T.scale;
706     loc = T.loc;
707     matrix = T.matrix;
708   } 
709   else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
710     loc.Multiply (T.matrix);
711     loc.Add (T.loc);
712     matrix.PreMultiply(T.matrix);
713   }
714   else if (shape == gp_Translation && T.shape == gp_Translation) {
715     loc.Add (T.loc);
716   }
717   else if (shape == gp_Scale && T.shape == gp_Scale) {
718     loc.Multiply (T.scale);
719     loc.Add (T.loc);
720     scale = scale * T.scale;
721   }
722   else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
723     scale = 1.0;
724     shape = gp_Translation;
725     loc.Reverse();
726     loc.Add (T.loc);
727   }
728   else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
729     shape = gp_Rotation;
730     loc.Multiply (T.matrix);
731     loc.Add (T.loc);
732     matrix.PreMultiply(T.matrix);
733   }
734   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
735             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
736            && T.shape == gp_Translation) {
737     loc.Add (T.loc);
738   }
739   else if ((shape == gp_Scale || shape == gp_PntMirror)
740            && T.shape == gp_Translation) {
741     loc.Add (T.loc);
742   }
743   else if (shape == gp_Translation && 
744            (T.shape == gp_CompoundTrsf || T.shape == gp_Rotation
745             || T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)) {
746     shape = gp_CompoundTrsf;
747     matrix = T.matrix;
748     if (T.scale == 1.0)  loc.Multiply (T.matrix);
749     else {
750       scale = T.scale;
751       loc.Multiply (matrix);
752       loc.Multiply (scale);
753     }
754     loc.Add (T.loc);
755   }
756   else if ((T.shape == gp_Scale || T.shape == gp_PntMirror)
757            && shape == gp_Translation) {
758     loc.Multiply (T.scale);
759     loc.Add (T.loc);
760     scale = T.scale;
761     shape = T.shape;
762   }
763   else if ((shape == gp_PntMirror || shape == gp_Scale) &&
764            (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
765     shape = gp_CompoundTrsf;
766     loc.Multiply (T.scale);
767     loc.Add (T.loc);
768     scale = scale * T.scale;
769   }
770   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
771             shape == gp_Ax1Mirror || shape == gp_Ax2Mirror) 
772            && (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
773     shape = gp_CompoundTrsf;
774     loc.Multiply (T.scale);
775     loc.Add (T.loc);
776     scale = scale * T.scale;
777   } 
778   else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
779             T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror) 
780            && (shape == gp_Scale || shape == gp_PntMirror)) {
781     shape = gp_CompoundTrsf;
782     matrix = T.matrix;
783     if (T.scale == 1.0)  loc.Multiply (T.matrix);
784     else {
785       loc.Multiply (matrix);
786       loc.Multiply (T.scale);
787       scale = T.scale * scale;
788     }
789     loc.Add (T.loc);
790   } 
791   else {
792     shape = gp_CompoundTrsf;
793     loc.Multiply (T.matrix);
794     if (T.scale != 1.0) {
795       loc.Multiply (T.scale);    scale = scale * T.scale;
796     }
797     loc.Add (T.loc);
798     matrix.PreMultiply(T.matrix);
799   }
800 }
801
802 //=======================================================================
803 //function : GetRotation
804 //purpose  : algorithm from A.Korn, M.Korn, "Mathematical Handbook for
805 //           scientists and Engineers" McGraw-Hill, 1961, ch.14.10-2.
806 //=======================================================================
807
808 Standard_Boolean gp_Trsf::GetRotation (gp_XYZ&        theAxis,
809                                        Standard_Real& theAngle) const
810 {
811   gp_Quaternion Q = GetRotation();
812   gp_Vec aVec;
813   Q.GetVectorAndAngle (aVec, theAngle);
814   theAxis = aVec.XYZ();
815   return Standard_True;
816 }
817
818 //=======================================================================
819 //function : Orthogonalize
820 //purpose  : 
821 //ATTENTION!!!
822 //      Orthogonalization is not equivalent transformation. Therefore, 
823 //        transformation with source matrix and with orthogonalized matrix can
824 //        lead to different results for one shape. Consequently, source matrix must
825 //        be close to orthogonalized matrix for reducing these differences.
826 //=======================================================================
827 void gp_Trsf::Orthogonalize()
828 {
829   //Matrix M is called orthogonal if and only if
830   //    M*Transpose(M) == E
831   //where E is identity matrix.
832
833   //Set of all rows (as of all columns) of matrix M (for gp_Trsf class) is
834   //orthonormal basis. If this condition is not satisfied then the basis can be
835   //orthonormalized in accordance with below described algorithm.
836
837   //In 3D-space, we have the linear span of three basis vectors: V1, V2 and V3.
838   //Correspond orthonormalized basis is formed by vectors Vn1, Vn2 and Vn3.
839
840   //In this case,
841   //    Vn_{i}*Vn_{j} = (i == j)? 1 : 0.
842
843   //The algorithm includes following steps:
844
845   //1. Normalize V1 vector:
846   //    V1n=V1/|V1|;
847   //
848   //2. Let
849   //    V2n=V2-m*V1n.
850   //    
851   //After multiplication two parts of this equation by V1n,
852   //we will have following equation:
853   //    0=V2*V1n-m <==> m=V2*V1n.
854   //    
855   //Consequently,
856   //    V2n=V2-(V2*V1n)*V1n.
857
858   //3. Let
859   //    V3n=V3-m1*V1n-m2*V2n.
860   //    
861   //After multiplication two parts of this equation by V1n,
862   //we will have following equation:
863   //    0=V3*V1n-m1 <==> m1=V3*V1n.
864   //    
865   //After multiplication two parts of main equation by V2n,
866   //we will have following equation:
867   //    0=V3*V2n-m2 <==> m2=V3*V2n.
868   //    
869   //In conclusion,
870   //    V3n=V3-(V3*V1n)*V1n-(V3*V2n)*V2n.
871
872   gp_Mat aTM(matrix);
873
874   gp_XYZ aV1 = aTM.Column(1);
875   gp_XYZ aV2 = aTM.Column(2);
876   gp_XYZ aV3 = aTM.Column(3);
877
878   aV1.Normalize();
879
880   aV2 -= aV1*(aV2.Dot(aV1));
881   aV2.Normalize();
882
883   aV3 -= aV1*(aV3.Dot(aV1)) + aV2*(aV3.Dot(aV2));
884   aV3.Normalize();
885
886   aTM.SetCols(aV1, aV2, aV3);
887
888   aV1 = aTM.Row(1);
889   aV2 = aTM.Row(2);
890   aV3 = aTM.Row(3);
891
892   aV1.Normalize();
893
894   aV2 -= aV1*(aV2.Dot(aV1));
895   aV2.Normalize();
896
897   aV3 -= aV1*(aV3.Dot(aV1)) + aV2*(aV3.Dot(aV2));
898   aV3.Normalize();
899
900   aTM.SetRows(aV1, aV2, aV3);
901
902   matrix = aTM;
903 }
904
905 //=======================================================================
906 //function : DumpJson
907 //purpose  : 
908 //=======================================================================
909 void gp_Trsf::DumpJson (Standard_OStream& theOStream, Standard_Integer) const
910 {
911   OCCT_DUMP_VECTOR_CLASS (theOStream, "Location", 3, loc.X(), loc.Y(), loc.Z())
912   OCCT_DUMP_VECTOR_CLASS (theOStream, "Matrix", 9, matrix.Value(1, 1), matrix.Value(1, 2), matrix.Value(1, 3),
913                                                    matrix.Value(2, 1), matrix.Value(2, 2), matrix.Value(2, 3),
914                                                    matrix.Value(3, 1), matrix.Value(3, 2), matrix.Value(3, 3))
915   OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, shape)
916   OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, scale)
917 }