Data hosted with ♥ by Pastebin.com - Download Raw - See Original
  1. #include <math.h>
  2.  
  3. /// Computes the sine and cosine of two angles
  4. /// in: angles = Two angles, expressed in radians, in the [-PI,PI] range.
  5. /// out: results = vector containing [sin(angles[0]),cos(angles[0]),sin(angles[1]),cos(angles[1])]
  6. static inline void vsincos(const float angles[2], float results[4]) {
  7.     static const float constants[]  = {
  8.     /* q1 */  0,                M_PI_2,           0,                M_PI_2,
  9.     /* q2 */  M_PI,             M_PI,             M_PI,             M_PI,
  10.     /* q3 */  4.f/M_PI,         4.f/M_PI,         4.f/M_PI,         4.f/M_PI,
  11.     /* q4 */ -4.f/(M_PI*M_PI), -4.f/(M_PI*M_PI), -4.f/(M_PI*M_PI), -4.f/(M_PI*M_PI),
  12.     /* q5 */  2.f,              2.f,              2.f,              2.f,
  13.     /* q6 */  .225f,            .225f,            .225f,            .225f
  14.     };  
  15.     asm volatile(
  16.         // Load q0 with [angle1,angle1,angle2,angle2]
  17.         "vldmia %1, { d3 }\n\t"
  18.         "vdup.f32 d0, d3[0]\n\t"
  19.         "vdup.f32 d1, d3[1]\n\t"
  20.         // Load q1-q6 with constants
  21.         "vldmia %2, { q1-q6 }\n\t"
  22.         // Cos(x) = Sin(x+PI/2), so
  23.         // q0 = [angle1, angle1+PI/2, angle2, angle2+PI/2]
  24.         "vadd.f32 q0,q0,q1\n\t"
  25.         // if angle1+PI/2>PI, substract 2*PI
  26.         // q0-=(q0>PI)?2*PI:0
  27.         "vcge.f32 q1,q0,q2\n\t"
  28.         "vand.f32 q1,q1,q2\n\t"
  29.         "vmls.f32 q0,q1,q5\n\t"
  30.         // q0=(4/PI)*q0 - q0*abs(q0)*4/(PI*PI)
  31.         "vabs.f32 q1,q0\n\t"
  32.         "vmul.f32 q1,q0,q1\n\t"
  33.         "vmul.f32 q0,q0,q3\n\t"
  34.         "vmul.f32 q1,q1,q4\n\t"
  35.         "vadd.f32 q0,q0,q1\n\t"
  36.         // q0+=.225*(q0*abs(q0) - q0)
  37.         "vabs.f32 q1,q0\n\t"
  38.         "vmul.f32 q1,q0,q1\n\t"
  39.         "vsub.f32 q1,q0\n\t"
  40.         "vmla.f32 q0,q1,q6\n\t"
  41.         "vstmia %0, { q0 }\n\t"
  42.         :: "r"(results), "r"(angles), "r"(constants)
  43.         : "memory","cc","q0","q1","q2","q3","q4","q5","q6"
  44.     );  
  45. }