src/FDM/YASim/Math.cpp

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