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