src/gp/gp_Mat.lxx

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