A fixed point library from Trenki http://www.trenki.net/content/view/17/1/ Slightly modified so that the fixed point data structure can automatically cast itself to float or int when used in an expression, and added the functionality to cast between precisions.
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
Generated on Sat Jul 23 2022 12:09:06 by 1.7.2