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