Oskar Weigl / fixedpoint

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@0:ca79fb4a8165, 2011-11-27 (annotated)

Committer:
madcowswe
Date:
Sun Nov 27 00:28:31 2011 +0000
Revision:
0:ca79fb4a8165
added the functionality to cast between precisions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 0:ca79fb4a8165 1 /*
madcowswe 0:ca79fb4a8165 2 Copyright (c) 2007, Markus Trenkwalder
madcowswe 0:ca79fb4a8165 3
madcowswe 0:ca79fb4a8165 4 Portions taken from the Vicent 3D rendering library
madcowswe 0:ca79fb4a8165 5 Copyright (c) 2004, David Blythe, Hans Martin Will
madcowswe 0:ca79fb4a8165 6
madcowswe 0:ca79fb4a8165 7 All rights reserved.
madcowswe 0:ca79fb4a8165 8
madcowswe 0:ca79fb4a8165 9 Redistribution and use in source and binary forms, with or without
madcowswe 0:ca79fb4a8165 10 modification, are permitted provided that the following conditions are met:
madcowswe 0:ca79fb4a8165 11
madcowswe 0:ca79fb4a8165 12 * Redistributions of source code must retain the above copyright notice,
madcowswe 0:ca79fb4a8165 13 this list of conditions and the following disclaimer.
madcowswe 0:ca79fb4a8165 14
madcowswe 0:ca79fb4a8165 15 * Redistributions in binary form must reproduce the above copyright notice,
madcowswe 0:ca79fb4a8165 16 this list of conditions and the following disclaimer in the documentation
madcowswe 0:ca79fb4a8165 17 and/or other materials provided with the distribution.
madcowswe 0:ca79fb4a8165 18
madcowswe 0:ca79fb4a8165 19 * Neither the name of the library's copyright owner nor the names of its
madcowswe 0:ca79fb4a8165 20 contributors may be used to endorse or promote products derived from this
madcowswe 0:ca79fb4a8165 21 software without specific prior written permission.
madcowswe 0:ca79fb4a8165 22
madcowswe 0:ca79fb4a8165 23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
madcowswe 0:ca79fb4a8165 24 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
madcowswe 0:ca79fb4a8165 25 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
madcowswe 0:ca79fb4a8165 26 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
madcowswe 0:ca79fb4a8165 27 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
madcowswe 0:ca79fb4a8165 28 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
madcowswe 0:ca79fb4a8165 29 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
madcowswe 0:ca79fb4a8165 30 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
madcowswe 0:ca79fb4a8165 31 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
madcowswe 0:ca79fb4a8165 32 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
madcowswe 0:ca79fb4a8165 33 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
madcowswe 0:ca79fb4a8165 34 */
madcowswe 0:ca79fb4a8165 35
madcowswe 0:ca79fb4a8165 36 #include "fixed_func.h"
madcowswe 0:ca79fb4a8165 37
madcowswe 0:ca79fb4a8165 38 namespace fixedpoint {
madcowswe 0:ca79fb4a8165 39
madcowswe 0:ca79fb4a8165 40 static const int32_t FIX16_2PI = float2fix<16>(6.28318530717958647692f);
madcowswe 0:ca79fb4a8165 41 static const int32_t FIX16_R2PI = float2fix<16>(1.0f/6.28318530717958647692f);
madcowswe 0:ca79fb4a8165 42
madcowswe 0:ca79fb4a8165 43 static const uint16_t sin_tab[] = {
madcowswe 0:ca79fb4a8165 44 #include "fixsintab.h"
madcowswe 0:ca79fb4a8165 45 };
madcowswe 0:ca79fb4a8165 46
madcowswe 0:ca79fb4a8165 47 int32_t fixcos16(int32_t a)
madcowswe 0:ca79fb4a8165 48 {
madcowswe 0:ca79fb4a8165 49 int32_t v;
madcowswe 0:ca79fb4a8165 50 /* reduce to [0,1) */
madcowswe 0:ca79fb4a8165 51 while (a < 0) a += FIX16_2PI;
madcowswe 0:ca79fb4a8165 52 a = fixmul<16>(a, FIX16_R2PI);
madcowswe 0:ca79fb4a8165 53 a += 0x4000;
madcowswe 0:ca79fb4a8165 54
madcowswe 0:ca79fb4a8165 55 /* now in the range [0, 0xffff], reduce to [0, 0xfff] */
madcowswe 0:ca79fb4a8165 56 a >>= 4;
madcowswe 0:ca79fb4a8165 57
madcowswe 0:ca79fb4a8165 58 v = (a & 0x400) ? sin_tab[0x3ff - (a & 0x3ff)] : sin_tab[a & 0x3ff];
madcowswe 0:ca79fb4a8165 59 v = fixmul<16>(v, 1 << 16);
madcowswe 0:ca79fb4a8165 60 return (a & 0x800) ? -v : v;
madcowswe 0:ca79fb4a8165 61 }
madcowswe 0:ca79fb4a8165 62
madcowswe 0:ca79fb4a8165 63 int32_t fixsin16(int32_t a)
madcowswe 0:ca79fb4a8165 64 {
madcowswe 0:ca79fb4a8165 65 int32_t v;
madcowswe 0:ca79fb4a8165 66
madcowswe 0:ca79fb4a8165 67 /* reduce to [0,1) */
madcowswe 0:ca79fb4a8165 68 while (a < 0) a += FIX16_2PI;
madcowswe 0:ca79fb4a8165 69 a = fixmul<16>(a, FIX16_R2PI);
madcowswe 0:ca79fb4a8165 70
madcowswe 0:ca79fb4a8165 71 /* now in the range [0, 0xffff], reduce to [0, 0xfff] */
madcowswe 0:ca79fb4a8165 72 a >>= 4;
madcowswe 0:ca79fb4a8165 73
madcowswe 0:ca79fb4a8165 74 v = (a & 0x400) ? sin_tab[0x3ff - (a & 0x3ff)] : sin_tab[a & 0x3ff];
madcowswe 0:ca79fb4a8165 75 v = fixmul<16>(v, 1 << 16);
madcowswe 0:ca79fb4a8165 76 return (a & 0x800) ? -v : v;
madcowswe 0:ca79fb4a8165 77 }
madcowswe 0:ca79fb4a8165 78
madcowswe 0:ca79fb4a8165 79 int32_t fixrsqrt16(int32_t a)
madcowswe 0:ca79fb4a8165 80 {
madcowswe 0:ca79fb4a8165 81 int32_t x;
madcowswe 0:ca79fb4a8165 82
madcowswe 0:ca79fb4a8165 83 static const uint16_t rsq_tab[] = { /* domain 0.5 .. 1.0-1/16 */
madcowswe 0:ca79fb4a8165 84 0xb504, 0xaaaa, 0xa1e8, 0x9a5f, 0x93cd, 0x8e00, 0x88d6, 0x8432,
madcowswe 0:ca79fb4a8165 85 };
madcowswe 0:ca79fb4a8165 86
madcowswe 0:ca79fb4a8165 87 int32_t i, exp;
madcowswe 0:ca79fb4a8165 88 if (a == 0) return 0x7fffffff;
madcowswe 0:ca79fb4a8165 89 if (a == (1<<16)) return a;
madcowswe 0:ca79fb4a8165 90
madcowswe 0:ca79fb4a8165 91 exp = detail::CountLeadingZeros(a);
madcowswe 0:ca79fb4a8165 92 x = rsq_tab[(a>>(28-exp))&0x7]<<1;
madcowswe 0:ca79fb4a8165 93
madcowswe 0:ca79fb4a8165 94 exp -= 16;
madcowswe 0:ca79fb4a8165 95 if (exp <= 0)
madcowswe 0:ca79fb4a8165 96 x >>= -exp>>1;
madcowswe 0:ca79fb4a8165 97 else
madcowswe 0:ca79fb4a8165 98 x <<= (exp>>1)+(exp&1);
madcowswe 0:ca79fb4a8165 99 if (exp&1) x = fixmul<16>(x, rsq_tab[0]);
madcowswe 0:ca79fb4a8165 100
madcowswe 0:ca79fb4a8165 101
madcowswe 0:ca79fb4a8165 102 /* newton-raphson */
madcowswe 0:ca79fb4a8165 103 /* x = x/2*(3-(a*x)*x) */
madcowswe 0:ca79fb4a8165 104 i = 0;
madcowswe 0:ca79fb4a8165 105 do {
madcowswe 0:ca79fb4a8165 106
madcowswe 0:ca79fb4a8165 107 x = fixmul<16>((x>>1),((1<<16)*3 - fixmul<16>(fixmul<16>(a,x),x)));
madcowswe 0:ca79fb4a8165 108 } while(++i < 3);
madcowswe 0:ca79fb4a8165 109
madcowswe 0:ca79fb4a8165 110 return x;
madcowswe 0:ca79fb4a8165 111 }
madcowswe 0:ca79fb4a8165 112
madcowswe 0:ca79fb4a8165 113 static inline int32_t fast_div16(int32_t a, int32_t b)
madcowswe 0:ca79fb4a8165 114 {
madcowswe 0:ca79fb4a8165 115 if ((b >> 24) && (b >> 24) + 1) {
madcowswe 0:ca79fb4a8165 116 return fixmul<16>(a >> 8, fixinv<16>(b >> 8));
madcowswe 0:ca79fb4a8165 117 } else {
madcowswe 0:ca79fb4a8165 118 return fixmul<16>(a, fixinv<16>(b));
madcowswe 0:ca79fb4a8165 119 }
madcowswe 0:ca79fb4a8165 120 }
madcowswe 0:ca79fb4a8165 121
madcowswe 0:ca79fb4a8165 122 int32_t fixsqrt16(int32_t a)
madcowswe 0:ca79fb4a8165 123 {
madcowswe 0:ca79fb4a8165 124 int32_t s;
madcowswe 0:ca79fb4a8165 125 int32_t i;
madcowswe 0:ca79fb4a8165 126 s = (a + (1<<16)) >> 1;
madcowswe 0:ca79fb4a8165 127 /* 6 iterations to converge */
madcowswe 0:ca79fb4a8165 128 for (i = 0; i < 6; i++)
madcowswe 0:ca79fb4a8165 129 s = (s + fast_div16(a, s)) >> 1;
madcowswe 0:ca79fb4a8165 130 return s;
madcowswe 0:ca79fb4a8165 131 }
madcowswe 0:ca79fb4a8165 132
madcowswe 0:ca79fb4a8165 133 } // end namespace fixedpoint