fixed_func.cpp

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 #include "fixed_func.h"
00037 
00038 namespace fixedpoint {
00039 
00040 static const int32_t FIX16_2PI    = float2fix<16>(6.28318530717958647692f);
00041 static const int32_t FIX16_R2PI = float2fix<16>(1.0f/6.28318530717958647692f);
00042 
00043 static const uint16_t sin_tab[] = {
00044 #include "fixsintab.h"
00045 };
00046 
00047 int32_t fixcos16(int32_t a) 
00048 {
00049     int32_t v;
00050     /* reduce to [0,1) */
00051     while (a < 0) a += FIX16_2PI;
00052     a = fixmul<16>(a, FIX16_R2PI);
00053     a += 0x4000;
00054 
00055     /* now in the range [0, 0xffff], reduce to [0, 0xfff] */
00056     a >>= 4;
00057 
00058     v = (a & 0x400) ? sin_tab[0x3ff - (a & 0x3ff)] : sin_tab[a & 0x3ff];
00059     v = fixmul<16>(v, 1 << 16);
00060     return (a & 0x800) ? -v : v;
00061 }
00062 
00063 int32_t fixsin16(int32_t a)
00064 {
00065     int32_t v;
00066 
00067     /* reduce to [0,1) */
00068     while (a < 0) a += FIX16_2PI;
00069     a = fixmul<16>(a, FIX16_R2PI);
00070 
00071     /* now in the range [0, 0xffff], reduce to [0, 0xfff] */
00072     a >>= 4;
00073 
00074     v = (a & 0x400) ? sin_tab[0x3ff - (a & 0x3ff)] : sin_tab[a & 0x3ff];
00075     v = fixmul<16>(v, 1 << 16);
00076     return (a & 0x800) ? -v : v;
00077 }
00078 
00079 int32_t fixrsqrt16(int32_t a)
00080 {
00081     int32_t x;
00082 
00083     static const uint16_t rsq_tab[] = { /* domain 0.5 .. 1.0-1/16 */
00084         0xb504, 0xaaaa, 0xa1e8, 0x9a5f, 0x93cd, 0x8e00, 0x88d6, 0x8432,
00085     };
00086 
00087     int32_t i, exp;
00088     if (a == 0) return 0x7fffffff;
00089     if (a == (1<<16)) return a;
00090 
00091     exp = detail::CountLeadingZeros(a);
00092     x = rsq_tab[(a>>(28-exp))&0x7]<<1;
00093 
00094     exp -= 16;
00095     if (exp <= 0)
00096         x >>= -exp>>1;
00097     else
00098         x <<= (exp>>1)+(exp&1);
00099     if (exp&1) x = fixmul<16>(x, rsq_tab[0]);
00100 
00101 
00102     /* newton-raphson */
00103     /* x = x/2*(3-(a*x)*x) */
00104     i = 0;
00105     do {
00106 
00107         x = fixmul<16>((x>>1),((1<<16)*3 - fixmul<16>(fixmul<16>(a,x),x)));
00108     } while(++i < 3);
00109 
00110     return x;
00111 }
00112 
00113 static inline int32_t fast_div16(int32_t a, int32_t b)
00114 {
00115     if ((b >> 24) && (b >> 24) + 1) {
00116         return fixmul<16>(a >> 8, fixinv<16>(b >> 8));
00117     } else {
00118         return fixmul<16>(a, fixinv<16>(b));
00119     }
00120 }
00121 
00122 int32_t fixsqrt16(int32_t a) 
00123 {
00124     int32_t s;
00125     int32_t i;
00126     s = (a + (1<<16)) >> 1;
00127     /* 6 iterations to converge */
00128     for (i = 0; i < 6; i++)
00129         s = (s + fast_div16(a, s)) >> 1;
00130     return s;
00131 }
00132 
00133 } // end namespace fixedpoint