fixed_func.h

00001 /*
00002 Copyright (c) 2007, Markus Trenkwalder
00003 
00004 Portions taken from the Vicent 3D rendering library
00005 Copyright (c) 2004, David Blythe, Hans Martin Will
00006 
00007 All rights reserved.
00008 
00009 Redistribution and use in source and binary forms, with or without 
00010 modification, are permitted provided that the following conditions are met:
00011 
00012 * Redistributions of source code must retain the above copyright notice, 
00013   this list of conditions and the following disclaimer.
00014 
00015 * Redistributions in binary form must reproduce the above copyright notice,
00016   this list of conditions and the following disclaimer in the documentation 
00017   and/or other materials provided with the distribution.
00018 
00019 * Neither the name of the library's copyright owner nor the names of its 
00020   contributors may be used to endorse or promote products derived from this 
00021   software without specific prior written permission.
00022 
00023 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
00026 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
00027 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00028 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00029 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00030 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00031 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00032 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00033 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00034 */
00035 
00036 #ifndef FIXEDP_FUNC_H_INCLUDED
00037 #define FIXEDP_FUNC_H_INCLUDED
00038 
00039 #ifdef _MSC_VER
00040 #pragma once
00041 #endif
00042 
00043 #ifndef _MSC_VER
00044 #include <stdint.h>
00045 #else
00046 #include "stdint.h"
00047 #endif
00048 
00049 namespace fixedpoint {
00050 
00051 // The template argument p in all of the following functions refers to the 
00052 // fixed point precision (e.g. p = 8 gives 24.8 fixed point functions).
00053 
00054 // Perform a fixed point multiplication without a 64-bit intermediate result.
00055 // This is fast but beware of overflow!
00056 template <int p> 
00057 inline int32_t fixmulf(int32_t a, int32_t b)
00058 {
00059     return (a * b) >> p;
00060 }
00061 
00062 // Perform a fixed point multiplication using a 64-bit intermediate result to
00063 // prevent overflow problems.
00064 template <int p>
00065 inline int32_t fixmul(int32_t a, int32_t b)
00066 {
00067     return (int32_t)(((int64_t)a * b) >> p);
00068 }
00069 
00070 // Fixed point division
00071 template <int p>
00072 inline int fixdiv(int32_t a, int32_t b)
00073 {
00074 #if 0
00075     return (int32_t)((((int64_t)a) << p) / b);
00076 #else    
00077     // The following produces the same results as the above but gcc 4.0.3 
00078     // generates fewer instructions (at least on the ARM processor).
00079     union {
00080         int64_t a;
00081         struct {
00082             int32_t l;
00083             int32_t h;
00084         };
00085     } x;
00086 
00087     x.l = a << p;
00088     x.h = a >> (sizeof(int32_t) * 8 - p);
00089     return (int32_t)(x.a / b);
00090 #endif
00091 }
00092 
00093 namespace detail {
00094     inline uint32_t CountLeadingZeros(uint32_t x)
00095     {
00096         uint32_t exp = 31;
00097     
00098         if (x & 0xffff0000) { 
00099             exp -= 16; 
00100             x >>= 16; 
00101         }
00102     
00103         if (x & 0xff00) { 
00104             exp -= 8; 
00105             x >>= 8; 
00106         }
00107         
00108         if (x & 0xf0) { 
00109             exp -= 4; 
00110             x >>= 4; 
00111         }
00112     
00113         if (x & 0xc) { 
00114             exp -= 2; 
00115             x >>= 2; 
00116         }
00117         
00118         if (x & 0x2) { 
00119             exp -= 1; 
00120         }
00121     
00122         return exp;
00123     }
00124 }
00125 
00126 // q is the precision of the input
00127 // output has 32-q bits of fraction
00128 template <int q>
00129 inline int fixinv(int32_t a)
00130 {
00131     int32_t x;
00132 
00133     bool sign = false;
00134 
00135     if (a < 0) {
00136         sign = true;
00137         a = -a;
00138     }
00139 
00140     static const uint16_t rcp_tab[] = { 
00141         0x8000, 0x71c7, 0x6666, 0x5d17, 0x5555, 0x4ec4, 0x4924, 0x4444
00142     };
00143         
00144     int32_t exp = detail::CountLeadingZeros(a);
00145     x = ((int32_t)rcp_tab[(a>>(28-exp))&0x7]) << 2;
00146     exp -= 16;
00147 
00148     if (exp <= 0)
00149         x >>= -exp;
00150     else
00151         x <<= exp;
00152 
00153     /* two iterations of newton-raphson  x = x(2-ax) */
00154     x = fixmul<(32-q)>(x,((2<<(32-q)) - fixmul<q>(a,x)));
00155     x = fixmul<(32-q)>(x,((2<<(32-q)) - fixmul<q>(a,x)));
00156 
00157     if (sign)
00158         return -x;
00159     else
00160         return x;
00161 }
00162 
00163 // Conversion from and to float
00164 
00165 template <int p>
00166 float fix2float(int32_t f)
00167 {
00168     return (float)f / (1 << p);
00169 }
00170 
00171 template <int p>
00172 int32_t float2fix(float f)
00173 {
00174     return (int32_t)(f * (1 << p));
00175 }
00176 
00177 int32_t fixcos16(int32_t a);
00178 int32_t fixsin16(int32_t a);
00179 int32_t fixrsqrt16(int32_t a);
00180 int32_t fixsqrt16(int32_t a);
00181 
00182 } // end namespace fixedpoint
00183 
00184 #endif