src/gp/gp_Mat.lxx

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