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