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