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