0032137: Coding Rules - merge redundant .lxx files into header files within Package gp
[occt.git] / src / gp / gp_Trsf2d.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 08/01/91 Modif introduction des classes Mat2d et XY dans gp
16
17 #define No_Standard_OutOfRange
18
19 #include <gp_Trsf2d.hxx>
20
21 #include <gp.hxx>
22 #include <gp_Ax2d.hxx>
23 #include <gp_GTrsf2d.hxx>
24 #include <gp_Mat2d.hxx>
25 #include <gp_Pnt2d.hxx>
26 #include <gp_Trsf.hxx>
27 #include <gp_Vec2d.hxx>
28 #include <gp_XY.hxx>
29 #include <Standard_ConstructionError.hxx>
30 #include <Standard_OutOfRange.hxx>
31
32 void gp_Trsf2d::SetMirror (const gp_Ax2d& A)
33 {
34   shape = gp_Ax1Mirror;
35   scale = - 1.0;
36   const gp_Dir2d& V = A.Direction ();
37   const gp_Pnt2d& P = A.Location ();
38   Standard_Real VX = V.X();
39   Standard_Real VY = V.Y();
40   Standard_Real X0 = P.X();
41   Standard_Real Y0 = P.Y();
42   matrix.SetCol (1, gp_XY (1.0 - 2.0 * VX * VX, -2.0 * VX * VY));
43   matrix.SetCol (2, gp_XY (-2.0 * VX * VY, 1.0 - 2.0 * VY * VY));
44
45   loc.SetCoord  (-2.0 * ((VX * VX - 1.0) * X0 + (VX * VY * Y0)),
46                  -2.0 * ((VX * VY * X0) + (VY * VY - 1.0) * Y0));
47 }
48
49 void gp_Trsf2d::SetTransformation (const gp_Ax2d& FromA1,
50                                    const gp_Ax2d& ToA2)
51 {
52   shape = gp_CompoundTrsf;
53   scale = 1.0;
54   //matrix from XOY to A2 :
55   const gp_XY& V1 = ToA2.Direction().XY();
56   gp_XY V2 (-V1.Y(), V1.X());
57   matrix.SetCol (1, V1);
58   matrix.SetCol (2, V2);
59   loc = ToA2.Location().XY();
60   matrix.Transpose();
61   loc.Multiply (matrix);
62   loc.Reverse();
63   //matrix FromA1 to XOY
64   const gp_XY& V3 = FromA1.Direction().XY();
65   gp_XY V4 (-V3.Y(), V3.X());
66   gp_Mat2d MA1 (V3, V4);
67   gp_XY MA1loc = FromA1.Location().XY();
68   //matrix * MA1 => FromA1 ToA2
69   MA1loc.Multiply (matrix);
70   loc.Add (MA1loc);
71   matrix.Multiply (MA1);
72 }
73
74 void gp_Trsf2d::SetTransformation (const gp_Ax2d& A)
75 {
76   shape = gp_CompoundTrsf;
77   scale = 1.0;
78   const gp_XY& V1 = A.Direction().XY();
79   gp_XY V2 (-V1.Y(), V1.X());
80   matrix.SetCol (1, V1);
81   matrix.SetCol (2, V2);
82   loc = A.Location().XY();
83   matrix.Transpose();
84   loc.Multiply (matrix);
85   loc.Reverse();
86 }
87
88 void gp_Trsf2d::SetTranslationPart (const gp_Vec2d& V)
89 {   
90   loc = V.XY();
91   Standard_Real X = loc.X();
92   if (X < 0) X = - X;
93   Standard_Real Y = loc.Y();
94   if (Y < 0) Y = - Y;
95   if (X <= gp::Resolution() && Y <= gp::Resolution()) {
96     if (shape == gp_Identity  || shape == gp_PntMirror ||
97         shape == gp_Scale     || shape == gp_Rotation  ||
98         shape == gp_Ax1Mirror ) { }
99     else if (shape == gp_Translation) { shape = gp_Identity; }
100     else { shape = gp_CompoundTrsf; }
101   }
102   else {
103     if (shape == gp_Translation || shape == gp_Scale ||
104         shape == gp_PntMirror) { }
105     else if (shape == gp_Identity) { shape = gp_Translation; }
106     else { shape = gp_CompoundTrsf; }
107   }
108 }
109
110 void gp_Trsf2d::SetScaleFactor (const Standard_Real S)
111 {   
112   if (S == 1.0) {
113     Standard_Real X = loc.X();
114     if (X < 0) X = - X;
115     Standard_Real Y = loc.Y();
116     if (Y < 0) Y = - Y;
117     if (X <= gp::Resolution() && Y <= gp::Resolution()) {
118       if (shape == gp_Identity || shape == gp_Rotation) { }
119       else if (shape == gp_Scale)  { shape = gp_Identity; }
120       else if (shape == gp_PntMirror) { shape = gp_Translation; }
121       else { shape = gp_CompoundTrsf; }
122     }
123     else {
124       if (shape == gp_Identity || shape == gp_Rotation ||
125           shape == gp_Scale)  { }
126       else if (shape == gp_PntMirror) { shape = gp_Translation; }
127       else { shape = gp_CompoundTrsf; }
128     }
129   }
130   else if (S == -1) {
131     if (shape == gp_PntMirror || shape == gp_Ax1Mirror) { }
132     else if (shape == gp_Identity || shape == gp_Scale) { 
133       shape = gp_PntMirror;
134     }
135     else { shape = gp_CompoundTrsf; }
136   }
137   else {
138     if (shape == gp_Scale) { }
139     else if (shape == gp_Identity || shape == gp_Translation ||
140              shape == gp_PntMirror) { shape = gp_Scale; }
141     else { shape = gp_CompoundTrsf; }
142   }
143   scale = S;
144 }
145
146 gp_Mat2d gp_Trsf2d::VectorialPart () const
147
148   if (scale == 1.0)  return matrix;
149   gp_Mat2d M = matrix;
150   if (shape == gp_Scale || shape == gp_PntMirror)
151     M.SetDiagonal (matrix.Value(1,1) * scale, matrix.Value(2,2) * scale);
152   else
153     M.Multiply (scale);
154   return M;
155 }
156
157 Standard_Real gp_Trsf2d::RotationPart () const
158
159   return ATan2 ( matrix.Value(2,1), matrix.Value(1,1) );
160 }
161
162 void gp_Trsf2d::Invert()
163
164   //                                    -1
165   //  X' = scale * R * X + T  =>  X = (R  / scale)  * ( X' - T)
166   //
167   // Pour les gp_Trsf2d puisque le scale est extrait de la matrice R
168   // on a toujours determinant (R) = 1 et R-1 = R transposee.
169   if (shape == gp_Identity) { }
170   else if ( shape == gp_Translation || shape == gp_PntMirror) { 
171     loc.Reverse(); 
172   }
173   else if ( shape == gp_Scale) {
174     Standard_Real As = scale;
175     if (As < 0) As = - As;
176     Standard_ConstructionError_Raise_if (As <= gp::Resolution(), "gp_Trsf2d::Invert() - transformation has zero scale");
177     scale = 1.0 / scale;
178     loc.Multiply (-scale);
179   }
180   else {
181     Standard_Real As = scale;
182     if (As < 0) As = - As;
183     Standard_ConstructionError_Raise_if (As <= gp::Resolution(), "gp_Trsf2d::Invert() - transformation has zero scale");
184     scale = 1.0 / scale;
185     matrix.Transpose();
186     loc.Multiply (matrix);
187     loc.Multiply (-scale);
188   }
189 }
190
191 void gp_Trsf2d::Multiply(const gp_Trsf2d& T)
192 {
193   if (T.shape == gp_Identity) { }
194   else if (shape == gp_Identity) {
195     shape = T.shape;
196     scale = T.scale;
197     loc = T.loc;
198     matrix = T.matrix;
199   } 
200   else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
201     if (loc.X() != 0.0 || loc.Y() != 0.0) {
202       loc.Add (T.loc.Multiplied (matrix));
203     }
204     matrix.Multiply(T.matrix);
205   }
206   else if (shape == gp_Translation && T.shape == gp_Translation) {
207     loc.Add (T.loc);
208   }
209   else if (shape == gp_Scale && T.shape == gp_Scale) {
210     loc.Add (T.loc.Multiplied(scale));
211     scale = scale * T.scale;
212   }
213   else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
214     scale = 1.0;
215     shape = gp_Translation;
216     loc.Add (T.loc.Reversed());
217   }
218   else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
219     shape = gp_Rotation;
220     gp_XY Tloc (T.loc);
221     Tloc.Multiply (matrix);
222     Tloc.Multiply (scale);
223     scale = scale * T.scale;
224     loc.Add (Tloc);
225     matrix.Multiply (T.matrix);
226   }
227   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
228             shape == gp_Ax1Mirror) && T.shape == gp_Translation) {
229     gp_XY Tloc (T.loc);
230     Tloc.Multiply (matrix);
231     if (scale != 1.0) Tloc.Multiply (scale);
232     loc.Add (Tloc);
233   }
234   else if ((shape == gp_Scale || shape == gp_PntMirror)
235            && T.shape == gp_Translation) {
236     gp_XY Tloc (T.loc);
237     Tloc.Multiply (scale);
238     loc.Add (Tloc);
239   }
240   else if (shape == gp_Translation &&
241            (T.shape == gp_CompoundTrsf ||
242             T.shape == gp_Rotation || T.shape == gp_Ax1Mirror)) {
243     shape = gp_CompoundTrsf;
244     scale = T.scale;
245     loc.Add (T.loc);
246     matrix = T.matrix;
247   }
248   else if (shape == gp_Translation && 
249            (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
250     shape = T.shape;
251     loc.Add (T.loc);
252     scale = T.scale;
253   }
254   else if ((shape == gp_PntMirror || shape == gp_Scale) &&
255            (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
256     shape = gp_CompoundTrsf;
257     gp_XY Tloc (T.loc);
258     Tloc.Multiply (scale);
259     loc.Add (Tloc);
260     scale = scale * T.scale;
261   }
262   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
263             shape == gp_Ax1Mirror)
264            && (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
265     shape = gp_CompoundTrsf;
266     gp_XY Tloc (T.loc);
267     Tloc.Multiply(matrix);
268     if (scale == 1.0)  scale = T.scale;
269     else {
270       Tloc.Multiply (scale);
271       scale = scale * T.scale;
272     }
273     loc.Add (Tloc);
274   }
275   else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
276             T.shape == gp_Ax1Mirror)
277            && (shape == gp_Scale || shape == gp_PntMirror)) {
278     shape = gp_CompoundTrsf;
279     gp_XY Tloc (T.loc);
280     Tloc.Multiply (scale);
281     scale = scale * T.scale;
282     loc.Add (Tloc);
283     matrix = T.matrix;
284   }
285   else {
286     shape = gp_CompoundTrsf;
287     gp_XY Tloc (T.loc);
288     Tloc.Multiply (matrix);
289     if (scale != 1.0) {
290       Tloc.Multiply (scale);
291       scale = scale * T.scale;
292     }
293     else { scale = T.scale; }
294     loc.Add (Tloc);
295     matrix.Multiply (T.matrix);
296   }
297 }
298
299 void gp_Trsf2d::Power (const Standard_Integer N)
300 {
301   if (shape == gp_Identity) { }
302   else {
303     if (N == 0)  {
304       scale = 1.0;
305       shape = gp_Identity;
306       matrix.SetIdentity();
307       loc = gp_XY (0.0, 0.0);
308     }
309     else if (N == 1)  { }
310     else if (N == -1) Invert();
311     else {
312       if (N < 0) Invert();
313       if (shape == gp_Translation) { 
314         Standard_Integer Npower = N;
315         if (Npower < 0) Npower = - Npower;
316         Npower--;
317         gp_XY Temploc = loc;
318         for(;;) {
319           if (IsOdd(Npower))  loc.Add (Temploc);
320           if (Npower == 1) break;
321           Temploc.Add (Temploc);
322           Npower = Npower/2;
323         }
324       }
325       else if (shape == gp_Scale) {
326         Standard_Integer Npower = N;
327         if (Npower < 0) Npower = - Npower;
328         Npower--;
329         gp_XY Temploc = loc;
330         Standard_Real Tempscale = scale;
331         for(;;) {
332           if (IsOdd(Npower)) {
333             loc.Add (Temploc.Multiplied(scale));
334             scale = scale * Tempscale;
335           }
336           if (Npower == 1) break;
337           Temploc.Add (Temploc.Multiplied(Tempscale));
338           Tempscale = Tempscale * Tempscale;
339           Npower = Npower/2;
340         }
341       }
342       else if (shape == gp_Rotation) {
343         Standard_Integer Npower = N;
344         if (Npower < 0) Npower = - Npower;
345         Npower--;
346         gp_Mat2d Tempmatrix (matrix);
347         if (loc.X() == 0.0 && loc.Y() == 0.0) {
348           for(;;) {
349             if (IsOdd(Npower))  matrix.Multiply (Tempmatrix);
350             if (Npower == 1) break;
351             Tempmatrix.Multiply (Tempmatrix);
352             Npower = Npower/2;
353           }
354         }
355         else {
356           gp_XY Temploc = loc;
357           for(;;) {
358             if (IsOdd(Npower)) {
359               loc.Add (Temploc.Multiplied (matrix));
360               matrix.Multiply (Tempmatrix);
361             }
362             if (Npower == 1) break;
363             Temploc.Add (Temploc.Multiplied (Tempmatrix));
364             Tempmatrix.Multiply (Tempmatrix);
365             Npower = Npower/2;
366           }
367         }
368       }
369       else if (shape == gp_PntMirror || shape == gp_Ax1Mirror) {
370         if (IsEven (N)) {
371           shape = gp_Identity;
372           scale = 1.0;
373           matrix.SetIdentity ();
374           loc.SetCoord (0.0, 0.0);
375         }
376       }
377       else {
378         shape = gp_CompoundTrsf;
379         Standard_Integer Npower = N;
380         if (Npower < 0) Npower = - Npower;
381         Npower--;
382         matrix.SetDiagonal (scale*matrix.Value(1,1),
383                             scale*matrix.Value(2,2));
384         gp_XY Temploc = loc;
385         Standard_Real Tempscale = scale;
386         gp_Mat2d Tempmatrix (matrix);
387         for(;;) {
388           if (IsOdd(Npower)) {
389             loc.Add ((Temploc.Multiplied (matrix)).Multiplied (scale));
390             scale = scale * Tempscale;
391             matrix.Multiply (Tempmatrix);
392           }
393           if (Npower == 1) break;
394           Tempscale = Tempscale * Tempscale;
395           Temploc.Add ( (Temploc.Multiplied (Tempmatrix)).Multiplied 
396                        (Tempscale)
397                        );
398           Tempmatrix.Multiply (Tempmatrix);
399           Npower = Npower/2;
400         }
401       }
402     }
403   }
404 }
405
406 void gp_Trsf2d::PreMultiply (const gp_Trsf2d& T)
407 {
408   if (T.shape == gp_Identity) { }
409   else if (shape == gp_Identity) {
410     shape = T.shape;
411     scale = T.scale;
412     loc = T.loc;
413     matrix = T.matrix;
414   } 
415   else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
416     loc.Multiply (T.matrix);
417     loc.Add (T.loc);
418     matrix.PreMultiply(T.matrix);
419   }
420   else if (shape == gp_Translation && T.shape == gp_Translation) {
421     loc.Add (T.loc);
422   }
423   else if (shape == gp_Scale && T.shape == gp_Scale) {
424     loc.Multiply (T.scale);
425     loc.Add (T.loc);
426     scale = scale * T.scale;
427   }
428   else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
429     scale = 1.0;
430     shape = gp_Translation;
431     loc.Reverse();
432     loc.Add (T.loc);
433   }
434   else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
435     shape = gp_Rotation;
436     loc.Multiply (T.matrix);
437     loc.Multiply(T.scale);
438     scale = scale * T.scale;
439     loc.Add (T.loc);
440     matrix.PreMultiply(T.matrix);
441   }
442   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
443             shape == gp_Ax1Mirror) && T.shape == gp_Translation) {
444     loc.Add (T.loc);
445   }
446   else if ((shape == gp_Scale || shape == gp_PntMirror)
447            && T.shape == gp_Translation) {
448     loc.Add (T.loc);
449   }
450   else if (shape == gp_Translation &&
451            (T.shape == gp_CompoundTrsf ||
452             T.shape == gp_Rotation || T.shape == gp_Ax1Mirror)) {
453     shape = gp_CompoundTrsf;
454     matrix = T.matrix;
455     if (T.scale == 1.0)  loc.Multiply (T.matrix);
456     else {
457       scale = T.scale;
458       loc.Multiply (matrix);
459       loc.Multiply (scale);
460     }
461     loc.Add (T.loc);
462   }
463   else if ((T.shape == gp_Scale || T.shape == gp_PntMirror)
464            && shape == gp_Translation) {
465     loc.Multiply (T.scale);
466     loc.Add (T.loc);
467     scale = T.scale;
468     shape = T.shape;
469   }
470   else if ((shape == gp_PntMirror || shape == gp_Scale) &&
471            (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
472     shape = gp_CompoundTrsf;
473     loc.Multiply (T.scale);
474     loc.Add (T.loc);
475     scale = scale * T.scale;
476   }
477   else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
478             shape == gp_Ax1Mirror)
479            && (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
480     shape = gp_CompoundTrsf;
481     loc.Multiply (T.scale);
482     loc.Add (T.loc);
483     scale = scale * T.scale;
484   } 
485   else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
486             T.shape == gp_Ax1Mirror) 
487            && (shape == gp_Scale || shape == gp_PntMirror)) {
488     shape = gp_CompoundTrsf;
489     matrix = T.matrix;
490     if (T.scale == 1.0)  loc.Multiply (T.matrix);
491     else {
492       loc.Multiply (matrix);
493       loc.Multiply (T.scale);
494       scale = T.scale * scale;
495     }
496     loc.Add (T.loc);
497   } 
498   else {
499     shape = gp_CompoundTrsf;
500     loc.Multiply (T.matrix);
501     if (T.scale != 1.0) {
502       loc.Multiply(T.scale);   scale = scale * T.scale;
503     }
504     loc.Add (T.loc);
505     matrix.PreMultiply(T.matrix);
506   }
507 }
508
509 //=======================================================================
510 //function : SetValues
511 //purpose  : 
512 //=======================================================================
513 void gp_Trsf2d::SetValues(const Standard_Real a11,
514                           const Standard_Real a12,
515                           const Standard_Real a13,
516                           const Standard_Real a21,
517                           const Standard_Real a22,
518                           const Standard_Real a23)
519 {
520   gp_XY col1(a11,a21);
521   gp_XY col2(a12,a22);
522   gp_XY col3(a13,a23);
523   // compute the determinant
524   gp_Mat2d M(col1,col2);
525   Standard_Real s = M.Determinant();
526   Standard_Real As = s;
527   if (As < 0)
528     As = - As;
529   Standard_ConstructionError_Raise_if
530     (As < gp::Resolution(),"gp_Trsf2d::SetValues, null determinant");
531
532   if (s > 0)
533     s = sqrt(s);
534   else
535     s = sqrt(-s);
536   
537   M.Divide(s);
538
539   scale = s;
540   shape = gp_CompoundTrsf;
541
542   matrix = M;
543   Orthogonalize();
544   
545   loc = col3;
546 }
547
548
549 //=======================================================================
550 //function : Orthogonalize
551 //purpose  : 
552 //ATTENTION!!!
553 //      Orthogonalization is not equivalent transformation.Therefore, transformation with
554 //        source matrix and with orthogonalized matrix can lead to different results for
555 //        one shape. Consequently, source matrix must be close to orthogonalized 
556 //        matrix for reducing these differences.
557 //=======================================================================
558 void gp_Trsf2d::Orthogonalize()
559 {
560   //See correspond comment in gp_Trsf::Orthogonalize() method in order to make this
561   //algorithm clear.
562
563   gp_Mat2d aTM(matrix);
564
565   gp_XY aV1 = aTM.Column(1);
566   gp_XY aV2 = aTM.Column(2);
567
568   aV1.Normalize();
569
570   aV2 -= aV1*(aV2.Dot(aV1));
571   aV2.Normalize();
572
573   aTM.SetCols(aV1, aV2);
574
575   aV1 = aTM.Row(1);
576   aV2 = aTM.Row(2);
577
578   aV1.Normalize();
579
580   aV2 -= aV1*(aV2.Dot(aV1));
581   aV2.Normalize();
582
583   aTM.SetRows(aV1, aV2);
584
585   matrix = aTM;
586 }