src/FDM/YASim/Math.cpp

   1 #include <math.h>
   2
   3 #include "Math.hpp"
   4 namespace yasim {
   5
   6 float Math::abs(float f)
   7 {
   8     return ::fabs(f);
   9 }
  10
  11 float Math::sqrt(float f)
  12 {
  13     return ::sqrt(f);
  14 }
  15
  16 float Math::pow(float base, float exp)
  17 {
  18     return ::pow(base, exp);
  19 }
  20
  21 float Math::ceil(float f)
  22 {
  23     return ::ceil(f);
  24 }
  25
  26 float Math::cos(float f)
  27 {
  28     return ::cos(f);
  29 }
  30
  31 float Math::sin(float f)
  32 {
  33     return ::sin(f);
  34 }
  35
  36 float Math::tan(float f)
  37 {
  38     return ::tan(f);
  39 }
  40
  41 float Math::atan2(float y, float x)
  42 {
  43     return ::atan2(y, x);
  44 }
  45
  46 double Math::abs(double f)
  47 {
  48     return ::fabs(f);
  49 }
  50
  51 double Math::sqrt(double f)
  52 {
  53     return ::sqrt(f);
  54 }
  55
  56 double Math::pow(double base, double exp)
  57 {
  58     return ::pow(base, exp);
  59 }
  60
  61 double Math::ceil(double f)
  62 {
  63     return ::ceil(f);
  64 }
  65
  66 double Math::cos(double f)
  67 {
  68     return ::cos(f);
  69 }
  70
  71 double Math::sin(double f)
  72 {
  73     return ::sin(f);
  74 }
  75
  76 double Math::tan(double f)
  77 {
  78     return ::tan(f);
  79 }
  80
  81 double Math::atan2(double y, double x)
  82 {
  83     return ::atan2(y, x);
  84 }
  85
  86 void Math::set3(float* v, float* out)
  87 {
  88     out[0] = v[0];
  89     out[1] = v[1];
  90     out[2] = v[2];
  91 }
  92
  93 float Math::dot3(float* a, float* b)
  94 {
  95     return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
  96 }
  97
  98 void Math::mul3(float scalar, float* v, float* out)
  99 {
 100     out[0] = scalar * v[0];
 101     out[1] = scalar * v[1];
 102     out[2] = scalar * v[2];
 103 }
 104
 105 void Math::add3(float* a, float* b, float* out)
 106 {
 107     out[0] = a[0] + b[0];
 108     out[1] = a[1] + b[1];
 109     out[2] = a[2] + b[2];
 110 }
 111
 112 void Math::sub3(float* a, float* b, float* out)
 113 {
 114     out[0] = a[0] - b[0];
 115     out[1] = a[1] - b[1];
 116     out[2] = a[2] - b[2];
 117 }
 118
 119 float Math::mag3(float* v)
 120 {
 121     return sqrt(dot3(v, v));
 122 }
 123
 124
 125 void Math::unit3(float* v, float* out)
 126 {
 127     float imag = 1/mag3(v);
 128     mul3(imag, v, out);
 129 }
 130
 131 void Math::cross3(float* a, float* b, float* out)
 132 {
 133     float ax=a[0], ay=a[1], az=a[2];
 134     float bx=b[0], by=b[1], bz=b[2];
 135     out[0] = ay*bz - by*az;
 136     out[1] = az*bx - bz*ax;
 137     out[2] = ax*by - bx*ay;
 138 }
 139
 140 void Math::mmul33(float* a, float* b, float* out)
 141 {
 142     float tmp[9];
 143     tmp[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6];
 144     tmp[3] = a[3]*b[0] + a[4]*b[3] + a[5]*b[6];
 145     tmp[6] = a[6]*b[0] + a[7]*b[3] + a[8]*b[6];
 146
 147     tmp[1] = a[0]*b[1] + a[1]*b[4] + a[2]*b[7];
 148     tmp[4] = a[3]*b[1] + a[4]*b[4] + a[5]*b[7];
 149     tmp[7] = a[6]*b[1] + a[7]*b[4] + a[8]*b[7];
 150
 151     tmp[2] = a[0]*b[2] + a[1]*b[5] + a[2]*b[8];
 152     tmp[5] = a[3]*b[2] + a[4]*b[5] + a[5]*b[8];
 153     tmp[8] = a[6]*b[2] + a[7]*b[5] + a[8]*b[8];
 154
 155     for(int i=0; i<9; i++)
 156         out[i] = tmp[i];
 157 }
 158
 159 void Math::vmul33(float* m, float* v, float* out)
 160 {
 161     float x = v[0], y = v[1], z = v[2];
 162     out[0] = x*m[0] + y*m[1] + z*m[2];
 163     out[1] = x*m[3] + y*m[4] + z*m[5];
 164     out[2] = x*m[6] + y*m[7] + z*m[8];
 165 }
 166
 167 void Math::tmul33(float* m, float* v, float* out)
 168 {
 169     float x = v[0], y = v[1], z = v[2];
 170     out[0] = x*m[0] + y*m[3] + z*m[6];
 171     out[1] = x*m[1] + y*m[4] + z*m[7];
 172     out[2] = x*m[2] + y*m[5] + z*m[8];
 173 }
 174
 175 void Math::invert33(float* m, float* out)
 176 {
 177     // Compute the inverse as the adjoint matrix times 1/(det M).
 178     // A, B ... I are the cofactors of a b c
 179     //                                 d e f
 180     //                                 g h i
 181     float a=m[0], b=m[1], c=m[2];
 182     float d=m[3], e=m[4], f=m[5];
 183     float g=m[6], h=m[7], i=m[8];
 184
 185     float A =  (e*i - h*f);
 186     float B = -(d*i - g*f);
 187     float C =  (d*h - g*e);
 188     float D = -(b*i - h*c);
 189     float E =  (a*i - g*c);
 190     float F = -(a*h - g*b);
 191     float G =  (b*f - e*c);
 192     float H = -(a*f - d*c);
 193     float I =  (a*e - d*b);
 194
 195     float id = 1/(a*A + b*B + c*C);
 196
 197     out[0] = id*A; out[1] = id*D; out[2] = id*G;
 198     out[3] = id*B; out[4] = id*E; out[5] = id*H;
 199     out[6] = id*C; out[7] = id*F; out[8] = id*I;
 200 }
 201
 202 void Math::trans33(float* m, float* out)
 203 {
 204     // 0 1 2   Elements 0, 4, and 8 are the same
 205     // 3 4 5   Swap elements 1/3, 2/6, and 5/7
 206     // 6 7 8
 207     out[0] = m[0];
 208     out[4] = m[4];
 209     out[8] = m[8];
 210
 211     float tmp = m[1];
 212     out[1] = m[3];
 213     out[3] = tmp;
 214
 215     tmp = m[2];
 216     out[2] = m[6];
 217     out[6] = tmp;
 218
 219     tmp = m[5];
 220     out[5] = m[7];
 221     out[7] = tmp;
 222 }
 223
 224 void Math::ortho33(float* x, float* y,
 225                    float* xOut, float* yOut, float* zOut)
 226 {
 227     float x0[3], y0[3];
 228     set3(x, x0);
 229     set3(y, y0);
 230
 231     unit3(x0, xOut);
 232     cross3(xOut, y0, zOut);
 233     unit3(zOut, zOut);
 234     cross3(zOut, xOut, yOut);
 235 }
 236
 237 }; // namespace yasim