src/gp/gp_Trsf.cxx

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