arm_sin_cos_f32.c

00001 /* ----------------------------------------------------------------------    
00002 * Copyright (C) 2010-2014 ARM Limited. All rights reserved.    
00003 *    
00004 * $Date:        19. March 2015
00005 * $Revision:    V.1.4.5
00006 *    
00007 * Project:      CMSIS DSP Library    
00008 * Title:        arm_sin_cos_f32.c    
00009 *    
00010 * Description:  Sine and 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 #include "arm_common_tables.h"
00043 
00044 /**    
00045  * @ingroup groupController    
00046  */
00047 
00048 /**    
00049  * @defgroup SinCos Sine Cosine   
00050  *    
00051  * Computes the trigonometric sine and cosine values using a combination of table lookup   
00052  * and linear interpolation.     
00053  * There are separate functions for Q31 and floating-point data types.   
00054  * The input to the floating-point version is in degrees while the   
00055  * fixed-point Q31 have a scaled input with the range   
00056  * [-1 0.9999] mapping to [-180 +180] degrees.   
00057  *
00058  * The floating point function also allows values that are out of the usual range. When this happens, the function will
00059  * take extra time to adjust the input value to the range of [-180 180].
00060  *   
00061  * The implementation is based on table lookup using 360 values together with linear interpolation.   
00062  * The steps used are:   
00063  *  -# Calculation of the nearest integer table index.   
00064  *  -# Compute the fractional portion (fract) of the input.   
00065  *  -# Fetch the value corresponding to \c index from sine table to \c y0 and also value from \c index+1 to \c y1.      
00066  *  -# Sine value is computed as <code> *psinVal = y0 + (fract * (y1 - y0))</code>.    
00067  *  -# Fetch the value corresponding to \c index from cosine table to \c y0 and also value from \c index+1 to \c y1.      
00068  *  -# Cosine value is computed as <code> *pcosVal = y0 + (fract * (y1 - y0))</code>.    
00069  */
00070 
00071  /**    
00072  * @addtogroup SinCos    
00073  * @{    
00074  */
00075 
00076 /**    
00077  * @brief  Floating-point sin_cos function.   
00078  * @param[in]  theta    input value in degrees    
00079  * @param[out] *pSinVal points to the processed sine output.    
00080  * @param[out] *pCosVal points to the processed cos output.    
00081  * @return none.   
00082  */
00083 
00084 void arm_sin_cos_f32(
00085   float32_t theta,
00086   float32_t * pSinVal,
00087   float32_t * pCosVal)
00088 {
00089   float32_t fract, in;                             /* Temporary variables for input, output */
00090   uint16_t indexS, indexC;                         /* Index variable */
00091   float32_t f1, f2, d1, d2;                        /* Two nearest output values */
00092   int32_t n;
00093   float32_t findex, Dn, Df, temp;
00094 
00095   /* input x is in degrees */
00096   /* Scale the input, divide input by 360, for cosine add 0.25 (pi/2) to read sine table */
00097   in = theta * 0.00277777777778f;
00098 
00099   /* Calculation of floor value of input */
00100   n = (int32_t) in;
00101 
00102   /* Make negative values towards -infinity */
00103   if(in < 0.0f)
00104   {
00105     n--;
00106   }
00107   /* Map input value to [0 1] */
00108   in = in - (float32_t) n;
00109 
00110   /* Calculation of index of the table */
00111   findex = (float32_t) FAST_MATH_TABLE_SIZE * in;
00112   indexS = ((uint16_t)findex) & 0x1ff;
00113   indexC = (indexS + (FAST_MATH_TABLE_SIZE / 4)) & 0x1ff;
00114 
00115   /* fractional value calculation */
00116   fract = findex - (float32_t) indexS;
00117 
00118   /* Read two nearest values of input value from the cos & sin tables */
00119   f1 = sinTable_f32[indexC+0];
00120   f2 = sinTable_f32[indexC+1];
00121   d1 = -sinTable_f32[indexS+0];
00122   d2 = -sinTable_f32[indexS+1];
00123 
00124   Dn = 0.0122718463030f; // delta between the two points (fixed), in this case 2*pi/FAST_MATH_TABLE_SIZE
00125   Df = f2 - f1; // delta between the values of the functions
00126   temp = Dn*(d1 + d2) - 2*Df;
00127   temp = fract*temp + (3*Df - (d2 + 2*d1)*Dn);
00128   temp = fract*temp + d1*Dn;
00129 
00130   /* Calculation of cosine value */
00131   *pCosVal = fract*temp + f1;
00132   
00133   /* Read two nearest values of input value from the cos & sin tables */
00134   f1 = sinTable_f32[indexS+0];
00135   f2 = sinTable_f32[indexS+1];
00136   d1 = sinTable_f32[indexC+0];
00137   d2 = sinTable_f32[indexC+1];
00138 
00139   Df = f2 - f1; // delta between the values of the functions
00140   temp = Dn*(d1 + d2) - 2*Df;
00141   temp = fract*temp + (3*Df - (d2 + 2*d1)*Dn);
00142   temp = fract*temp + d1*Dn;
00143   
00144   /* Calculation of sine value */
00145   *pSinVal = fract*temp + f1;
00146 }
00147 /**    
00148  * @} end of SinCos group    
00149  */