altos: Add sinf to math code

Needed for the quaternion gyro tracking code

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/src/math/math.h b/src/math/math.h
index c62f583d561f28e5c4d19bbeef485821a6f5008f..fd543bc218cba9f016a4b88133cc3fbc110a3e9c 100644 (file)
--- a/src/math/math.h
+++ b/src/math/math.h
@@ -22,6 +22,8 @@ float acosf(float x);
 
 float cosf(float x);
 
+float sinf(float x);
+
 float sqrtf(float x);
 
 float fabsf(float x);

diff --git a/src/math/sf_sin.c b/src/math/sf_sin.c
new file mode 100644 (file)
index 0000000..da81845
--- /dev/null
+++ b/src/math/sf_sin.c
@@ -0,0 +1,62 @@
+/* sf_sin.c -- float version of s_sin.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+       float sinf(float x)
+#else
+       float sinf(x)
+       float x;
+#endif
+{
+       float y[2],z=0.0;
+       __int32_t n,ix;
+
+       GET_FLOAT_WORD(ix,x);
+
+    /* |x| ~< pi/4 */
+       ix &= 0x7fffffff;
+       if(ix <= 0x3f490fd8) return __kernel_sinf(x,z,0);
+
+    /* sin(Inf or NaN) is NaN */
+       else if (!FLT_UWORD_IS_FINITE(ix)) return x-x;
+
+    /* argument reduction needed */
+       else {
+           n = __ieee754_rem_pio2f(x,y);
+           switch(n&3) {
+               case 0: return  __kernel_sinf(y[0],y[1],1);
+               case 1: return  __kernel_cosf(y[0],y[1]);
+               case 2: return -__kernel_sinf(y[0],y[1],1);
+               default:
+                       return -__kernel_cosf(y[0],y[1]);
+           }
+       }
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+       double sin(double x)
+#else
+       double sin(x)
+       double x;
+#endif
+{
+       return (double) sinf((float) x);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */