tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FastMathFunctions/arm_cos_f32.c

   1 /* ----------------------------------------------------------------------
   2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
   3 *
   4 * $Date:        17. January 2013
   5 * $Revision:    V1.4.1
   6 *
   7 * Project:          CMSIS DSP Library
   8 * Title:                arm_cos_f32.c
   9 *
  10 * Description:  Fast cosine calculation for floating-point values.
  11 *
  12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  13 *
  14 * Redistribution and use in source and binary forms, with or without
  15 * modification, are permitted provided that the following conditions
  16 * are met:
  17 *   - Redistributions of source code must retain the above copyright
  18 *     notice, this list of conditions and the following disclaimer.
  19 *   - Redistributions in binary form must reproduce the above copyright
  20 *     notice, this list of conditions and the following disclaimer in
  21 *     the documentation and/or other materials provided with the
  22 *     distribution.
  23 *   - Neither the name of ARM LIMITED nor the names of its contributors
  24 *     may be used to endorse or promote products derived from this
  25 *     software without specific prior written permission.
  26 *
  27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  38 * POSSIBILITY OF SUCH DAMAGE.
  39 * -------------------------------------------------------------------- */
  40
  41 #include "arm_math.h"
  42 /**
  43  * @ingroup groupFastMath
  44  */
  45
  46 /**
  47  * @defgroup cos Cosine
  48  *
  49  * Computes the trigonometric cosine function using a combination of table lookup
  50  * and cubic interpolation.  There are separate functions for
  51  * Q15, Q31, and floating-point data types.
  52  * The input to the floating-point version is in radians while the
  53  * fixed-point Q15 and Q31 have a scaled input with the range
  54  * [0 +0.9999] mapping to [0 2*pi).  The fixed-point range is chosen so that a
  55  * value of 2*pi wraps around to 0.
  56  *
  57  * The implementation is based on table lookup using 256 values together with cubic interpolation.
  58  * The steps used are:
  59  *  -# Calculation of the nearest integer table index
  60  *  -# Fetch the four table values a, b, c, and d
  61  *  -# Compute the fractional portion (fract) of the table index.
  62  *  -# Calculation of wa, wb, wc, wd
  63  *  -# The final result equals <code>a*wa + b*wb + c*wc + d*wd</code>
  64  *
  65  * where
  66  * <pre>
  67  *    a=Table[index-1];
  68  *    b=Table[index+0];
  69  *    c=Table[index+1];
  70  *    d=Table[index+2];
  71  * </pre>
  72  * and
  73  * <pre>
  74  *    wa=-(1/6)*fract.^3 + (1/2)*fract.^2 - (1/3)*fract;
  75  *    wb=(1/2)*fract.^3 - fract.^2 - (1/2)*fract + 1;
  76  *    wc=-(1/2)*fract.^3+(1/2)*fract.^2+fract;
  77  *    wd=(1/6)*fract.^3 - (1/6)*fract;
  78  * </pre>
  79  */
  80
  81  /**
  82  * @addtogroup cos
  83  * @{
  84  */
  85
  86
  87 /**
  88 * \par
  89 * <b>Example code for Generation of Cos Table:</b>
  90 * <pre>
  91 * tableSize = 256;
  92 * for(n = -1; n < (tableSize + 2); n++)
  93 * {
  94 *       cosTable[n+1]= cos(2*pi*n/tableSize);
  95 * } </pre>
  96 * where pi value is  3.14159265358979
  97 */
  98
  99 static const float32_t cosTable[260] = {
 100   0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
 101   0.998795449733734130f, 0.997290432453155520f, 0.995184719562530520f,
 102   0.992479562759399410f, 0.989176511764526370f,
 103   0.985277652740478520f, 0.980785250663757320f, 0.975702106952667240f,
 104   0.970031261444091800f, 0.963776051998138430f, 0.956940352916717530f,
 105   0.949528157711029050f, 0.941544055938720700f,
 106   0.932992815971374510f, 0.923879504203796390f, 0.914209783077239990f,
 107   0.903989315032958980f, 0.893224298954010010f, 0.881921291351318360f,
 108   0.870086967945098880f, 0.857728600502014160f,
 109   0.844853579998016360f, 0.831469595432281490f, 0.817584812641143800f,
 110   0.803207516670227050f, 0.788346409797668460f, 0.773010432720184330f,
 111   0.757208824157714840f, 0.740951120853424070f,
 112   0.724247097969055180f, 0.707106769084930420f, 0.689540565013885500f,
 113   0.671558976173400880f, 0.653172850608825680f, 0.634393274784088130f,
 114   0.615231573581695560f, 0.595699310302734380f,
 115   0.575808167457580570f, 0.555570244789123540f, 0.534997642040252690f,
 116   0.514102756977081300f, 0.492898195981979370f, 0.471396744251251220f,
 117   0.449611335992813110f, 0.427555084228515630f,
 118   0.405241310596466060f, 0.382683426141738890f, 0.359895050525665280f,
 119   0.336889863014221190f, 0.313681751489639280f, 0.290284663438797000f,
 120   0.266712754964828490f, 0.242980182170867920f,
 121   0.219101235270500180f, 0.195090323686599730f, 0.170961886644363400f,
 122   0.146730467677116390f, 0.122410677373409270f, 0.098017141222953796f,
 123   0.073564566671848297f, 0.049067676067352295f,
 124   0.024541229009628296f, 0.000000000000000061f, -0.024541229009628296f,
 125   -0.049067676067352295f, -0.073564566671848297f, -0.098017141222953796f,
 126   -0.122410677373409270f, -0.146730467677116390f,
 127   -0.170961886644363400f, -0.195090323686599730f, -0.219101235270500180f,
 128   -0.242980182170867920f, -0.266712754964828490f, -0.290284663438797000f,
 129   -0.313681751489639280f, -0.336889863014221190f,
 130   -0.359895050525665280f, -0.382683426141738890f, -0.405241310596466060f,
 131   -0.427555084228515630f, -0.449611335992813110f, -0.471396744251251220f,
 132   -0.492898195981979370f, -0.514102756977081300f,
 133   -0.534997642040252690f, -0.555570244789123540f, -0.575808167457580570f,
 134   -0.595699310302734380f, -0.615231573581695560f, -0.634393274784088130f,
 135   -0.653172850608825680f, -0.671558976173400880f,
 136   -0.689540565013885500f, -0.707106769084930420f, -0.724247097969055180f,
 137   -0.740951120853424070f, -0.757208824157714840f, -0.773010432720184330f,
 138   -0.788346409797668460f, -0.803207516670227050f,
 139   -0.817584812641143800f, -0.831469595432281490f, -0.844853579998016360f,
 140   -0.857728600502014160f, -0.870086967945098880f, -0.881921291351318360f,
 141   -0.893224298954010010f, -0.903989315032958980f,
 142   -0.914209783077239990f, -0.923879504203796390f, -0.932992815971374510f,
 143   -0.941544055938720700f, -0.949528157711029050f, -0.956940352916717530f,
 144   -0.963776051998138430f, -0.970031261444091800f,
 145   -0.975702106952667240f, -0.980785250663757320f, -0.985277652740478520f,
 146   -0.989176511764526370f, -0.992479562759399410f, -0.995184719562530520f,
 147   -0.997290432453155520f, -0.998795449733734130f,
 148   -0.999698817729949950f, -1.000000000000000000f, -0.999698817729949950f,
 149   -0.998795449733734130f, -0.997290432453155520f, -0.995184719562530520f,
 150   -0.992479562759399410f, -0.989176511764526370f,
 151   -0.985277652740478520f, -0.980785250663757320f, -0.975702106952667240f,
 152   -0.970031261444091800f, -0.963776051998138430f, -0.956940352916717530f,
 153   -0.949528157711029050f, -0.941544055938720700f,
 154   -0.932992815971374510f, -0.923879504203796390f, -0.914209783077239990f,
 155   -0.903989315032958980f, -0.893224298954010010f, -0.881921291351318360f,
 156   -0.870086967945098880f, -0.857728600502014160f,
 157   -0.844853579998016360f, -0.831469595432281490f, -0.817584812641143800f,
 158   -0.803207516670227050f, -0.788346409797668460f, -0.773010432720184330f,
 159   -0.757208824157714840f, -0.740951120853424070f,
 160   -0.724247097969055180f, -0.707106769084930420f, -0.689540565013885500f,
 161   -0.671558976173400880f, -0.653172850608825680f, -0.634393274784088130f,
 162   -0.615231573581695560f, -0.595699310302734380f,
 163   -0.575808167457580570f, -0.555570244789123540f, -0.534997642040252690f,
 164   -0.514102756977081300f, -0.492898195981979370f, -0.471396744251251220f,
 165   -0.449611335992813110f, -0.427555084228515630f,
 166   -0.405241310596466060f, -0.382683426141738890f, -0.359895050525665280f,
 167   -0.336889863014221190f, -0.313681751489639280f, -0.290284663438797000f,
 168   -0.266712754964828490f, -0.242980182170867920f,
 169   -0.219101235270500180f, -0.195090323686599730f, -0.170961886644363400f,
 170   -0.146730467677116390f, -0.122410677373409270f, -0.098017141222953796f,
 171   -0.073564566671848297f, -0.049067676067352295f,
 172   -0.024541229009628296f, -0.000000000000000184f, 0.024541229009628296f,
 173   0.049067676067352295f, 0.073564566671848297f, 0.098017141222953796f,
 174   0.122410677373409270f, 0.146730467677116390f,
 175   0.170961886644363400f, 0.195090323686599730f, 0.219101235270500180f,
 176   0.242980182170867920f, 0.266712754964828490f, 0.290284663438797000f,
 177   0.313681751489639280f, 0.336889863014221190f,
 178   0.359895050525665280f, 0.382683426141738890f, 0.405241310596466060f,
 179   0.427555084228515630f, 0.449611335992813110f, 0.471396744251251220f,
 180   0.492898195981979370f, 0.514102756977081300f,
 181   0.534997642040252690f, 0.555570244789123540f, 0.575808167457580570f,
 182   0.595699310302734380f, 0.615231573581695560f, 0.634393274784088130f,
 183   0.653172850608825680f, 0.671558976173400880f,
 184   0.689540565013885500f, 0.707106769084930420f, 0.724247097969055180f,
 185   0.740951120853424070f, 0.757208824157714840f, 0.773010432720184330f,
 186   0.788346409797668460f, 0.803207516670227050f,
 187   0.817584812641143800f, 0.831469595432281490f, 0.844853579998016360f,
 188   0.857728600502014160f, 0.870086967945098880f, 0.881921291351318360f,
 189   0.893224298954010010f, 0.903989315032958980f,
 190   0.914209783077239990f, 0.923879504203796390f, 0.932992815971374510f,
 191   0.941544055938720700f, 0.949528157711029050f, 0.956940352916717530f,
 192   0.963776051998138430f, 0.970031261444091800f,
 193   0.975702106952667240f, 0.980785250663757320f, 0.985277652740478520f,
 194   0.989176511764526370f, 0.992479562759399410f, 0.995184719562530520f,
 195   0.997290432453155520f, 0.998795449733734130f,
 196   0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
 197   0.998795449733734130f
 198 };
 199
 200 /**
 201  * @brief  Fast approximation to the trigonometric cosine function for floating-point data.
 202  * @param[in] x input value in radians.
 203  * @return cos(x).
 204  */
 205
 206
 207 float32_t arm_cos_f32(
 208   float32_t x)
 209 {
 210   float32_t cosVal, fract, in;
 211   int32_t index;
 212   uint32_t tableSize = (uint32_t) TABLE_SIZE;
 213   float32_t wa, wb, wc, wd;
 214   float32_t a, b, c, d;
 215   float32_t *tablePtr;
 216   int32_t n;
 217   float32_t fractsq, fractby2, fractby6, fractby3, fractsqby2;
 218   float32_t oneminusfractby2;
 219   float32_t frby2xfrsq, frby6xfrsq;
 220
 221   /* input x is in radians */
 222   /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */
 223   in = x * 0.159154943092f;
 224
 225   /* Calculation of floor value of input */
 226   n = (int32_t) in;
 227
 228   /* Make negative values towards -infinity */
 229   if(x < 0.0f)
 230   {
 231     n = n - 1;
 232   }
 233
 234   /* Map input value to [0 1] */
 235   in = in - (float32_t) n;
 236
 237   /* Calculation of index of the table */
 238   index = (uint32_t) (tableSize * in);
 239
 240   /* fractional value calculation */
 241   fract = ((float32_t) tableSize * in) - (float32_t) index;
 242
 243   /* Checking min and max index of table */
 244   if(index < 0)
 245   {
 246     index = 0;
 247   }
 248   else if(index > 256)
 249   {
 250     index = 256;
 251   }
 252
 253   /* Initialise table pointer */
 254   tablePtr = (float32_t *) & cosTable[index];
 255
 256   /* Read four nearest values of input value from the cos table */
 257   a = tablePtr[0];
 258   b = tablePtr[1];
 259   c = tablePtr[2];
 260   d = tablePtr[3];
 261
 262   /* Cubic interpolation process */
 263   fractsq = fract * fract;
 264   fractby2 = fract * 0.5f;
 265   fractby6 = fract * 0.166666667f;
 266   fractby3 = fract * 0.3333333333333f;
 267   fractsqby2 = fractsq * 0.5f;
 268   frby2xfrsq = (fractby2) * fractsq;
 269   frby6xfrsq = (fractby6) * fractsq;
 270   oneminusfractby2 = 1.0f - fractby2;
 271   wb = fractsqby2 - fractby3;
 272   wc = (fractsqby2 + fract);
 273   wa = wb - frby6xfrsq;
 274   wb = frby2xfrsq - fractsq;
 275   cosVal = wa * a;
 276   wc = wc - frby2xfrsq;
 277   wd = (frby6xfrsq) - fractby6;
 278   wb = wb + oneminusfractby2;
 279
 280   /* Calculate cos value */
 281   cosVal = (cosVal + (b * wb)) + ((c * wc) + (d * wd));
 282
 283   /* Return the output value */
 284   return (cosVal);
 285
 286 }
 287
 288 /**
 289  * @} end of cos group
 290  */