src/gp/gp_Trsf.cxx

   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
  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
  35 void 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
  56 void 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
  76 void 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
  93 void 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
 106 void 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
 124 void 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
 152 void 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
 170 void 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
 183 void 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
 213 void 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
 253 void 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
 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 ,
 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
 395 gp_Quaternion gp_Trsf::GetRotation () const
 396 {
 397   return gp_Quaternion (matrix);
 398 }
 399
 400 //=======================================================================
 401 //function : VectorialPart
 402 //purpose  :
 403 //=======================================================================
 404
 405 gp_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
 423 void 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
 457 void 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) {
 467     if (T.loc.X() != 0.0 || T.loc.Y() != 0.0 || T.loc.Z() != 0.0) {
 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
 570 void 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
 683 void 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
 791 Standard_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 }