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