1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
4 // This file is part of Open CASCADE Technology software library.
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.
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
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
20 #define No_Standard_OutOfRange
27 #include <gp_GTrsf.hxx>
30 #include <gp_Quaternion.hxx>
31 #include <gp_Trsf.hxx>
32 #include <gp_Trsf2d.hxx>
35 #include <Standard_ConstructionError.hxx>
36 #include <Standard_OutOfRange.hxx>
37 #include <Standard_Dump.hxx>
39 //=======================================================================
41 //purpose : Constructor from 2d
42 //=======================================================================
43 gp_Trsf::gp_Trsf (const gp_Trsf2d& T) :
44 scale(T.ScaleFactor()),
46 loc(T.TranslationPart().X(),T.TranslationPart().Y(), 0.0)
48 const gp_Mat2d& M = T.HVectorialPart();
54 if (shape == gp_Ax1Mirror)
61 //=======================================================================
62 //function : SetMirror
64 //=======================================================================
66 void gp_Trsf::SetMirror (const gp_Ax1& A1)
70 loc = A1.Location().XYZ();
71 matrix.SetDot(A1.Direction().XYZ());
73 matrix.SetDiagonal (matrix.Value (1,1) + 1,
74 matrix.Value (2,2) + 1,
75 matrix.Value (3,3) + 1);
77 loc.Multiply (matrix);
78 loc.Add (A1.Location().XYZ());
82 //=======================================================================
83 //function : SetMirror
85 //=======================================================================
87 void gp_Trsf::SetMirror (const gp_Ax2& A2)
91 loc = A2.Location().XYZ();
92 matrix.SetDot(A2.Direction().XYZ());
94 matrix.SetDiagonal (matrix.Value (1,1) - 1,
95 matrix.Value (2,2) - 1,
96 matrix.Value (3,3) - 1);
98 loc.Multiply (matrix);
99 loc.Add (A2.Location().XYZ());
102 //=======================================================================
103 //function : SetRotation
105 //=======================================================================
107 void gp_Trsf::SetRotation (const gp_Ax1& A1,
108 const Standard_Real Ang)
112 loc = A1.Location().XYZ();
113 matrix.SetRotation (A1.Direction().XYZ(), Ang);
115 loc.Multiply (matrix);
116 loc.Add (A1.Location().XYZ());
119 //=======================================================================
120 //function : SetRotation
122 //=======================================================================
124 void gp_Trsf::SetRotation (const gp_Quaternion& R)
128 loc.SetCoord (0., 0., 0.);
129 matrix = R.GetMatrix();
132 //=======================================================================
133 //function : SetScale
135 //=======================================================================
137 void gp_Trsf::SetScale (const gp_Pnt& P, const Standard_Real S)
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 ();
150 //=======================================================================
151 //function : SetTransformation
153 //=======================================================================
155 void gp_Trsf::SetTransformation (const gp_Ax3& FromA1,
158 shape = gp_CompoundTrsf;
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);
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();
173 gp_Mat MA1 (xDir, yDir, zDir);
174 gp_XYZ MA1loc = FromA1.Location().XYZ();
176 // matrix * MA1 => FromA1 ToA2 :
177 MA1loc.Multiply (matrix);
179 matrix.Multiply (MA1);
182 void gp_Trsf::SetTransformation (const gp_Ax3& A3)
184 shape = gp_CompoundTrsf;
186 matrix.SetRows (A3.XDirection().XYZ(),
187 A3.YDirection().XYZ(),
188 A3. Direction().XYZ());
189 loc = A3.Location().XYZ();
190 loc.Multiply (matrix);
194 //=======================================================================
195 //function : SetTransformation
197 //=======================================================================
199 void gp_Trsf::SetTransformation (const gp_Quaternion& R, const gp_Vec& T)
201 shape = gp_CompoundTrsf;
204 matrix = R.GetMatrix();
207 //=======================================================================
208 //function : SetDisplacement
210 //=======================================================================
212 void gp_Trsf::SetDisplacement (const gp_Ax3& FromA1,
215 shape = gp_CompoundTrsf;
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);
228 gp_XYZ MA1loc = FromA1.Location().XYZ();
229 MA1loc.Multiply (MA1);
232 MA1loc.Multiply (matrix);
234 matrix.Multiply (MA1);
237 //=======================================================================
238 //function : SetTranslationPart
240 //=======================================================================
242 void gp_Trsf::SetTranslationPart (const gp_Vec& V) {
245 const Standard_Boolean locnull = (loc.SquareModulus() < gp::Resolution());
250 if (!locnull) shape = gp_Translation;
253 case gp_Translation :
254 if (locnull) shape = gp_Identity;
262 case gp_CompoundTrsf :
265 shape = gp_CompoundTrsf;
271 //=======================================================================
272 //function : SetScaleFactor
274 //=======================================================================
276 void gp_Trsf::SetScaleFactor (const Standard_Real S)
278 Standard_Real As = S;
279 if (As < 0) As = - As;
280 Standard_ConstructionError_Raise_if
281 (As <= gp::Resolution(),"gp_Trsf::SetScaleFactor");
284 if (As < 0) As = - As;
285 Standard_Boolean unit = As <= gp::Resolution(); // = (scale == 1)
287 if (As < 0) As = - As;
288 Standard_Boolean munit = As <= gp::Resolution(); // = (scale == -1)
292 case gp_Translation :
293 if (!unit) shape = gp_Scale;
294 if (munit) shape = gp_PntMirror;
297 if (!unit) shape = gp_CompoundTrsf;
302 if (!munit) shape = gp_Scale;
303 if (unit) shape = gp_Identity;
306 if (unit) shape = gp_Identity;
307 if (munit) shape = gp_PntMirror;
309 case gp_CompoundTrsf :
316 //=======================================================================
317 //function : SetValues
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 //=======================================================================
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)
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");
354 shape = gp_CompoundTrsf;
362 //=======================================================================
363 //function : GetRotation
365 //=======================================================================
367 gp_Quaternion gp_Trsf::GetRotation () const
369 return gp_Quaternion (matrix);
372 //=======================================================================
373 //function : VectorialPart
375 //=======================================================================
377 gp_Mat gp_Trsf::VectorialPart () const
379 if (scale == 1.0) return matrix;
381 if (shape == gp_Scale || shape == gp_PntMirror)
382 M.SetDiagonal(scale*M.Value(1,1),
390 //=======================================================================
393 //=======================================================================
395 void gp_Trsf::Invert()
398 // X' = scale * R * X + T => X = (R / scale) * ( X' - T)
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");
407 loc.Multiply (-scale);
410 Standard_ConstructionError_Raise_if (Abs(scale) <= gp::Resolution(), "gp_Trsf::Invert() - transformation has zero scale");
413 loc.Multiply (matrix);
414 loc.Multiply (-scale);
418 //=======================================================================
419 //function : Multiply
421 //=======================================================================
423 void gp_Trsf::Multiply(const gp_Trsf& T)
425 if (T.shape == gp_Identity) { }
426 else if (shape == gp_Identity) {
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));
436 matrix.Multiply(T.matrix);
438 else if (shape == gp_Translation && T.shape == gp_Translation) {
441 else if (shape == gp_Scale && T.shape == gp_Scale) {
442 loc.Add (T.loc.Multiplied(scale));
443 scale = scale * T.scale;
445 else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
447 shape = gp_Translation;
448 loc.Add (T.loc.Reversed());
450 else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
452 loc.Add (T.loc.Multiplied (matrix));
453 matrix.Multiply(T.matrix);
455 else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
456 shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
457 && T.shape == gp_Translation) {
459 Tloc.Multiply(matrix);
460 if (scale != 1.0) { Tloc.Multiply(scale); }
463 else if ((shape == gp_Scale || shape == gp_PntMirror)
464 && T.shape == gp_Translation) {
466 Tloc.Multiply (scale);
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;
477 else if (shape == gp_Translation &&
478 (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
483 else if ((shape == gp_PntMirror || shape == gp_Scale) &&
484 (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
485 shape = gp_CompoundTrsf;
487 Tloc.Multiply (scale);
489 scale = scale * T.scale;
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;
498 Tloc.Multiply(matrix);
501 Tloc.Multiply (matrix);
502 Tloc.Multiply (scale);
503 scale = scale * T.scale;
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;
512 Tloc.Multiply(scale);
514 scale = scale * T.scale;
518 shape = gp_CompoundTrsf;
520 Tloc.Multiply (matrix);
522 Tloc.Multiply (scale);
523 scale = scale * T.scale;
525 else { scale = T.scale; }
527 matrix.Multiply(T.matrix);
531 //=======================================================================
534 //=======================================================================
536 void gp_Trsf::Power (const Standard_Integer N)
538 if (shape == gp_Identity) { }
543 matrix.SetIdentity();
544 loc = gp_XYZ (0.0, 0.0, 0.0);
547 else if (N == -1) { Invert(); }
549 if (N < 0) { Invert(); }
550 if (shape == gp_Translation) {
551 Standard_Integer Npower = N;
552 if (Npower < 0) Npower = - Npower;
554 gp_XYZ Temploc = loc;
556 if (IsOdd(Npower)) loc.Add (Temploc);
557 if (Npower == 1) break;
558 Temploc.Add (Temploc);
562 else if (shape == gp_Scale) {
563 Standard_Integer Npower = N;
564 if (Npower < 0) Npower = - Npower;
566 gp_XYZ Temploc = loc;
567 Standard_Real Tempscale = scale;
570 loc.Add (Temploc.Multiplied(scale));
571 scale = scale * Tempscale;
573 if (Npower == 1) break;
574 Temploc.Add (Temploc.Multiplied(Tempscale));
575 Tempscale = Tempscale * Tempscale;
579 else if (shape == gp_Rotation) {
580 Standard_Integer Npower = N;
581 if (Npower < 0) Npower = - Npower;
583 gp_Mat Tempmatrix (matrix);
584 if (loc.X() == 0.0 && loc.Y() == 0.0 && loc.Z() == 0.0) {
586 if (IsOdd(Npower)) matrix.Multiply (Tempmatrix);
587 if (Npower == 1) break;
588 Tempmatrix.Multiply (Tempmatrix);
593 gp_XYZ Temploc = loc;
596 loc.Add (Temploc.Multiplied (matrix));
597 matrix.Multiply (Tempmatrix);
599 if (Npower == 1) break;
600 Temploc.Add (Temploc.Multiplied (Tempmatrix));
601 Tempmatrix.Multiply (Tempmatrix);
606 else if (shape == gp_PntMirror || shape == gp_Ax1Mirror ||
607 shape == gp_Ax2Mirror) {
611 matrix.SetIdentity ();
618 shape = gp_CompoundTrsf;
619 Standard_Integer Npower = N;
620 if (Npower < 0) Npower = - Npower;
622 gp_XYZ Temploc = loc;
623 Standard_Real Tempscale = scale;
624 gp_Mat Tempmatrix (matrix);
627 loc.Add ((Temploc.Multiplied (matrix)).Multiplied (scale));
628 scale = scale * Tempscale;
629 matrix.Multiply (Tempmatrix);
631 if (Npower == 1) break;
632 Tempscale = Tempscale * Tempscale;
633 Temploc.Add ( (Temploc.Multiplied (Tempmatrix)).Multiplied
636 Tempmatrix.Multiply (Tempmatrix);
644 //=======================================================================
645 //function : PreMultiply
647 //=======================================================================
649 void gp_Trsf::PreMultiply (const gp_Trsf& T)
651 if (T.shape == gp_Identity) { }
652 else if (shape == gp_Identity) {
658 else if (shape == gp_Rotation && T.shape == gp_Rotation) {
659 loc.Multiply (T.matrix);
661 matrix.PreMultiply(T.matrix);
663 else if (shape == gp_Translation && T.shape == gp_Translation) {
666 else if (shape == gp_Scale && T.shape == gp_Scale) {
667 loc.Multiply (T.scale);
669 scale = scale * T.scale;
671 else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
673 shape = gp_Translation;
677 else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
679 loc.Multiply (T.matrix);
681 matrix.PreMultiply(T.matrix);
683 else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
684 shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
685 && T.shape == gp_Translation) {
688 else if ((shape == gp_Scale || shape == gp_PntMirror)
689 && T.shape == gp_Translation) {
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;
697 if (T.scale == 1.0) loc.Multiply (T.matrix);
700 loc.Multiply (matrix);
701 loc.Multiply (scale);
705 else if ((T.shape == gp_Scale || T.shape == gp_PntMirror)
706 && shape == gp_Translation) {
707 loc.Multiply (T.scale);
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);
717 scale = scale * T.scale;
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);
725 scale = scale * T.scale;
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;
732 if (T.scale == 1.0) loc.Multiply (T.matrix);
734 loc.Multiply (matrix);
735 loc.Multiply (T.scale);
736 scale = T.scale * scale;
741 shape = gp_CompoundTrsf;
742 loc.Multiply (T.matrix);
743 if (T.scale != 1.0) {
744 loc.Multiply (T.scale); scale = scale * T.scale;
747 matrix.PreMultiply(T.matrix);
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 //=======================================================================
757 Standard_Boolean gp_Trsf::GetRotation (gp_XYZ& theAxis,
758 Standard_Real& theAngle) const
760 gp_Quaternion Q = GetRotation();
762 Q.GetVectorAndAngle (aVec, theAngle);
763 theAxis = aVec.XYZ();
764 return Standard_True;
767 //=======================================================================
768 //function : Orthogonalize
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()
778 //Matrix M is called orthogonal if and only if
779 // M*Transpose(M) == E
780 //where E is identity matrix.
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.
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.
790 // Vn_{i}*Vn_{j} = (i == j)? 1 : 0.
792 //The algorithm includes following steps:
794 //1. Normalize V1 vector:
800 //After multiplication two parts of this equation by V1n,
801 //we will have following equation:
802 // 0=V2*V1n-m <==> m=V2*V1n.
805 // V2n=V2-(V2*V1n)*V1n.
808 // V3n=V3-m1*V1n-m2*V2n.
810 //After multiplication two parts of this equation by V1n,
811 //we will have following equation:
812 // 0=V3*V1n-m1 <==> m1=V3*V1n.
814 //After multiplication two parts of main equation by V2n,
815 //we will have following equation:
816 // 0=V3*V2n-m2 <==> m2=V3*V2n.
819 // V3n=V3-(V3*V1n)*V1n-(V3*V2n)*V2n.
823 gp_XYZ aV1 = aTM.Column(1);
824 gp_XYZ aV2 = aTM.Column(2);
825 gp_XYZ aV3 = aTM.Column(3);
829 aV2 -= aV1*(aV2.Dot(aV1));
832 aV3 -= aV1*(aV3.Dot(aV1)) + aV2*(aV3.Dot(aV2));
835 aTM.SetCols(aV1, aV2, aV3);
843 aV2 -= aV1*(aV2.Dot(aV1));
846 aV3 -= aV1*(aV3.Dot(aV1)) + aV2*(aV3.Dot(aV2));
849 aTM.SetRows(aV1, aV2, aV3);
854 //=======================================================================
855 //function : DumpJson
857 //=======================================================================
858 void gp_Trsf::DumpJson (Standard_OStream& theOStream, const Standard_Integer theDepth) const
860 DUMP_CLASS_BEGIN (theOStream, gp_Trsf);
862 DUMP_FIELD_VALUES_DUMPED (theOStream, theDepth, &loc);
863 DUMP_FIELD_VALUES_DUMPED (theOStream, theDepth, &matrix);
865 DUMP_FIELD_VALUE_NUMERICAL (theOStream, shape);
866 DUMP_FIELD_VALUE_NUMERICAL (theOStream, scale);