Полезная математика

1. Приближенное значение синуса угла

Приближенное значение синуса

const float PI = 3.14159265358979323846264338327950288f;

const float PI_SQR = 9.86960440108935861883449099987615114f;

const float B = 4/PI;

const float C = -4/(PI_SQR);

float sin_approx( float x ){

    return B * x + C * x * abs(x);

}

2. Приближенное значение арккосинуса

Приближенное значение арккосинуса

float acos_approx( float x ){

    return sqrt(1-x)*(1.5707963267948966192313216916398f + x*(-0.213300989f + x*(0.077980478f + x*-0.02164095f)));

}

3. Приближенное значение квадратного корня (в действительности, степени -2)

Обратный корень квадратный

float InvSqrt (float x){ 

  union{  

    int32_t i;  

    float   f; 

  } conv; 

  conv.f = x; 

  conv.i = 0x5f3759df - (conv.i >> 1); 

  return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);

} 

4. Преобразование кватерниона положения в углы Эйлера

Кватернион в углы Эйлера

void quat_2_euler( float q[4], float e[3] ){

   float sqw = q[0]*q[0];

   float sqx = q[1]*q[1];

   float sqy = q[2]*q[2];

   float sqz = q[3]*q[3];

   e[2] = atan2f(2.f * (q[1]*q[2] + q[3]*q[0]), sqx - sqy - sqz + sqw);

   e[1] = asinf(-2.f * (q[1]*q[3] - q[2]*q[0]));

   e[0] = atan2f(2.f * (q[2]*q[3] + q[1]*q[0]), -sqx - sqy + sqz + sqw);

}