src/FDM/YASim/Math.hpp

   1 #ifndef _MATH_HPP
   2 #define _MATH_HPP
   3
   4 #include <math.h>
   5
   6 namespace yasim {
   7
   8 class Math
   9 {
  10 public:
  11     // Dumb utilities
  12     static inline float clamp(float val, float min, float max) {
  13         if(val < min) return min;
  14         if(val > max) return max;
  15         return val;
  16     }
  17
  18     // Simple wrappers around library routines
  19     static inline float abs(float f)  { return (float)::fabs(f); }
  20     static inline float sqrt(float f) { return (float)::sqrt(f); }
  21     static inline float ceil(float f) { return (float)::ceil(f); }
  22     static inline float sin(float f)  { return (float)::sin(f);  }
  23     static inline float cos(float f)  { return (float)::cos(f);  }
  24     static inline float tan(float f)  { return (float)::tan(f);  }
  25     static inline float atan(float f) { return (float)::atan(f); }
  26     static inline float atan2(float y, float x) { return (float)::atan2(y,x); }
  27     static inline float asin(float f) { return (float)::asin(f); }
  28     static inline float acos(float f) { return (float)::acos(f); }
  29     static inline float exp(float f)  { return (float)::exp(f);  }
  30     static inline float sqr(float f)  { return f*f; }
  31
  32     // Takes two args and runs afoul of the Koenig rules.
  33     static inline float pow(double base, double exp) { return (float)::pow(base, exp); }
  34
  35     // double variants of the above
  36     static inline double abs(double f)  { return ::fabs(f); }
  37     static inline double sqrt(double f) { return ::sqrt(f); }
  38     static inline double ceil(double f) { return ::ceil(f); }
  39     static inline double sin(double f)  { return ::sin(f); }
  40     static inline double cos(double f)  { return ::cos(f); }
  41     static inline double tan(double f)  { return ::tan(f); }
  42     static inline double atan2(double y, double x) { return ::atan2(y,x); }
  43     static inline double floor(double x) { return ::floor(x); }
  44
  45     // Some 3D vector stuff.  In all cases, it is permissible for the
  46     // "out" vector to be the same as one of the inputs.
  47     static inline void  set3(float* v, float* out) {
  48         out[0] = v[0];
  49         out[1] = v[1];
  50         out[2] = v[2];
  51     }
  52
  53     static inline float dot3(float* a, float* b) {
  54         return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
  55     }
  56
  57     static inline void  cross3(float* a, float* b, float* out) {
  58         float ax=a[0], ay=a[1], az=a[2];
  59         float bx=b[0], by=b[1], bz=b[2];
  60         out[0] = ay*bz - by*az;
  61         out[1] = az*bx - bz*ax;
  62         out[2] = ax*by - bx*ay;
  63     }
  64
  65     static inline void  mul3(float scalar, float* v, float* out)
  66     {
  67         out[0] = scalar * v[0];
  68         out[1] = scalar * v[1];
  69         out[2] = scalar * v[2];
  70     }
  71
  72     static inline void  add3(float* a, float* b, float* out){
  73         out[0] = a[0] + b[0];
  74         out[1] = a[1] + b[1];
  75         out[2] = a[2] + b[2];
  76     }
  77
  78     static inline void  sub3(float* a, float* b, float* out) {
  79         out[0] = a[0] - b[0];
  80         out[1] = a[1] - b[1];
  81         out[2] = a[2] - b[2];
  82     }
  83
  84     static inline float mag3(float* v) {
  85         return sqrt(dot3(v, v));
  86     }
  87
  88     static inline void  unit3(float* v, float* out) {
  89         float imag = 1/mag3(v);
  90         mul3(imag, v, out);
  91     }
  92
  93     // Matrix array convention: 0 1 2
  94     //                          3 4 5
  95     //                          6 7 8
  96
  97     // Multiply two matrices
  98     static void mmul33(float* a, float* b, float* out) {
  99         float tmp[9];
 100         tmp[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6];
 101         tmp[3] = a[3]*b[0] + a[4]*b[3] + a[5]*b[6];
 102         tmp[6] = a[6]*b[0] + a[7]*b[3] + a[8]*b[6];
 103
 104         tmp[1] = a[0]*b[1] + a[1]*b[4] + a[2]*b[7];
 105         tmp[4] = a[3]*b[1] + a[4]*b[4] + a[5]*b[7];
 106         tmp[7] = a[6]*b[1] + a[7]*b[4] + a[8]*b[7];
 107
 108         tmp[2] = a[0]*b[2] + a[1]*b[5] + a[2]*b[8];
 109         tmp[5] = a[3]*b[2] + a[4]*b[5] + a[5]*b[8];
 110         tmp[8] = a[6]*b[2] + a[7]*b[5] + a[8]*b[8];
 111
 112         for(int i=0; i<9; ++i)
 113             out[i] = tmp[i];
 114     }
 115
 116     // Multiply by vector
 117     static inline void vmul33(float* m, float* v, float* out) {
 118         float x = v[0], y = v[1], z = v[2];
 119         out[0] = x*m[0] + y*m[1] + z*m[2];
 120         out[1] = x*m[3] + y*m[4] + z*m[5];
 121         out[2] = x*m[6] + y*m[7] + z*m[8];
 122     }
 123
 124     // Multiply the vector by the matrix transpose.  Or pre-multiply the
 125     // matrix by v as a row vector.  Same thing.
 126     static inline void tmul33(float* m, float* v, float* out) {
 127         float x = v[0], y = v[1], z = v[2];
 128         out[0] = x*m[0] + y*m[3] + z*m[6];
 129         out[1] = x*m[1] + y*m[4] + z*m[7];
 130         out[2] = x*m[2] + y*m[5] + z*m[8];
 131     }
 132
 133     // Invert matrix
 134     static void invert33(float* m, float* out) {
 135         // Compute the inverse as the adjoint matrix times 1/(det M).
 136         // A, B ... I are the cofactors of a b c
 137         //                                 d e f
 138         //                                 g h i
 139         float a=m[0], b=m[1], c=m[2];
 140         float d=m[3], e=m[4], f=m[5];
 141         float g=m[6], h=m[7], i=m[8];
 142
 143         float A =  (e*i - h*f);
 144         float B = -(d*i - g*f);
 145         float C =  (d*h - g*e);
 146         float D = -(b*i - h*c);
 147         float E =  (a*i - g*c);
 148         float F = -(a*h - g*b);
 149         float G =  (b*f - e*c);
 150         float H = -(a*f - d*c);
 151         float I =  (a*e - d*b);
 152
 153         float id = 1/(a*A + b*B + c*C);
 154
 155         out[0] = id*A; out[1] = id*D; out[2] = id*G;
 156         out[3] = id*B; out[4] = id*E; out[5] = id*H;
 157         out[6] = id*C; out[7] = id*F; out[8] = id*I;
 158     }
 159
 160     // Transpose matrix (for an orthonormal orientation matrix, this
 161     // is the same as the inverse).
 162     static inline void trans33(float* m, float* out) {
 163         // 0 1 2   Elements 0, 4, and 8 are the same
 164         // 3 4 5   Swap elements 1/3, 2/6, and 5/7
 165         // 6 7 8
 166         out[0] = m[0];
 167         out[4] = m[4];
 168         out[8] = m[8];
 169
 170         float tmp = m[1];
 171         out[1] = m[3];
 172         out[3] = tmp;
 173
 174         tmp = m[2];
 175         out[2] = m[6];
 176         out[6] = tmp;
 177
 178         tmp = m[5];
 179         out[5] = m[7];
 180         out[7] = tmp;
 181     }
 182
 183     // Generates an orthonormal basis:
 184     //   xOut becomes the unit vector in the direction of x
 185     //   yOut is perpendicular to xOut in the x/y plane
 186     //   zOut becomes the unit vector: (xOut cross yOut)
 187     static void ortho33(float* x, float* y,
 188                         float* xOut, float* yOut, float* zOut) {
 189         float x0[3], y0[3];
 190         set3(x, x0);
 191         set3(y, y0);
 192
 193         unit3(x0, xOut);
 194         cross3(xOut, y0, zOut);
 195         unit3(zOut, zOut);
 196         cross3(zOut, xOut, yOut);
 197     }
 198 };
 199
 200 }; // namespace yasim
 201 #endif // _MATH_HPP