0023361: Bug in gp_Trsf::Multiply
[occt.git] / src / gp / gp_Trsf.cxx
CommitLineData
b311480e 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
7fd59977 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
35void 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
56void 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
76void 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
93void 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
106void 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
124void 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
152void 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
170void 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
183void 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
213void 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
253void 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
300void 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
395gp_Quaternion gp_Trsf::GetRotation () const
396{
397 return gp_Quaternion (matrix);
398}
399
400//=======================================================================
401//function : VectorialPart
402//purpose :
403//=======================================================================
404
405gp_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
423void 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
457void 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) {
f6f03db9 467 if (T.loc.X() != 0.0 || T.loc.Y() != 0.0 || T.loc.Z() != 0.0) {
7fd59977 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
570void 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
683void 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
791Standard_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}