1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2012 OPEN CASCADE SAS
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.
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.
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.
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
24 #define No_Standard_OutOfRange
26 #include <gp_Trsf.ixx>
28 #include <Standard_ConstructionError.hxx>
30 //=======================================================================
31 //function : SetMirror
33 //=======================================================================
35 void gp_Trsf::SetMirror (const gp_Ax1& A1)
39 loc = A1.Location().XYZ();
40 matrix.SetDot(A1.Direction().XYZ());
42 matrix.SetDiagonal (matrix.Value (1,1) + 1,
43 matrix.Value (2,2) + 1,
44 matrix.Value (3,3) + 1);
46 loc.Multiply (matrix);
47 loc.Add (A1.Location().XYZ());
51 //=======================================================================
52 //function : SetMirror
54 //=======================================================================
56 void gp_Trsf::SetMirror (const gp_Ax2& A2)
60 loc = A2.Location().XYZ();
61 matrix.SetDot(A2.Direction().XYZ());
63 matrix.SetDiagonal (matrix.Value (1,1) - 1,
64 matrix.Value (2,2) - 1,
65 matrix.Value (3,3) - 1);
67 loc.Multiply (matrix);
68 loc.Add (A2.Location().XYZ());
71 //=======================================================================
72 //function : SetRotation
74 //=======================================================================
76 void gp_Trsf::SetRotation (const gp_Ax1& A1,
77 const Standard_Real Ang)
81 loc = A1.Location().XYZ();
82 matrix.SetRotation (A1.Direction().XYZ(), Ang);
84 loc.Multiply (matrix);
85 loc.Add (A1.Location().XYZ());
88 //=======================================================================
89 //function : SetRotation
91 //=======================================================================
93 void gp_Trsf::SetRotation (const gp_Quaternion& R)
97 loc.SetCoord (0., 0., 0.);
98 matrix = R.GetMatrix();
101 //=======================================================================
102 //function : SetScale
104 //=======================================================================
106 void gp_Trsf::SetScale (const gp_Pnt& P, const Standard_Real S)
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 ();
119 //=======================================================================
120 //function : SetTransformation
122 //=======================================================================
124 void gp_Trsf::SetTransformation (const gp_Ax3& FromA1,
127 shape = gp_CompoundTrsf;
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();
135 loc.Multiply (matrix);
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();
143 gp_Mat MA1 (xDir, yDir, zDir);
144 gp_XYZ MA1loc = FromA1.Location().XYZ();
146 // matrix * MA1 => FromA1 ToA2 :
147 MA1loc.Multiply (matrix);
149 matrix.Multiply (MA1);
152 void gp_Trsf::SetTransformation (const gp_Ax3& A3)
154 shape = gp_CompoundTrsf;
156 loc = A3.Location().XYZ();
157 matrix.SetCols (A3.XDirection().XYZ(),
158 A3.YDirection().XYZ(),
159 A3. Direction().XYZ());
161 loc.Multiply (matrix);
165 //=======================================================================
166 //function : SetTransformation
168 //=======================================================================
170 void gp_Trsf::SetTransformation (const gp_Quaternion& R, const gp_Vec& T)
172 shape = gp_CompoundTrsf;
175 matrix = R.GetMatrix();
178 //=======================================================================
179 //function : SetDisplacement
181 //=======================================================================
183 void gp_Trsf::SetDisplacement (const gp_Ax3& FromA1,
186 shape = gp_CompoundTrsf;
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);
199 gp_XYZ MA1loc = FromA1.Location().XYZ();
200 MA1loc.Multiply (MA1);
203 MA1loc.Multiply (matrix);
205 matrix.Multiply (MA1);
208 //=======================================================================
209 //function : SetTranslationPart
211 //=======================================================================
213 void gp_Trsf::SetTranslationPart (const gp_Vec& V) {
216 Standard_Real X = loc.X();
218 Standard_Real Y = loc.Y();
220 Standard_Real Z = loc.Z();
222 Standard_Boolean locnull =
223 (X <= gp::Resolution() &&
224 Y <= gp::Resolution() &&
225 Z <= gp::Resolution());
230 if (!locnull) shape = gp_Translation;
233 case gp_Translation :
234 if (locnull) shape = gp_Identity;
242 case gp_CompoundTrsf :
248 //=======================================================================
249 //function : SetScaleFactor
251 //=======================================================================
253 void gp_Trsf::SetScaleFactor (const Standard_Real S)
255 Standard_Real As = S;
256 if (As < 0) As = - As;
257 Standard_ConstructionError_Raise_if
258 (As <= gp::Resolution(),"gp_Trsf::SetScaleFactor");
261 if (As < 0) As = - As;
262 Standard_Boolean unit = As <= gp::Resolution();
264 if (As < 0) As = - As;
265 Standard_Boolean munit = As <= gp::Resolution();
269 case gp_Translation :
270 if (!unit) shape = gp_Scale;
271 if (munit) shape = gp_PntMirror;
274 if (!unit) shape = gp_CompoundTrsf;
279 if (!munit) shape = gp_Scale;
280 if (unit) shape = gp_Identity;
283 if (unit) shape = gp_Identity;
284 if (munit) shape = gp_PntMirror;
286 case gp_CompoundTrsf :
293 //=======================================================================
294 //function : SetValues
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 //=======================================================================
300 void 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 ,
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");
337 // check if the matrix is a rotation matrix
338 // the transposition should be the invert.
343 // don t trust the initial values !
346 anIdentity.SetIdentity() ;
347 TM.Subtract(anIdentity);
349 if (As < 0) As = - As;
350 Standard_ConstructionError_Raise_if
351 (As > TolDist,"gp_Trsf::SeValues, non uniform");
353 if (As < 0) As = - As;
354 Standard_ConstructionError_Raise_if
355 (As > TolDist,"gp_Trsf::SeValues, non uniform");
357 if (As < 0) As = - As;
358 Standard_ConstructionError_Raise_if
359 (As > TolDist,"gp_Trsf::SeValues, non uniform");
361 if (As < 0) As = - As;
362 Standard_ConstructionError_Raise_if
363 (As > TolDist,"gp_Trsf::SeValues, non uniform");
365 if (As < 0) As = - As;
366 Standard_ConstructionError_Raise_if
367 (As > TolDist,"gp_Trsf::SeValues, non uniform");
369 if (As < 0) As = - As;
370 Standard_ConstructionError_Raise_if
371 (As > TolDist,"gp_Trsf::SeValues, non uniform");
373 if (As < 0) As = - As;
374 Standard_ConstructionError_Raise_if
375 (As > TolDist,"gp_Trsf::SeValues, non uniform");
377 if (As < 0) As = - As;
378 Standard_ConstructionError_Raise_if
379 (As > TolDist,"gp_Trsf::SeValues, non uniform");
381 if (As < 0) As = - As;
382 Standard_ConstructionError_Raise_if
383 (As > TolDist,"gp_Trsf::SeValues, non uniform");
385 shape = gp_CompoundTrsf;
390 //=======================================================================
391 //function : GetRotation
393 //=======================================================================
395 gp_Quaternion gp_Trsf::GetRotation () const
397 return gp_Quaternion (matrix);
400 //=======================================================================
401 //function : VectorialPart
403 //=======================================================================
405 gp_Mat gp_Trsf::VectorialPart () const
407 if (scale == 1.0) return matrix;
409 if (shape == gp_Scale || shape == gp_PntMirror)
410 M.SetDiagonal(scale*M.Value(1,1),
418 //=======================================================================
421 //=======================================================================
423 void gp_Trsf::Invert()
426 // X' = scale * R * X + T => X = (R / scale) * ( X' - T)
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(),"");
438 loc.Multiply (-scale);
441 Standard_Real As = scale;
442 if (As < 0) As = - As;
443 Standard_ConstructionError_Raise_if
444 (As <= gp::Resolution(),"");
447 loc.Multiply (matrix);
448 loc.Multiply (-scale);
452 //=======================================================================
453 //function : Multiply
455 //=======================================================================
457 void gp_Trsf::Multiply(const gp_Trsf& T)
459 if (T.shape == gp_Identity) { }
460 else if (shape == gp_Identity) {
466 else if (shape == gp_Rotation && T.shape == gp_Rotation) {
467 if (T.loc.X() != 0.0 || T.loc.Y() != 0.0 || T.loc.Z() != 0.0) {
468 loc.Add (T.loc.Multiplied (matrix));
470 matrix.Multiply(T.matrix);
472 else if (shape == gp_Translation && T.shape == gp_Translation) {
475 else if (shape == gp_Scale && T.shape == gp_Scale) {
476 loc.Add (T.loc.Multiplied(scale));
477 scale = scale * T.scale;
479 else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
481 shape = gp_Translation;
482 loc.Add (T.loc.Reversed());
484 else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
486 loc.Add (T.loc.Multiplied (matrix));
487 matrix.Multiply(T.matrix);
489 else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
490 shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
491 && T.shape == gp_Translation) {
493 Tloc.Multiply(matrix);
494 if (scale != 1.0) { Tloc.Multiply(scale); }
497 else if ((shape == gp_Scale || shape == gp_PntMirror)
498 && T.shape == gp_Translation) {
500 Tloc.Multiply (scale);
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;
511 else if (shape == gp_Translation &&
512 (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
517 else if ((shape == gp_PntMirror || shape == gp_Scale) &&
518 (T.shape == gp_PntMirror || T.shape == gp_Scale)) {
519 shape = gp_CompoundTrsf;
521 Tloc.Multiply (scale);
523 scale = scale * T.scale;
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;
532 Tloc.Multiply(matrix);
535 Tloc.Multiply (matrix);
536 Tloc.Multiply (scale);
537 scale = scale * T.scale;
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;
546 Tloc.Multiply(scale);
548 scale = scale * T.scale;
552 shape = gp_CompoundTrsf;
554 Tloc.Multiply (matrix);
556 Tloc.Multiply (scale);
557 scale = scale * T.scale;
559 else { scale = T.scale; }
561 matrix.Multiply(T.matrix);
565 //=======================================================================
568 //=======================================================================
570 void gp_Trsf::Power (const Standard_Integer N)
572 if (shape == gp_Identity) { }
577 matrix.SetIdentity();
578 loc = gp_XYZ (0.0, 0.0, 0.0);
581 else if (N == -1) { Invert(); }
583 if (N < 0) { Invert(); }
584 if (shape == gp_Translation) {
585 Standard_Integer Npower = N;
586 if (Npower < 0) Npower = - Npower;
588 gp_XYZ Temploc = loc;
590 if (IsOdd(Npower)) loc.Add (Temploc);
591 if (Npower == 1) break;
592 Temploc.Add (Temploc);
596 else if (shape == gp_Scale) {
597 Standard_Integer Npower = N;
598 if (Npower < 0) Npower = - Npower;
600 gp_XYZ Temploc = loc;
601 Standard_Real Tempscale = scale;
604 loc.Add (Temploc.Multiplied(scale));
605 scale = scale * Tempscale;
607 if (Npower == 1) break;
608 Temploc.Add (Temploc.Multiplied(Tempscale));
609 Tempscale = Tempscale * Tempscale;
613 else if (shape == gp_Rotation) {
614 Standard_Integer Npower = N;
615 if (Npower < 0) Npower = - Npower;
617 gp_Mat Tempmatrix (matrix);
618 if (loc.X() == 0.0 && loc.Y() == 0.0 && loc.Z() == 0.0) {
620 if (IsOdd(Npower)) matrix.Multiply (Tempmatrix);
621 if (Npower == 1) break;
622 Tempmatrix.Multiply (Tempmatrix);
627 gp_XYZ Temploc = loc;
630 loc.Add (Temploc.Multiplied (matrix));
631 matrix.Multiply (Tempmatrix);
633 if (Npower == 1) break;
634 Temploc.Add (Temploc.Multiplied (Tempmatrix));
635 Tempmatrix.Multiply (Tempmatrix);
640 else if (shape == gp_PntMirror || shape == gp_Ax1Mirror ||
641 shape == gp_Ax2Mirror) {
645 matrix.SetIdentity ();
652 shape = gp_CompoundTrsf;
653 Standard_Integer Npower = N;
654 if (Npower < 0) Npower = - Npower;
656 gp_XYZ Temploc = loc;
657 Standard_Real Tempscale = scale;
658 gp_Mat Tempmatrix (matrix);
661 loc.Add ((Temploc.Multiplied (matrix)).Multiplied (scale));
662 scale = scale * Tempscale;
663 matrix.Multiply (Tempmatrix);
665 if (Npower == 1) break;
666 Tempscale = Tempscale * Tempscale;
667 Temploc.Add ( (Temploc.Multiplied (Tempmatrix)).Multiplied
670 Tempmatrix.Multiply (Tempmatrix);
678 //=======================================================================
679 //function : PreMultiply
681 //=======================================================================
683 void gp_Trsf::PreMultiply (const gp_Trsf& T)
685 if (T.shape == gp_Identity) { }
686 else if (shape == gp_Identity) {
692 else if (shape == gp_Rotation && T.shape == gp_Rotation) {
693 loc.Multiply (T.matrix);
695 matrix.PreMultiply(T.matrix);
697 else if (shape == gp_Translation && T.shape == gp_Translation) {
700 else if (shape == gp_Scale && T.shape == gp_Scale) {
701 loc.Multiply (T.scale);
703 scale = scale * T.scale;
705 else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
707 shape = gp_Translation;
711 else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
713 loc.Multiply (T.matrix);
715 matrix.PreMultiply(T.matrix);
717 else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
718 shape == gp_Ax1Mirror || shape == gp_Ax2Mirror)
719 && T.shape == gp_Translation) {
722 else if ((shape == gp_Scale || shape == gp_PntMirror)
723 && T.shape == gp_Translation) {
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;
731 if (T.scale == 1.0) loc.Multiply (T.matrix);
734 loc.Multiply (matrix);
735 loc.Multiply (scale);
739 else if ((T.shape == gp_Scale || T.shape == gp_PntMirror)
740 && shape == gp_Translation) {
741 loc.Multiply (T.scale);
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);
751 scale = scale * T.scale;
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);
759 scale = scale * T.scale;
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;
766 if (T.scale == 1.0) loc.Multiply (T.matrix);
768 loc.Multiply (matrix);
769 loc.Multiply (T.scale);
770 scale = T.scale * scale;
775 shape = gp_CompoundTrsf;
776 loc.Multiply (T.matrix);
777 if (T.scale != 1.0) {
778 loc.Multiply (T.scale); scale = scale * T.scale;
781 matrix.PreMultiply(T.matrix);
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 //=======================================================================
791 Standard_Boolean gp_Trsf::GetRotation (gp_XYZ& theAxis,
792 Standard_Real& theAngle) const
794 gp_Quaternion Q = GetRotation();
796 Q.GetVectorAndAngle (aVec, theAngle);
797 theAxis = aVec.XYZ();
798 return Standard_True;