src/gp/gp_Mat.lxx

   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 #include <gp.hxx>
  16 #include <Standard_OutOfRange.hxx>
  17 #include <Standard_ConstructionError.hxx>
  18
  19 #define Mat00 matrix[0][0]
  20 #define Mat01 matrix[0][1]
  21 #define Mat02 matrix[0][2]
  22 #define Mat10 matrix[1][0]
  23 #define Mat11 matrix[1][1]
  24 #define Mat12 matrix[1][2]
  25 #define Mat20 matrix[2][0]
  26 #define Mat21 matrix[2][1]
  27 #define Mat22 matrix[2][2]
  28
  29 #define Nat00 NewMat.matrix[0][0]
  30 #define Nat01 NewMat.matrix[0][1]
  31 #define Nat02 NewMat.matrix[0][2]
  32 #define Nat10 NewMat.matrix[1][0]
  33 #define Nat11 NewMat.matrix[1][1]
  34 #define Nat12 NewMat.matrix[1][2]
  35 #define Nat20 NewMat.matrix[2][0]
  36 #define Nat21 NewMat.matrix[2][1]
  37 #define Nat22 NewMat.matrix[2][2]
  38
  39 #define Oat00 Other.matrix[0][0]
  40 #define Oat01 Other.matrix[0][1]
  41 #define Oat02 Other.matrix[0][2]
  42 #define Oat10 Other.matrix[1][0]
  43 #define Oat11 Other.matrix[1][1]
  44 #define Oat12 Other.matrix[1][2]
  45 #define Oat20 Other.matrix[2][0]
  46 #define Oat21 Other.matrix[2][1]
  47 #define Oat22 Other.matrix[2][2]
  48
  49 inline gp_Mat::gp_Mat () {
  50   Mat00 =
  51     Mat01 =
  52       Mat02 =
  53         Mat10 =
  54           Mat11 =
  55             Mat12 =
  56               Mat20 =
  57                 Mat21 =
  58                   Mat22 = 0.0;
  59 }
  60
  61 inline gp_Mat::gp_Mat (const Standard_Real a11,
  62                        const Standard_Real a12,
  63                        const Standard_Real a13,
  64                        const Standard_Real a21,
  65                        const Standard_Real a22,
  66                        const Standard_Real a23,
  67                        const Standard_Real a31,
  68                        const Standard_Real a32,
  69                        const Standard_Real a33) {
  70
  71   Mat00 = a11;
  72   Mat01 = a12;
  73   Mat02 = a13;
  74   Mat10 = a21;
  75   Mat11 = a22;
  76   Mat12 = a23;
  77   Mat20 = a31;
  78   Mat21 = a32;
  79   Mat22 = a33;
  80 }
  81
  82 inline void gp_Mat::SetDiagonal (const Standard_Real X1,
  83                                  const Standard_Real X2,
  84                                  const Standard_Real X3)
  85 {
  86   Mat00 = X1;   Mat11 = X2;   Mat22 = X3;
  87 }
  88
  89 inline void gp_Mat::SetIdentity ()
  90 {
  91   Mat00 = Mat11 = Mat22 = 1.0;
  92   Mat01 = Mat02 = Mat10  = Mat12 = Mat20 = Mat21 = 0.0;
  93  }
  94
  95 inline void gp_Mat::SetScale (const Standard_Real S)
  96 {
  97   Mat00 = Mat11 =  Mat22 = S;
  98   Mat01 = Mat02 = Mat10 = Mat12 = Mat20 = Mat21 = 0.0;
  99 }
 100
 101 inline void gp_Mat::SetValue (const Standard_Integer Row,
 102                               const Standard_Integer Col,
 103                               const Standard_Real Value)
 104 {
 105   Standard_OutOfRange_Raise_if
 106     (Row < 1 || Row > 3 || Col < 1 || Col > 3, " ");
 107   matrix[Row-1][Col-1] = Value;
 108 }
 109
 110 inline Standard_Real gp_Mat::Determinant () const
 111 {
 112   return
 113     Mat00 * (Mat11 * Mat22 - Mat21 * Mat12) -
 114       Mat01 * (Mat10 * Mat22 - Mat20 * Mat12) +
 115         Mat02 * (Mat10 * Mat21 - Mat20 * Mat11);
 116 }
 117
 118 inline const Standard_Real& gp_Mat::Value (const Standard_Integer Row,
 119                                            const Standard_Integer Col) const
 120 {
 121   Standard_OutOfRange_Raise_if
 122     (Row < 1 || Row > 3 || Col < 1 || Col > 3, " ");
 123   return matrix[Row-1][Col-1];
 124 }
 125
 126 inline Standard_Real& gp_Mat::ChangeValue (const Standard_Integer Row,
 127                                            const Standard_Integer Col)
 128 {
 129   Standard_OutOfRange_Raise_if
 130     (Row < 1 || Row > 3 || Col < 1 || Col > 3, " ");
 131   return matrix[Row-1][Col-1];
 132 }
 133
 134 inline Standard_Boolean gp_Mat::IsSingular () const
 135 {
 136   // Pour etre sur que Gauss va fonctionner, il faut faire Gauss ...
 137   Standard_Real val = Determinant();
 138   if (val < 0) val = - val;
 139   return val <= gp::Resolution();
 140 }
 141
 142 inline void gp_Mat::Add (const gp_Mat& Other)
 143 {
 144   Mat00 = Mat00 + Oat00;
 145   Mat01 = Mat01 + Oat01;
 146   Mat02 = Mat02 + Oat02;
 147   Mat10 = Mat10 + Oat10;
 148   Mat11 = Mat11 + Oat11;
 149   Mat12 = Mat12 + Oat12;
 150   Mat20 = Mat20 + Oat20;
 151   Mat21 = Mat21 + Oat21;
 152   Mat22 = Mat22 + Oat22;
 153 }
 154
 155 inline gp_Mat gp_Mat::Added (const gp_Mat& Other) const
 156 {
 157   gp_Mat NewMat;
 158   Nat00 = Mat00 + Oat00;
 159   Nat01 = Mat01 + Oat01;
 160   Nat02 = Mat02 + Oat02;
 161   Nat10 = Mat10 + Oat10;
 162   Nat11 = Mat11 + Oat11;
 163   Nat12 = Mat12 + Oat12;
 164   Nat20 = Mat20 + Oat20;
 165   Nat21 = Mat21 + Oat21;
 166   Nat22 = Mat22 + Oat22;
 167   return NewMat;
 168 }
 169
 170 inline void gp_Mat::Divide (const Standard_Real Scalar)
 171 {
 172   Standard_Real val = Scalar;
 173   if (val < 0) val = - val;
 174   Standard_ConstructionError_Raise_if
 175       (val <= gp::Resolution(),"gp_Mat : Divide by 0");
 176   Standard_Real UnSurScalar = 1.0 / Scalar;
 177   Mat00 *= UnSurScalar;
 178   Mat01 *= UnSurScalar;
 179   Mat02 *= UnSurScalar;
 180   Mat10 *= UnSurScalar;
 181   Mat11 *= UnSurScalar;
 182   Mat12 *= UnSurScalar;
 183   Mat20 *= UnSurScalar;
 184   Mat21 *= UnSurScalar;
 185   Mat22 *= UnSurScalar;
 186 }
 187
 188 inline gp_Mat gp_Mat::Divided (const Standard_Real Scalar) const
 189 {
 190   Standard_Real val = Scalar;
 191   if (val < 0) val = - val;
 192   Standard_ConstructionError_Raise_if
 193       (val <= gp::Resolution(),"gp_Mat : Divide by 0");
 194   gp_Mat NewMat;
 195   Standard_Real UnSurScalar = 1.0 / Scalar;
 196   Nat00 = Mat00 * UnSurScalar;
 197   Nat01 = Mat01 * UnSurScalar;
 198   Nat02 = Mat02 * UnSurScalar;
 199   Nat10 = Mat10 * UnSurScalar;
 200   Nat11 = Mat11 * UnSurScalar;
 201   Nat12 = Mat12 * UnSurScalar;
 202   Nat20 = Mat20 * UnSurScalar;
 203   Nat21 = Mat21 * UnSurScalar;
 204   Nat22 = Mat22 * UnSurScalar;
 205   return NewMat;
 206 }
 207
 208 inline gp_Mat gp_Mat::Multiplied (const gp_Mat& Other) const
 209 {
 210   gp_Mat NewMat = *this;
 211   NewMat.Multiply(Other);
 212   return NewMat;
 213 }
 214
 215 inline void gp_Mat::Multiply (const gp_Mat& Other)
 216 {
 217   Standard_Real T00,T01,T02,T10,T11,T12,T20,T21,T22;
 218   T00 = Mat00 * Oat00 + Mat01 * Oat10 + Mat02 * Oat20;
 219   T01 = Mat00 * Oat01 + Mat01 * Oat11 + Mat02 * Oat21;
 220   T02 = Mat00 * Oat02 + Mat01 * Oat12 + Mat02 * Oat22;
 221   T10 = Mat10 * Oat00 + Mat11 * Oat10 + Mat12 * Oat20;
 222   T11 = Mat10 * Oat01 + Mat11 * Oat11 + Mat12 * Oat21;
 223   T12 = Mat10 * Oat02 + Mat11 * Oat12 + Mat12 * Oat22;
 224   T20 = Mat20 * Oat00 + Mat21 * Oat10 + Mat22 * Oat20;
 225   T21 = Mat20 * Oat01 + Mat21 * Oat11 + Mat22 * Oat21;
 226   T22 = Mat20 * Oat02 + Mat21 * Oat12 + Mat22 * Oat22;
 227   Mat00 = T00;
 228   Mat01 = T01;
 229   Mat02 = T02;
 230   Mat10 = T10;
 231   Mat11 = T11;
 232   Mat12 = T12;
 233   Mat20 = T20;
 234   Mat21 = T21;
 235   Mat22 = T22;
 236 }
 237
 238 inline void gp_Mat::PreMultiply (const gp_Mat& Other)
 239 {
 240   Standard_Real T00,T01,T02,T10,T11,T12,T20,T21,T22;
 241   T00 = Oat00 * Mat00 + Oat01 * Mat10 + Oat02 * Mat20;
 242   T01 = Oat00 * Mat01 + Oat01 * Mat11 + Oat02 * Mat21;
 243   T02 = Oat00 * Mat02 + Oat01 * Mat12 + Oat02 * Mat22;
 244   T10 = Oat10 * Mat00 + Oat11 * Mat10 + Oat12 * Mat20;
 245   T11 = Oat10 * Mat01 + Oat11 * Mat11 + Oat12 * Mat21;
 246   T12 = Oat10 * Mat02 + Oat11 * Mat12 + Oat12 * Mat22;
 247   T20 = Oat20 * Mat00 + Oat21 * Mat10 + Oat22 * Mat20;
 248   T21 = Oat20 * Mat01 + Oat21 * Mat11 + Oat22 * Mat21;
 249   T22 = Oat20 * Mat02 + Oat21 * Mat12 + Oat22 * Mat22;
 250   Mat00 = T00;
 251   Mat01 = T01;
 252   Mat02 = T02;
 253   Mat10 = T10;
 254   Mat11 = T11;
 255   Mat12 = T12;
 256   Mat20 = T20;
 257   Mat21 = T21;
 258   Mat22 = T22;
 259 }
 260
 261 inline gp_Mat gp_Mat::Multiplied (const Standard_Real Scalar) const
 262 {
 263   gp_Mat NewMat;
 264   Nat00 = Scalar * Mat00;
 265   Nat01 = Scalar * Mat01;
 266   Nat02 = Scalar * Mat02;
 267   Nat10 = Scalar * Mat10;
 268   Nat11 = Scalar * Mat11;
 269   Nat12 = Scalar * Mat12;
 270   Nat20 = Scalar * Mat20;
 271   Nat21 = Scalar * Mat21;
 272   Nat22 = Scalar * Mat22;
 273   return NewMat;
 274 }
 275
 276 inline void gp_Mat::Multiply (const Standard_Real Scalar)
 277 {
 278   Mat00 *= Scalar;
 279   Mat01 *= Scalar;
 280   Mat02 *= Scalar;
 281   Mat10 *= Scalar;
 282   Mat11 *= Scalar;
 283   Mat12 *= Scalar;
 284   Mat20 *= Scalar;
 285   Mat21 *= Scalar;
 286   Mat22 *= Scalar;
 287 }
 288
 289 inline gp_Mat gp_Mat::Powered (const Standard_Integer N) const
 290 {
 291    gp_Mat MatN = *this;
 292    MatN.Power (N);
 293    return MatN;
 294 }
 295
 296 inline void gp_Mat::Subtract (const gp_Mat& Other)
 297 {
 298   Mat00 -= Oat00;
 299   Mat01 -= Oat01;
 300   Mat02 -= Oat02;
 301   Mat10 -= Oat10;
 302   Mat11 -= Oat11;
 303   Mat12 -= Oat12;
 304   Mat20 -= Oat20;
 305   Mat21 -= Oat21;
 306   Mat22 -= Oat22;
 307 }
 308
 309 inline gp_Mat gp_Mat::Subtracted (const gp_Mat& Other) const
 310 {
 311   gp_Mat NewMat;
 312   Nat00 = Mat00 - Oat00;
 313   Nat01 = Mat01 - Oat01;
 314   Nat02 = Mat02 - Oat02;
 315   Nat10 = Mat10 - Oat10;
 316   Nat11 = Mat11 - Oat11;
 317   Nat12 = Mat12 - Oat12;
 318   Nat20 = Mat20 - Oat20;
 319   Nat21 = Mat21 - Oat21;
 320   Nat22 = Mat22 - Oat22;
 321   return NewMat;
 322 }
 323
 324 inline void gp_Mat::Transpose ()
 325 {
 326   Standard_Real Temp;
 327   Temp   = Mat01;
 328   Mat01  = Mat10;
 329   Mat10  = Temp;
 330   Temp   = Mat02;
 331   Mat02  = Mat20;
 332   Mat20  = Temp;
 333   Temp   = Mat12;
 334   Mat12  = Mat21;
 335   Mat21  = Temp;
 336 }
 337
 338 inline gp_Mat gp_Mat::Transposed () const
 339 {
 340   gp_Mat NewMat = *this;
 341   NewMat.Transpose();
 342   return NewMat;
 343 }
 344
 345 inline gp_Mat operator* (const Standard_Real Scalar, const gp_Mat& Mat3D)
 346 { return Mat3D.Multiplied (Scalar); }
 347