simgear/scene/sky/clouds3d/mat33impl.hpp

   1 //------------------------------------------------------------------------------
   2 // File : mat33impl.hpp
   3 //------------------------------------------------------------------------------
   4 // GLVU : Copyright 1997 - 2002
   5 //        The University of North Carolina at Chapel Hill
   6 //------------------------------------------------------------------------------
   7 // Permission to use, copy, modify, distribute and sell this software and its
   8 // documentation for any purpose is hereby granted without fee, provided that
   9 // the above copyright notice appear in all copies and that both that copyright
  10 // notice and this permission notice appear in supporting documentation.
  11 // Binaries may be compiled with this software without any royalties or
  12 // restrictions.
  13 //
  14 // The University of North Carolina at Chapel Hill makes no representations
  15 // about the suitability of this software for any purpose. It is provided
  16 // "as is" without express or implied warranty.
  17
  18 //============================================================================
  19 // mat33impl.hpp : 3x3 matrix template.
  20 // This is the template implementation file. It is included by mat33.hpp.
  21 //============================================================================
  22
  23 //---------------------------------------------------------------------------
  24 // CONSTRUCTORS
  25 //---------------------------------------------------------------------------
  26 template<class Type>
  27 Mat33<Type>::Mat33()
  28 {
  29   Identity();
  30 }
  31
  32 template<class Type>
  33 Mat33<Type>::Mat33(const Type *N)
  34 {
  35   M[0]=N[0]; M[3]=N[3];  M[6]=N[6];
  36   M[1]=N[1]; M[4]=N[4];  M[7]=N[7];
  37   M[2]=N[2]; M[5]=N[5];  M[8]=N[8];
  38 }
  39
  40 template<class Type>
  41 Mat33<Type>::Mat33(Type M0, Type M3, Type M6,
  42                    Type M1, Type M4, Type M7,
  43                    Type M2, Type M5, Type M8)
  44
  45 {
  46   M[0]=M0; M[3]=M3; M[6]=M6;
  47   M[1]=M1; M[4]=M4; M[7]=M7;
  48   M[2]=M2; M[5]=M5; M[8]=M8;
  49 }
  50
  51 //---------------------------------------------------------------------------
  52 // MATRIX/MATRIX AND MATRIX/VECTOR OPERATORS
  53 //---------------------------------------------------------------------------
  54 template<class Type>
  55 Mat33<Type>& Mat33<Type>::operator = (const Mat33& A)      // ASSIGNMENT (=)
  56 {
  57   M[0]=A.M[0]; M[3]=A.M[3]; M[6]=A.M[6];
  58   M[1]=A.M[1]; M[4]=A.M[4]; M[7]=A.M[7];
  59   M[2]=A.M[2]; M[5]=A.M[5]; M[8]=A.M[8];
  60   return(*this);
  61 }
  62
  63 template<class Type>
  64 Mat33<Type>& Mat33<Type>::operator = (const Type* a) {
  65   for (int i=0;i<9;i++) {
  66     M[i] = a[i];
  67   }
  68   return *this;
  69 }
  70
  71 template<class Type>
  72 Mat33<Type> Mat33<Type>::operator * (const Mat33& A) const  // MULTIPLICATION (*)
  73 {
  74   Mat33<Type> NewM(
  75               M[0]*A.M[0] + M[3]*A.M[1] + M[6]*A.M[2],      // ROW 1
  76               M[0]*A.M[3] + M[3]*A.M[4] + M[6]*A.M[5],
  77               M[0]*A.M[6] + M[3]*A.M[7] + M[6]*A.M[8],
  78
  79               M[1]*A.M[0] + M[4]*A.M[1] + M[7]*A.M[2],      // ROW 2
  80               M[1]*A.M[3] + M[4]*A.M[4] + M[7]*A.M[5],
  81               M[1]*A.M[6] + M[4]*A.M[7] + M[7]*A.M[8],
  82
  83               M[2]*A.M[0] + M[5]*A.M[1] + M[8]*A.M[2],      // ROW 3
  84               M[2]*A.M[3] + M[5]*A.M[4] + M[8]*A.M[5],
  85               M[2]*A.M[6] + M[5]*A.M[7] + M[8]*A.M[8]);
  86   return(NewM);
  87 }
  88
  89 // MAT-VECTOR MULTIPLICATION (*)
  90 template<class Type>
  91 Vec3<Type> Mat33<Type>::operator * (const Vec3<Type>& V) const
  92 {
  93   Vec3<Type> NewV;
  94   NewV.x = M[0]*V.x + M[3]*V.y + M[6]*V.z;
  95   NewV.y = M[1]*V.x + M[4]*V.y + M[7]*V.z;
  96   NewV.z = M[2]*V.x + M[5]*V.y + M[8]*V.z;
  97   return(NewV);
  98 }
  99
 100 // MAT-VECTOR PRE-MULTIPLICATON (*) (non-member)
 101 // interpreted as V^t M
 102 template<class Type>
 103 Vec3<Type> operator *(const Vec3<Type>& V, const Mat33<Type>& A)
 104 {
 105   Vec3<Type> NewV;
 106   NewV.x = A[0]*V.x + A[1]*V.y + A[2]*V.z;
 107   NewV.y = A[3]*V.x + A[4]*V.y + A[5]*V.z;
 108   NewV.z = A[6]*V.x + A[7]*V.y + A[8]*V.z;
 109   return(NewV);
 110 }
 111
 112 // SCALAR POST-MULTIPLICATION
 113 template<class Type>
 114 Mat33<Type> Mat33<Type>::operator * (Type a) const
 115 {
 116   Mat33<Type> NewM;
 117   for (int i = 0; i < 9; i++)
 118     NewM[i] = M[i]*a;
 119   return(NewM);
 120 }
 121
 122 // SCALAR PRE-MULTIPLICATION (non-member)
 123 template <class Type>
 124 Mat33<Type> operator * (Type a, const Mat44<Type>& M)
 125 {
 126   Mat33<Type> NewM;
 127   for (int i = 0; i < 9; i++)
 128     NewM[i] = a*M[i];
 129   return(NewM);
 130 }
 131
 132 template <class Type>
 133 Mat33<Type> Mat33<Type>::operator / (Type a) const      // SCALAR DIVISION
 134 {
 135   Mat33<Type> NewM;
 136   Type ainv = Type(1.0)/a;
 137   for (int i = 0; i < 9; i++)
 138     NewM[i] = M[i]*ainv;
 139   return(NewM);
 140 }
 141
 142 template <class Type>
 143 Mat33<Type> Mat33<Type>::operator + (Mat33& N) const    // ADDITION (+)
 144 {
 145   Mat33<Type> NewM;
 146   for (int i = 0; i < 9; i++)
 147     NewM[i] = M[i]+N.M[i];
 148   return(NewM);
 149 }
 150 template <class Type>
 151 Mat33<Type>& Mat33<Type>::operator += (Mat33& N)  // ACCUMULATE ADD (+=)
 152 {
 153   for (int i = 0; i < 9; i++)
 154     M[i] += N.M[i];
 155   return(*this);
 156 }
 157
 158 template <class Type>
 159 Mat33<Type>& Mat33<Type>::operator -= (Mat33& N)  // ACCUMULATE SUB (-=)
 160 {
 161   for (int i = 0; i < 9; i++)
 162     M[i] -= N.M[i];
 163   return(*this);
 164 }
 165
 166 template <class Type>
 167 Mat33<Type>& Mat33<Type>::operator *= (Type a)   // ACCUMULATE MULTIPLY (*=)
 168 {
 169   for (int i = 0; i < 9; i++)
 170     M[i] *= a;
 171   return(*this);
 172 }
 173 template <class Type>
 174 Mat33<Type>& Mat33<Type>::operator /= (Type a)   // ACCUMULATE DIVIDE (/=)
 175 {
 176   Type ainv = Type(1.0)/a;
 177   for (int i = 0; i < 9; i++)
 178     M[i] *= ainv;
 179   return(*this);
 180 }
 181
 182 template<class Type>
 183 bool Mat33<Type>::Inverse(Mat33<Type> &inv, Type tolerance) const // MATRIX INVERSE
 184 {
 185   // Invert using cofactors.
 186
 187   inv[0] = M[4]*M[8] - M[7]*M[5];
 188   inv[3] = M[6]*M[5] - M[3]*M[8];
 189   inv[6] = M[3]*M[7] - M[6]*M[4];
 190   inv[1] = M[7]*M[2] - M[1]*M[8];
 191   inv[4] = M[0]*M[8] - M[6]*M[2];
 192   inv[7] = M[6]*M[1] - M[0]*M[7];
 193   inv[2] = M[1]*M[5] - M[4]*M[2];
 194   inv[5] = M[3]*M[2] - M[0]*M[5];
 195   inv[8] = M[0]*M[4] - M[3]*M[1];
 196
 197   Type det = M[0]*inv[0] + M[3]*inv[1] + M[6]*inv[2];
 198
 199   if (fabs(det) <= tolerance) // singular
 200     return false;
 201
 202   Type invDet = 1.0f / det;
 203   for (int i = 0; i < 9; i++) {
 204     inv[i] *= invDet;
 205   }
 206
 207   return true;
 208 }
 209
 210 template<class Type>
 211 Mat33<Type>::operator const Type*() const
 212 {
 213   return M;
 214 }
 215
 216 template<class Type>
 217 Mat33<Type>::operator Type*()
 218 {
 219   return M;
 220 }
 221
 222 template<class Type>
 223 void Mat33<Type>::RowMajor(Type m[9])
 224 {
 225   m[0] = M[0]; m[1] = M[3]; m[2] = M[6];
 226   m[3] = M[1]; m[4] = M[4]; m[5] = M[7];
 227   m[6] = M[2]; m[7] = M[5]; m[8] = M[8];
 228 }
 229
 230 template<class Type>
 231 Type& Mat33<Type>::operator()(int col, int row)
 232 {
 233   return M[3*col+row];
 234 }
 235
 236 template<class Type>
 237 const Type& Mat33<Type>::operator()(int col, int row) const
 238 {
 239   return M[3*col+row];
 240 }
 241
 242 template<class Type>
 243 void Mat33<Type>::Set(const Type* a)
 244 {
 245   for (int i=0;i<9;i++) {
 246     M[i] = a[i];
 247   }
 248 }
 249
 250 template<class Type>
 251 void Mat33<Type>::Set(Type M0, Type M3, Type M6,
 252                       Type M1, Type M4, Type M7,
 253                       Type M2, Type M5, Type M8)
 254 {
 255   M[0]=M0; M[3]=M3; M[6]=M6;
 256   M[1]=M1; M[4]=M4; M[7]=M7;
 257   M[2]=M2; M[5]=M5; M[8]=M8;
 258 }
 259
 260
 261 //---------------------------------------------------------------------------
 262 // Standard Matrix Operations
 263 //---------------------------------------------------------------------------
 264 template<class Type>
 265 void Mat33<Type>::Identity()
 266 {
 267   M[0]=M[4]=M[8]=1;
 268   M[1]=M[2]=M[3]=M[5]=M[6]=M[7]=0;
 269 }
 270 template<class Type>
 271 void Mat33<Type>::Zero()
 272 {
 273   M[0]=M[1]=M[2]=M[3]=M[4]=M[5]=M[6]=M[7]=M[8]=0;
 274 }
 275
 276 template<class Type>
 277 void Mat33<Type>::Transpose()
 278 {
 279   SWAP(M[1],M[3]);
 280   SWAP(M[2],M[6]);
 281   SWAP(M[5],M[7]);
 282 }
 283
 284 //---------------------------------------------------------------------------
 285 // Standard Matrix Affine Transformations
 286 //---------------------------------------------------------------------------
 287 template<class Type>
 288 void Mat33<Type>::Scale(Type Sx, Type Sy, Type Sz)
 289 {
 290   M[0]=Sx; M[3]=0;  M[6]=0;
 291   M[1]=0;  M[4]=Sy; M[7]=0;
 292   M[2]=0;  M[5]=0;  M[8]=Sz;
 293 }
 294
 295 template<class Type>
 296 void Mat33<Type>::Scale(const Vec3<Type>& S)
 297 {
 298   M[0]=S.x; M[3]=0;   M[6]=0;
 299   M[1]=0;   M[4]=S.y; M[7]=0;
 300   M[2]=0;   M[5]=0;   M[8]=S.z;
 301 }
 302
 303 template<class Type>
 304 void Mat33<Type>::invScale(Type Sx, Type Sy, Type Sz)
 305 {
 306   M[0]=1/Sx; M[3]=0;    M[6]=0;
 307   M[1]=0;    M[4]=1/Sy; M[7]=0;
 308   M[2]=0;    M[5]=0;    M[8]=1/Sz;
 309 }
 310
 311 template<class Type>
 312 void Mat33<Type>::invScale(const Vec3<Type>& S)
 313 {
 314   M[0]=1/S.x; M[3]=0;     M[6]=0;
 315   M[1]=0;     M[4]=1/S.y; M[7]=0;
 316   M[2]=0;     M[5]=0;     M[8]=1/S.z;
 317 }
 318
 319 template<class Type>
 320 void Mat33<Type>::Rotate(Type DegAng, const Vec3<Type>& Axis)
 321 {
 322   Type RadAng = DegAng*Mat33TORADS;
 323   Type ca=(Type)cos(RadAng),
 324         sa=(Type)sin(RadAng);
 325   if (Axis.x==1 && Axis.y==0 && Axis.z==0)  // ABOUT X-AXIS
 326   {
 327    M[0]=1; M[3]=0;  M[6]=0;
 328    M[1]=0; M[4]=ca; M[7]=-sa;
 329    M[2]=0; M[5]=sa; M[8]=ca;
 330
 331   }
 332   else if (Axis.x==0 && Axis.y==1 && Axis.z==0)  // ABOUT Y-AXIS
 333   {
 334    M[0]=ca;  M[3]=0; M[6]=sa;
 335    M[1]=0;   M[4]=1; M[7]=0;
 336    M[2]=-sa; M[5]=0; M[8]=ca;
 337   }
 338   else if (Axis.x==0 && Axis.y==0 && Axis.z==1)  // ABOUT Z-AXIS
 339   {
 340    M[0]=ca; M[3]=-sa; M[6]=0;
 341    M[1]=sa; M[4]=ca;  M[7]=0;
 342    M[2]=0;  M[5]=0;   M[8]=1;
 343   }
 344   else                                      // ARBITRARY AXIS
 345   {
 346    Type l = Axis.LengthSqr();
 347    Type x, y, z;
 348    x=Axis.x, y=Axis.y, z=Axis.z;
 349    if (l > Type(1.0001) || l < Type(0.9999) && l!=0)
 350    {
 351      // needs normalization
 352      l=Type(1.0)/sqrt(l);
 353      x*=l; y*=l; z*=l;
 354    }
 355    Type x2=x*x, y2=y*y, z2=z*z;
 356    M[0]=x2+ca*(1-x2); M[3]=(x*y)+ca*(-x*y)+sa*(-z); M[6]=(x*z)+ca*(-x*z)+sa*y;
 357    M[1]=(x*y)+ca*(-x*y)+sa*z; M[4]=y2+ca*(1-y2); M[7]=(y*z)+ca*(-y*z)+sa*(-x);
 358    M[2]=(x*z)+ca*(-x*z)+sa*(-y); M[5]=(y*z)+ca*(-y*z)+sa*x; M[8]=z2+ca*(1-z2);
 359   }
 360 }
 361
 362 template<class Type>
 363 void Mat33<Type>::invRotate(Type DegAng, const Vec3<Type>& Axis)
 364 {
 365   Rotate(DegAng,Axis);
 366   Transpose();
 367 }
 368
 369 template <class Type>
 370 inline void Mat33<Type>::Star(const Vec3<Type>& v)
 371 {
 372   M[0]=   0; M[3]=-v.z;  M[6]= v.y;
 373   M[1]= v.z; M[4]=   0;  M[7]=-v.x;
 374   M[2]=-v.y; M[5]= v.x; M[8]=   0;
 375 }
 376
 377 template <class Type>
 378 inline void Mat33<Type>::OuterProduct(const Vec3<Type>& u, const Vec3<Type>& v)
 379 {
 380   M[0]=u.x*v.x; M[3]=u.x*v.y;  M[6]=u.x*v.z;
 381   M[1]=u.y*v.x; M[4]=u.y*v.y;  M[7]=u.y*v.z;
 382   M[2]=u.z*v.x; M[5]=u.z*v.y; M[8]=u.z*v.z;
 383 }
 384
 385 template<class Type>
 386 inline Type Mat33<Type>::Trace() const
 387 {
 388   return M[0] + M[4] + M[8];
 389 }
 390
 391 //---------------------------------------------------------------------------
 392 // Handy matrix printing routine.
 393 //---------------------------------------------------------------------------
 394 template<class Type>
 395 void Mat33<Type>::Print() const
 396 {
 397   printf("\n%f %f %f\n",M[0],M[3],M[6]);
 398   printf("%f %f %f\n",M[1],M[4],M[7]);
 399   printf("%f %f %f\n",M[2],M[5],M[8]);
 400 }
 401
 402 //---------------------------------------------------------------------------
 403 // Copy contents of matrix into matrix array.
 404 //---------------------------------------------------------------------------
 405 template<class Type>
 406 void Mat33<Type>::CopyInto(Type *Mat) const
 407 {
 408   Mat[0]=M[0]; Mat[3]=M[3];  Mat[6]=M[6];
 409   Mat[1]=M[1]; Mat[4]=M[4];  Mat[7]=M[7];
 410   Mat[2]=M[2]; Mat[5]=M[5]; Mat[8]=M[8];
 411 }
 412