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