arm_cos_f32.c

00001 /* ----------------------------------------------------------------------    
00002 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.    
00003 *    
00004 * $Date:        17. January 2013
00005 * $Revision:    V1.4.1
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/Cortex-M0
00013 *  
00014 * Redistribution and use in source and binary forms, with or without 
00015 * modification, are permitted provided that the following conditions
00016 * are met:
00017 *   - Redistributions of source code must retain the above copyright
00018 *     notice, this list of conditions and the following disclaimer.
00019 *   - Redistributions in binary form must reproduce the above copyright
00020 *     notice, this list of conditions and the following disclaimer in
00021 *     the documentation and/or other materials provided with the 
00022 *     distribution.
00023 *   - Neither the name of ARM LIMITED nor the names of its contributors
00024 *     may be used to endorse or promote products derived from this
00025 *     software without specific prior written permission.
00026 *
00027 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00028 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00029 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00030 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
00031 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00032 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00033 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00034 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00035 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00036 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00037 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00038 * POSSIBILITY OF SUCH DAMAGE.    
00039 * -------------------------------------------------------------------- */
00040 
00041 #include "arm_math.h"
00042 /**    
00043  * @ingroup groupFastMath    
00044  */
00045 
00046 /**    
00047  * @defgroup cos Cosine    
00048  *    
00049  * Computes the trigonometric cosine function using a combination of table lookup   
00050  * and cubic interpolation.  There are separate functions for   
00051  * Q15, Q31, and floating-point data types.   
00052  * The input to the floating-point version is in radians while the   
00053  * fixed-point Q15 and Q31 have a scaled input with the range   
00054  * [0 +0.9999] mapping to [0 2*pi).  The fixed-point range is chosen so that a
00055  * value of 2*pi wraps around to 0.
00056  *   
00057  * The implementation is based on table lookup using 256 values together with cubic interpolation.   
00058  * The steps used are:   
00059  *  -# Calculation of the nearest integer table index   
00060  *  -# Fetch the four table values a, b, c, and d     
00061  *  -# Compute the fractional portion (fract) of the table index.   
00062  *  -# Calculation of wa, wb, wc, wd    
00063  *  -# The final result equals <code>a*wa + b*wb + c*wc + d*wd</code>   
00064  *   
00065  * where   
00066  * <pre>    
00067  *    a=Table[index-1];    
00068  *    b=Table[index+0];    
00069  *    c=Table[index+1];    
00070  *    d=Table[index+2];    
00071  * </pre>   
00072  * and   
00073  * <pre>    
00074  *    wa=-(1/6)*fract.^3 + (1/2)*fract.^2 - (1/3)*fract;    
00075  *    wb=(1/2)*fract.^3 - fract.^2 - (1/2)*fract + 1;    
00076  *    wc=-(1/2)*fract.^3+(1/2)*fract.^2+fract;    
00077  *    wd=(1/6)*fract.^3 - (1/6)*fract;    
00078  * </pre>    
00079  */
00080 
00081  /**    
00082  * @addtogroup cos    
00083  * @{    
00084  */
00085 
00086 
00087 /**    
00088 * \par    
00089 * <b>Example code for Generation of Cos Table:</b>
00090 * <pre>
00091 * tableSize = 256;    
00092 * for(n = -1; n < (tableSize + 2); n++)    
00093 * {    
00094 *   cosTable[n+1]= cos(2*pi*n/tableSize);    
00095 * } </pre>    
00096 * where pi value is  3.14159265358979    
00097 */
00098 
00099 static const float32_t cosTable [260] = {
00100   0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
00101   0.998795449733734130f, 0.997290432453155520f, 0.995184719562530520f,
00102   0.992479562759399410f, 0.989176511764526370f,
00103   0.985277652740478520f, 0.980785250663757320f, 0.975702106952667240f,
00104   0.970031261444091800f, 0.963776051998138430f, 0.956940352916717530f,
00105   0.949528157711029050f, 0.941544055938720700f,
00106   0.932992815971374510f, 0.923879504203796390f, 0.914209783077239990f,
00107   0.903989315032958980f, 0.893224298954010010f, 0.881921291351318360f,
00108   0.870086967945098880f, 0.857728600502014160f,
00109   0.844853579998016360f, 0.831469595432281490f, 0.817584812641143800f,
00110   0.803207516670227050f, 0.788346409797668460f, 0.773010432720184330f,
00111   0.757208824157714840f, 0.740951120853424070f,
00112   0.724247097969055180f, 0.707106769084930420f, 0.689540565013885500f,
00113   0.671558976173400880f, 0.653172850608825680f, 0.634393274784088130f,
00114   0.615231573581695560f, 0.595699310302734380f,
00115   0.575808167457580570f, 0.555570244789123540f, 0.534997642040252690f,
00116   0.514102756977081300f, 0.492898195981979370f, 0.471396744251251220f,
00117   0.449611335992813110f, 0.427555084228515630f,
00118   0.405241310596466060f, 0.382683426141738890f, 0.359895050525665280f,
00119   0.336889863014221190f, 0.313681751489639280f, 0.290284663438797000f,
00120   0.266712754964828490f, 0.242980182170867920f,
00121   0.219101235270500180f, 0.195090323686599730f, 0.170961886644363400f,
00122   0.146730467677116390f, 0.122410677373409270f, 0.098017141222953796f,
00123   0.073564566671848297f, 0.049067676067352295f,
00124   0.024541229009628296f, 0.000000000000000061f, -0.024541229009628296f,
00125   -0.049067676067352295f, -0.073564566671848297f, -0.098017141222953796f,
00126   -0.122410677373409270f, -0.146730467677116390f,
00127   -0.170961886644363400f, -0.195090323686599730f, -0.219101235270500180f,
00128   -0.242980182170867920f, -0.266712754964828490f, -0.290284663438797000f,
00129   -0.313681751489639280f, -0.336889863014221190f,
00130   -0.359895050525665280f, -0.382683426141738890f, -0.405241310596466060f,
00131   -0.427555084228515630f, -0.449611335992813110f, -0.471396744251251220f,
00132   -0.492898195981979370f, -0.514102756977081300f,
00133   -0.534997642040252690f, -0.555570244789123540f, -0.575808167457580570f,
00134   -0.595699310302734380f, -0.615231573581695560f, -0.634393274784088130f,
00135   -0.653172850608825680f, -0.671558976173400880f,
00136   -0.689540565013885500f, -0.707106769084930420f, -0.724247097969055180f,
00137   -0.740951120853424070f, -0.757208824157714840f, -0.773010432720184330f,
00138   -0.788346409797668460f, -0.803207516670227050f,
00139   -0.817584812641143800f, -0.831469595432281490f, -0.844853579998016360f,
00140   -0.857728600502014160f, -0.870086967945098880f, -0.881921291351318360f,
00141   -0.893224298954010010f, -0.903989315032958980f,
00142   -0.914209783077239990f, -0.923879504203796390f, -0.932992815971374510f,
00143   -0.941544055938720700f, -0.949528157711029050f, -0.956940352916717530f,
00144   -0.963776051998138430f, -0.970031261444091800f,
00145   -0.975702106952667240f, -0.980785250663757320f, -0.985277652740478520f,
00146   -0.989176511764526370f, -0.992479562759399410f, -0.995184719562530520f,
00147   -0.997290432453155520f, -0.998795449733734130f,
00148   -0.999698817729949950f, -1.000000000000000000f, -0.999698817729949950f,
00149   -0.998795449733734130f, -0.997290432453155520f, -0.995184719562530520f,
00150   -0.992479562759399410f, -0.989176511764526370f,
00151   -0.985277652740478520f, -0.980785250663757320f, -0.975702106952667240f,
00152   -0.970031261444091800f, -0.963776051998138430f, -0.956940352916717530f,
00153   -0.949528157711029050f, -0.941544055938720700f,
00154   -0.932992815971374510f, -0.923879504203796390f, -0.914209783077239990f,
00155   -0.903989315032958980f, -0.893224298954010010f, -0.881921291351318360f,
00156   -0.870086967945098880f, -0.857728600502014160f,
00157   -0.844853579998016360f, -0.831469595432281490f, -0.817584812641143800f,
00158   -0.803207516670227050f, -0.788346409797668460f, -0.773010432720184330f,
00159   -0.757208824157714840f, -0.740951120853424070f,
00160   -0.724247097969055180f, -0.707106769084930420f, -0.689540565013885500f,
00161   -0.671558976173400880f, -0.653172850608825680f, -0.634393274784088130f,
00162   -0.615231573581695560f, -0.595699310302734380f,
00163   -0.575808167457580570f, -0.555570244789123540f, -0.534997642040252690f,
00164   -0.514102756977081300f, -0.492898195981979370f, -0.471396744251251220f,
00165   -0.449611335992813110f, -0.427555084228515630f,
00166   -0.405241310596466060f, -0.382683426141738890f, -0.359895050525665280f,
00167   -0.336889863014221190f, -0.313681751489639280f, -0.290284663438797000f,
00168   -0.266712754964828490f, -0.242980182170867920f,
00169   -0.219101235270500180f, -0.195090323686599730f, -0.170961886644363400f,
00170   -0.146730467677116390f, -0.122410677373409270f, -0.098017141222953796f,
00171   -0.073564566671848297f, -0.049067676067352295f,
00172   -0.024541229009628296f, -0.000000000000000184f, 0.024541229009628296f,
00173   0.049067676067352295f, 0.073564566671848297f, 0.098017141222953796f,
00174   0.122410677373409270f, 0.146730467677116390f,
00175   0.170961886644363400f, 0.195090323686599730f, 0.219101235270500180f,
00176   0.242980182170867920f, 0.266712754964828490f, 0.290284663438797000f,
00177   0.313681751489639280f, 0.336889863014221190f,
00178   0.359895050525665280f, 0.382683426141738890f, 0.405241310596466060f,
00179   0.427555084228515630f, 0.449611335992813110f, 0.471396744251251220f,
00180   0.492898195981979370f, 0.514102756977081300f,
00181   0.534997642040252690f, 0.555570244789123540f, 0.575808167457580570f,
00182   0.595699310302734380f, 0.615231573581695560f, 0.634393274784088130f,
00183   0.653172850608825680f, 0.671558976173400880f,
00184   0.689540565013885500f, 0.707106769084930420f, 0.724247097969055180f,
00185   0.740951120853424070f, 0.757208824157714840f, 0.773010432720184330f,
00186   0.788346409797668460f, 0.803207516670227050f,
00187   0.817584812641143800f, 0.831469595432281490f, 0.844853579998016360f,
00188   0.857728600502014160f, 0.870086967945098880f, 0.881921291351318360f,
00189   0.893224298954010010f, 0.903989315032958980f,
00190   0.914209783077239990f, 0.923879504203796390f, 0.932992815971374510f,
00191   0.941544055938720700f, 0.949528157711029050f, 0.956940352916717530f,
00192   0.963776051998138430f, 0.970031261444091800f,
00193   0.975702106952667240f, 0.980785250663757320f, 0.985277652740478520f,
00194   0.989176511764526370f, 0.992479562759399410f, 0.995184719562530520f,
00195   0.997290432453155520f, 0.998795449733734130f,
00196   0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
00197   0.998795449733734130f
00198 };
00199 
00200 /**   
00201  * @brief  Fast approximation to the trigonometric cosine function for floating-point data.   
00202  * @param[in] x input value in radians.   
00203  * @return cos(x).   
00204  */
00205 
00206 
00207 float32_t arm_cos_f32(
00208   float32_t x)
00209 {
00210   float32_t cosVal, fract, in;
00211   int32_t index;
00212   uint32_t tableSize = (uint32_t) TABLE_SIZE;
00213   float32_t wa, wb, wc, wd;
00214   float32_t a, b, c, d;
00215   float32_t *tablePtr;
00216   int32_t n;
00217   float32_t fractsq, fractby2, fractby6, fractby3, fractsqby2;
00218   float32_t oneminusfractby2;
00219   float32_t frby2xfrsq, frby6xfrsq;
00220 
00221   /* input x is in radians */
00222   /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */
00223   in = x * 0.159154943092f;
00224 
00225   /* Calculation of floor value of input */
00226   n = (int32_t) in;
00227 
00228   /* Make negative values towards -infinity */
00229   if(x < 0.0f)
00230   {
00231     n = n - 1;
00232   }
00233 
00234   /* Map input value to [0 1] */
00235   in = in - (float32_t) n;
00236 
00237   /* Calculation of index of the table */
00238   index = (uint32_t) (tableSize * in);
00239 
00240   /* fractional value calculation */
00241   fract = ((float32_t) tableSize * in) - (float32_t) index;
00242 
00243   /* Checking min and max index of table */
00244   if(index < 0)
00245   {
00246     index = 0;
00247   }
00248   else if(index > 256)
00249   {
00250     index = 256;
00251   }
00252 
00253   /* Initialise table pointer */
00254   tablePtr = (float32_t *) & cosTable [index];
00255 
00256   /* Read four nearest values of input value from the cos table */
00257   a = tablePtr[0];
00258   b = tablePtr[1];
00259   c = tablePtr[2];
00260   d = tablePtr[3];
00261 
00262   /* Cubic interpolation process */
00263   fractsq = fract * fract;
00264   fractby2 = fract * 0.5f;
00265   fractby6 = fract * 0.166666667f;
00266   fractby3 = fract * 0.3333333333333f;
00267   fractsqby2 = fractsq * 0.5f;
00268   frby2xfrsq = (fractby2) * fractsq;
00269   frby6xfrsq = (fractby6) * fractsq;
00270   oneminusfractby2 = 1.0f - fractby2;
00271   wb = fractsqby2 - fractby3;
00272   wc = (fractsqby2 + fract);
00273   wa = wb - frby6xfrsq;
00274   wb = frby2xfrsq - fractsq;
00275   cosVal = wa * a;
00276   wc = wc - frby2xfrsq;
00277   wd = (frby6xfrsq) - fractby6;
00278   wb = wb + oneminusfractby2;
00279 
00280   /* Calculate cos value */
00281   cosVal = (cosVal + (b * wb)) + ((c * wc) + (d * wd));
00282 
00283   /* Return the output value */
00284   return (cosVal);
00285 
00286 }
00287 
00288 /**    
00289  * @} end of cos group    
00290  */