arm_cos_f32.c

00001 /* ----------------------------------------------------------------------  
00002 * Copyright (C) 2010 ARM Limited. All rights reserved.  
00003 *  
00004 * $Date:        29. November 2010  
00005 * $Revision:    V1.0.3  
00006 *  
00007 * Project:      CMSIS DSP Library  
00008 * Title:        arm_cos_f32.c  
00009 *  
00010 * Description:  Fast cosine calculation for floating-point values. 
00011 *  
00012 * Target Processor: Cortex-M4/Cortex-M3
00013 *  
00014 * Version 1.0.3 2010/11/29 
00015 *    Re-organized the CMSIS folders and updated documentation.  
00016 *   
00017 * Version 1.0.2 2010/11/11  
00018 *    Documentation updated.   
00019 *  
00020 * Version 1.0.1 2010/10/05   
00021 *    Production release and review comments incorporated.  
00022 *  
00023 * Version 1.0.0 2010/09/20   
00024 *    Production release and review comments incorporated.  
00025 * -------------------------------------------------------------------- */ 
00026  
00027 #include "arm_math.h" 
00028 /**  
00029  * @ingroup groupFastMath  
00030  */ 
00031  
00032 /**  
00033  * @defgroup cos Cosine  
00034  *  
00035  * Computes the trigonometric cosine function using a combination of table lookup 
00036  * and cubic interpolation.  There are separate functions for 
00037  * Q15, Q31, and floating-point data types. 
00038  * The input to the floating-point version is in radians while the 
00039  * fixed-point Q15 and Q31 have a scaled input with the range 
00040  * [0 1) mapping to [0 2*pi). 
00041  * 
00042  * The implementation is based on table lookup using 256 values together with cubic interpolation. 
00043  * The steps used are: 
00044  *  -# Calculation of the nearest integer table index 
00045  *  -# Fetch the four table values a, b, c, and d   
00046  *  -# Compute the fractional portion (fract) of the table index. 
00047  *  -# Calculation of wa, wb, wc, wd  
00048  *  -# The final result equals <code>a*wa + b*wb + c*wc + d*wd</code> 
00049  * 
00050  * where 
00051  * <pre>  
00052  *    a=Table[index-1];  
00053  *    b=Table[index+0];  
00054  *    c=Table[index+1];  
00055  *    d=Table[index+2];  
00056  * </pre> 
00057  * and 
00058  * <pre>  
00059  *    wa=-(1/6)*fract.^3 + (1/2)*fract.^2 - (1/3)*fract;  
00060  *    wb=(1/2)*fract.^3 - fract.^2 - (1/2)*fract + 1;  
00061  *    wc=-(1/2)*fract.^3+(1/2)*fract.^2+fract;  
00062  *    wd=(1/6)*fract.^3 - (1/6)*fract;  
00063  * </pre>  
00064  */ 
00065  
00066  /**  
00067  * @addtogroup cos  
00068  * @{  
00069  */ 
00070  
00071  
00072 /**  
00073 * \par  
00074 * <b>Example code for Generation of Cos Table:</b> 
00075 * tableSize = 256;  
00076 * <pre>for(n = -1; n < (tableSize + 1); n++)  
00077 * {  
00078 *   cosTable[n+1]= cos(2*pi*n/tableSize);  
00079 * } </pre>  
00080 * where pi value is  3.14159265358979  
00081 */ 
00082  
00083 static const float32_t cosTable [259] = { 
00084   0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f, 
00085   0.998795449733734130f, 0.997290432453155520f, 0.995184719562530520f, 
00086   0.992479562759399410f, 0.989176511764526370f, 
00087   0.985277652740478520f, 0.980785250663757320f, 0.975702106952667240f, 
00088   0.970031261444091800f, 0.963776051998138430f, 0.956940352916717530f, 
00089   0.949528157711029050f, 0.941544055938720700f, 
00090   0.932992815971374510f, 0.923879504203796390f, 0.914209783077239990f, 
00091   0.903989315032958980f, 0.893224298954010010f, 0.881921291351318360f, 
00092   0.870086967945098880f, 0.857728600502014160f, 
00093   0.844853579998016360f, 0.831469595432281490f, 0.817584812641143800f, 
00094   0.803207516670227050f, 0.788346409797668460f, 0.773010432720184330f, 
00095   0.757208824157714840f, 0.740951120853424070f, 
00096   0.724247097969055180f, 0.707106769084930420f, 0.689540565013885500f, 
00097   0.671558976173400880f, 0.653172850608825680f, 0.634393274784088130f, 
00098   0.615231573581695560f, 0.595699310302734380f, 
00099   0.575808167457580570f, 0.555570244789123540f, 0.534997642040252690f, 
00100   0.514102756977081300f, 0.492898195981979370f, 0.471396744251251220f, 
00101   0.449611335992813110f, 0.427555084228515630f, 
00102   0.405241310596466060f, 0.382683426141738890f, 0.359895050525665280f, 
00103   0.336889863014221190f, 0.313681751489639280f, 0.290284663438797000f, 
00104   0.266712754964828490f, 0.242980182170867920f, 
00105   0.219101235270500180f, 0.195090323686599730f, 0.170961886644363400f, 
00106   0.146730467677116390f, 0.122410677373409270f, 0.098017141222953796f, 
00107   0.073564566671848297f, 0.049067676067352295f, 
00108   0.024541229009628296f, 0.000000000000000061f, -0.024541229009628296f, 
00109   -0.049067676067352295f, -0.073564566671848297f, -0.098017141222953796f, 
00110   -0.122410677373409270f, -0.146730467677116390f, 
00111   -0.170961886644363400f, -0.195090323686599730f, -0.219101235270500180f, 
00112   -0.242980182170867920f, -0.266712754964828490f, -0.290284663438797000f, 
00113   -0.313681751489639280f, -0.336889863014221190f, 
00114   -0.359895050525665280f, -0.382683426141738890f, -0.405241310596466060f, 
00115   -0.427555084228515630f, -0.449611335992813110f, -0.471396744251251220f, 
00116   -0.492898195981979370f, -0.514102756977081300f, 
00117   -0.534997642040252690f, -0.555570244789123540f, -0.575808167457580570f, 
00118   -0.595699310302734380f, -0.615231573581695560f, -0.634393274784088130f, 
00119   -0.653172850608825680f, -0.671558976173400880f, 
00120   -0.689540565013885500f, -0.707106769084930420f, -0.724247097969055180f, 
00121   -0.740951120853424070f, -0.757208824157714840f, -0.773010432720184330f, 
00122   -0.788346409797668460f, -0.803207516670227050f, 
00123   -0.817584812641143800f, -0.831469595432281490f, -0.844853579998016360f, 
00124   -0.857728600502014160f, -0.870086967945098880f, -0.881921291351318360f, 
00125   -0.893224298954010010f, -0.903989315032958980f, 
00126   -0.914209783077239990f, -0.923879504203796390f, -0.932992815971374510f, 
00127   -0.941544055938720700f, -0.949528157711029050f, -0.956940352916717530f, 
00128   -0.963776051998138430f, -0.970031261444091800f, 
00129   -0.975702106952667240f, -0.980785250663757320f, -0.985277652740478520f, 
00130   -0.989176511764526370f, -0.992479562759399410f, -0.995184719562530520f, 
00131   -0.997290432453155520f, -0.998795449733734130f, 
00132   -0.999698817729949950f, -1.000000000000000000f, -0.999698817729949950f, 
00133   -0.998795449733734130f, -0.997290432453155520f, -0.995184719562530520f, 
00134   -0.992479562759399410f, -0.989176511764526370f, 
00135   -0.985277652740478520f, -0.980785250663757320f, -0.975702106952667240f, 
00136   -0.970031261444091800f, -0.963776051998138430f, -0.956940352916717530f, 
00137   -0.949528157711029050f, -0.941544055938720700f, 
00138   -0.932992815971374510f, -0.923879504203796390f, -0.914209783077239990f, 
00139   -0.903989315032958980f, -0.893224298954010010f, -0.881921291351318360f, 
00140   -0.870086967945098880f, -0.857728600502014160f, 
00141   -0.844853579998016360f, -0.831469595432281490f, -0.817584812641143800f, 
00142   -0.803207516670227050f, -0.788346409797668460f, -0.773010432720184330f, 
00143   -0.757208824157714840f, -0.740951120853424070f, 
00144   -0.724247097969055180f, -0.707106769084930420f, -0.689540565013885500f, 
00145   -0.671558976173400880f, -0.653172850608825680f, -0.634393274784088130f, 
00146   -0.615231573581695560f, -0.595699310302734380f, 
00147   -0.575808167457580570f, -0.555570244789123540f, -0.534997642040252690f, 
00148   -0.514102756977081300f, -0.492898195981979370f, -0.471396744251251220f, 
00149   -0.449611335992813110f, -0.427555084228515630f, 
00150   -0.405241310596466060f, -0.382683426141738890f, -0.359895050525665280f, 
00151   -0.336889863014221190f, -0.313681751489639280f, -0.290284663438797000f, 
00152   -0.266712754964828490f, -0.242980182170867920f, 
00153   -0.219101235270500180f, -0.195090323686599730f, -0.170961886644363400f, 
00154   -0.146730467677116390f, -0.122410677373409270f, -0.098017141222953796f, 
00155   -0.073564566671848297f, -0.049067676067352295f, 
00156   -0.024541229009628296f, -0.000000000000000184f, 0.024541229009628296f, 
00157   0.049067676067352295f, 0.073564566671848297f, 0.098017141222953796f, 
00158   0.122410677373409270f, 0.146730467677116390f, 
00159   0.170961886644363400f, 0.195090323686599730f, 0.219101235270500180f, 
00160   0.242980182170867920f, 0.266712754964828490f, 0.290284663438797000f, 
00161   0.313681751489639280f, 0.336889863014221190f, 
00162   0.359895050525665280f, 0.382683426141738890f, 0.405241310596466060f, 
00163   0.427555084228515630f, 0.449611335992813110f, 0.471396744251251220f, 
00164   0.492898195981979370f, 0.514102756977081300f, 
00165   0.534997642040252690f, 0.555570244789123540f, 0.575808167457580570f, 
00166   0.595699310302734380f, 0.615231573581695560f, 0.634393274784088130f, 
00167   0.653172850608825680f, 0.671558976173400880f, 
00168   0.689540565013885500f, 0.707106769084930420f, 0.724247097969055180f, 
00169   0.740951120853424070f, 0.757208824157714840f, 0.773010432720184330f, 
00170   0.788346409797668460f, 0.803207516670227050f, 
00171   0.817584812641143800f, 0.831469595432281490f, 0.844853579998016360f, 
00172   0.857728600502014160f, 0.870086967945098880f, 0.881921291351318360f, 
00173   0.893224298954010010f, 0.903989315032958980f, 
00174   0.914209783077239990f, 0.923879504203796390f, 0.932992815971374510f, 
00175   0.941544055938720700f, 0.949528157711029050f, 0.956940352916717530f, 
00176   0.963776051998138430f, 0.970031261444091800f, 
00177   0.975702106952667240f, 0.980785250663757320f, 0.985277652740478520f, 
00178   0.989176511764526370f, 0.992479562759399410f, 0.995184719562530520f, 
00179   0.997290432453155520f, 0.998795449733734130f, 
00180   0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f 
00181 }; 
00182  
00183 /** 
00184  * @brief  Fast approximation to the trigonometric cosine function for floating-point data. 
00185  * @param[in] x input value in radians. 
00186  * @return cos(x). 
00187  */ 
00188  
00189 float32_t arm_cos_f32( 
00190   float32_t x) 
00191 { 
00192   float32_t cosVal, fract, in; 
00193   uint32_t index; 
00194   uint32_t tableSize = (uint32_t) TABLE_SIZE; 
00195   float32_t wa, wb, wc, wd; 
00196   float32_t a, b, c, d; 
00197   float32_t *tablePtr; 
00198   int32_t n; 
00199  
00200   /* input x is in radians */ 
00201   /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */ 
00202   in = x * 0.159154943092f; 
00203  
00204   /* Calculation of floor value of input */ 
00205   n = (int32_t) in; 
00206  
00207   /* Make negative values towards -infinity */ 
00208   if(x < 0.0f) 
00209   { 
00210     n = n - 1; 
00211   } 
00212  
00213   /* Map input value to [0 1] */ 
00214   in = in - (float32_t) n; 
00215  
00216   /* Calculation of index of the table */ 
00217   index = (uint32_t) (tableSize * in); 
00218  
00219   /* fractional value calculation */ 
00220   fract = ((float32_t) tableSize * in) - (float32_t) index; 
00221  
00222   /* Initialise table pointer */ 
00223   tablePtr = (float32_t *) & cosTable [index]; 
00224  
00225   /* Read four nearest values of input value from the cos table */ 
00226   a = *tablePtr++; 
00227   b = *tablePtr++; 
00228   c = *tablePtr++; 
00229   d = *tablePtr++; 
00230  
00231   /* Cubic interpolation process */ 
00232   wa = -(((0.166666667f) * fract) * (fract * fract)) + 
00233         (((0.5f) * (fract * fract)) - ((0.3333333333333f) * fract)); 
00234   wb = ((((0.5f) * fract) * (fract * fract)) - (fract * fract)) + 
00235        (-((0.5f) * fract) + 1.0f); 
00236   wc = -(((0.5f) * fract) * (fract * fract)) +  
00237         (((0.5f) * (fract * fract)) + fract); 
00238   wd = (((0.166666667f) * fract) * (fract * fract)) -  
00239        ((0.166666667f) * fract); 
00240  
00241   /* Calculate cos value */ 
00242   cosVal = ((a * wa) + (b * wb)) + ((c * wc) + (d * wd)); 
00243  
00244   /* Return the output value */ 
00245   return (cosVal); 
00246  
00247 } 
00248  
00249 /**  
00250  * @} end of cos group  
00251  */