Steve Ravet / 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.

fixed_func.h@0:aa2871c4c041, 2010-10-20 (annotated)

Committer:
sravet
Date:
Wed Oct 20 02:20:46 2010 +0000
Revision:
0:aa2871c4c041
First checkin.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sravet 0:aa2871c4c041 1 /*
sravet 0:aa2871c4c041 2 Copyright (c) 2007, Markus Trenkwalder
sravet 0:aa2871c4c041 3
sravet 0:aa2871c4c041 4 Portions taken from the Vicent 3D rendering library
sravet 0:aa2871c4c041 5 Copyright (c) 2004, David Blythe, Hans Martin Will
sravet 0:aa2871c4c041 6
sravet 0:aa2871c4c041 7 All rights reserved.
sravet 0:aa2871c4c041 8
sravet 0:aa2871c4c041 9 Redistribution and use in source and binary forms, with or without
sravet 0:aa2871c4c041 10 modification, are permitted provided that the following conditions are met:
sravet 0:aa2871c4c041 11
sravet 0:aa2871c4c041 12 * Redistributions of source code must retain the above copyright notice,
sravet 0:aa2871c4c041 13 this list of conditions and the following disclaimer.
sravet 0:aa2871c4c041 14
sravet 0:aa2871c4c041 15 * Redistributions in binary form must reproduce the above copyright notice,
sravet 0:aa2871c4c041 16 this list of conditions and the following disclaimer in the documentation
sravet 0:aa2871c4c041 17 and/or other materials provided with the distribution.
sravet 0:aa2871c4c041 18
sravet 0:aa2871c4c041 19 * Neither the name of the library's copyright owner nor the names of its
sravet 0:aa2871c4c041 20 contributors may be used to endorse or promote products derived from this
sravet 0:aa2871c4c041 21 software without specific prior written permission.
sravet 0:aa2871c4c041 22
sravet 0:aa2871c4c041 23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
sravet 0:aa2871c4c041 24 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
sravet 0:aa2871c4c041 25 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
sravet 0:aa2871c4c041 26 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
sravet 0:aa2871c4c041 27 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
sravet 0:aa2871c4c041 28 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
sravet 0:aa2871c4c041 29 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
sravet 0:aa2871c4c041 30 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
sravet 0:aa2871c4c041 31 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
sravet 0:aa2871c4c041 32 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
sravet 0:aa2871c4c041 33 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
sravet 0:aa2871c4c041 34 */
sravet 0:aa2871c4c041 35
sravet 0:aa2871c4c041 36 #ifndef FIXEDP_FUNC_H_INCLUDED
sravet 0:aa2871c4c041 37 #define FIXEDP_FUNC_H_INCLUDED
sravet 0:aa2871c4c041 38
sravet 0:aa2871c4c041 39 #ifdef _MSC_VER
sravet 0:aa2871c4c041 40 #pragma once
sravet 0:aa2871c4c041 41 #endif
sravet 0:aa2871c4c041 42
sravet 0:aa2871c4c041 43 #ifndef _MSC_VER
sravet 0:aa2871c4c041 44 #include <stdint.h>
sravet 0:aa2871c4c041 45 #else
sravet 0:aa2871c4c041 46 #include "stdint.h"
sravet 0:aa2871c4c041 47 #endif
sravet 0:aa2871c4c041 48
sravet 0:aa2871c4c041 49 namespace fixedpoint {
sravet 0:aa2871c4c041 50
sravet 0:aa2871c4c041 51 // The template argument p in all of the following functions refers to the
sravet 0:aa2871c4c041 52 // fixed point precision (e.g. p = 8 gives 24.8 fixed point functions).
sravet 0:aa2871c4c041 53
sravet 0:aa2871c4c041 54 // Perform a fixed point multiplication without a 64-bit intermediate result.
sravet 0:aa2871c4c041 55 // This is fast but beware of overflow!
sravet 0:aa2871c4c041 56 template <int p>
sravet 0:aa2871c4c041 57 inline int32_t fixmulf(int32_t a, int32_t b)
sravet 0:aa2871c4c041 58 {
sravet 0:aa2871c4c041 59 return (a * b) >> p;
sravet 0:aa2871c4c041 60 }
sravet 0:aa2871c4c041 61
sravet 0:aa2871c4c041 62 // Perform a fixed point multiplication using a 64-bit intermediate result to
sravet 0:aa2871c4c041 63 // prevent overflow problems.
sravet 0:aa2871c4c041 64 template <int p>
sravet 0:aa2871c4c041 65 inline int32_t fixmul(int32_t a, int32_t b)
sravet 0:aa2871c4c041 66 {
sravet 0:aa2871c4c041 67 return (int32_t)(((int64_t)a * b) >> p);
sravet 0:aa2871c4c041 68 }
sravet 0:aa2871c4c041 69
sravet 0:aa2871c4c041 70 // Fixed point division
sravet 0:aa2871c4c041 71 template <int p>
sravet 0:aa2871c4c041 72 inline int fixdiv(int32_t a, int32_t b)
sravet 0:aa2871c4c041 73 {
sravet 0:aa2871c4c041 74 #if 0
sravet 0:aa2871c4c041 75 return (int32_t)((((int64_t)a) << p) / b);
sravet 0:aa2871c4c041 76 #else
sravet 0:aa2871c4c041 77 // The following produces the same results as the above but gcc 4.0.3
sravet 0:aa2871c4c041 78 // generates fewer instructions (at least on the ARM processor).
sravet 0:aa2871c4c041 79 union {
sravet 0:aa2871c4c041 80 int64_t a;
sravet 0:aa2871c4c041 81 struct {
sravet 0:aa2871c4c041 82 int32_t l;
sravet 0:aa2871c4c041 83 int32_t h;
sravet 0:aa2871c4c041 84 };
sravet 0:aa2871c4c041 85 } x;
sravet 0:aa2871c4c041 86
sravet 0:aa2871c4c041 87 x.l = a << p;
sravet 0:aa2871c4c041 88 x.h = a >> (sizeof(int32_t) * 8 - p);
sravet 0:aa2871c4c041 89 return (int32_t)(x.a / b);
sravet 0:aa2871c4c041 90 #endif
sravet 0:aa2871c4c041 91 }
sravet 0:aa2871c4c041 92
sravet 0:aa2871c4c041 93 namespace detail {
sravet 0:aa2871c4c041 94 inline uint32_t CountLeadingZeros(uint32_t x)
sravet 0:aa2871c4c041 95 {
sravet 0:aa2871c4c041 96 uint32_t exp = 31;
sravet 0:aa2871c4c041 97
sravet 0:aa2871c4c041 98 if (x & 0xffff0000) {
sravet 0:aa2871c4c041 99 exp -= 16;
sravet 0:aa2871c4c041 100 x >>= 16;
sravet 0:aa2871c4c041 101 }
sravet 0:aa2871c4c041 102
sravet 0:aa2871c4c041 103 if (x & 0xff00) {
sravet 0:aa2871c4c041 104 exp -= 8;
sravet 0:aa2871c4c041 105 x >>= 8;
sravet 0:aa2871c4c041 106 }
sravet 0:aa2871c4c041 107
sravet 0:aa2871c4c041 108 if (x & 0xf0) {
sravet 0:aa2871c4c041 109 exp -= 4;
sravet 0:aa2871c4c041 110 x >>= 4;
sravet 0:aa2871c4c041 111 }
sravet 0:aa2871c4c041 112
sravet 0:aa2871c4c041 113 if (x & 0xc) {
sravet 0:aa2871c4c041 114 exp -= 2;
sravet 0:aa2871c4c041 115 x >>= 2;
sravet 0:aa2871c4c041 116 }
sravet 0:aa2871c4c041 117
sravet 0:aa2871c4c041 118 if (x & 0x2) {
sravet 0:aa2871c4c041 119 exp -= 1;
sravet 0:aa2871c4c041 120 }
sravet 0:aa2871c4c041 121
sravet 0:aa2871c4c041 122 return exp;
sravet 0:aa2871c4c041 123 }
sravet 0:aa2871c4c041 124 }
sravet 0:aa2871c4c041 125
sravet 0:aa2871c4c041 126 // q is the precision of the input
sravet 0:aa2871c4c041 127 // output has 32-q bits of fraction
sravet 0:aa2871c4c041 128 template <int q>
sravet 0:aa2871c4c041 129 inline int fixinv(int32_t a)
sravet 0:aa2871c4c041 130 {
sravet 0:aa2871c4c041 131 int32_t x;
sravet 0:aa2871c4c041 132
sravet 0:aa2871c4c041 133 bool sign = false;
sravet 0:aa2871c4c041 134
sravet 0:aa2871c4c041 135 if (a < 0) {
sravet 0:aa2871c4c041 136 sign = true;
sravet 0:aa2871c4c041 137 a = -a;
sravet 0:aa2871c4c041 138 }
sravet 0:aa2871c4c041 139
sravet 0:aa2871c4c041 140 static const uint16_t rcp_tab[] = {
sravet 0:aa2871c4c041 141 0x8000, 0x71c7, 0x6666, 0x5d17, 0x5555, 0x4ec4, 0x4924, 0x4444
sravet 0:aa2871c4c041 142 };
sravet 0:aa2871c4c041 143
sravet 0:aa2871c4c041 144 int32_t exp = detail::CountLeadingZeros(a);
sravet 0:aa2871c4c041 145 x = ((int32_t)rcp_tab[(a>>(28-exp))&0x7]) << 2;
sravet 0:aa2871c4c041 146 exp -= 16;
sravet 0:aa2871c4c041 147
sravet 0:aa2871c4c041 148 if (exp <= 0)
sravet 0:aa2871c4c041 149 x >>= -exp;
sravet 0:aa2871c4c041 150 else
sravet 0:aa2871c4c041 151 x <<= exp;
sravet 0:aa2871c4c041 152
sravet 0:aa2871c4c041 153 /* two iterations of newton-raphson x = x(2-ax) */
sravet 0:aa2871c4c041 154 x = fixmul<(32-q)>(x,((2<<(32-q)) - fixmul<q>(a,x)));
sravet 0:aa2871c4c041 155 x = fixmul<(32-q)>(x,((2<<(32-q)) - fixmul<q>(a,x)));
sravet 0:aa2871c4c041 156
sravet 0:aa2871c4c041 157 if (sign)
sravet 0:aa2871c4c041 158 return -x;
sravet 0:aa2871c4c041 159 else
sravet 0:aa2871c4c041 160 return x;
sravet 0:aa2871c4c041 161 }
sravet 0:aa2871c4c041 162
sravet 0:aa2871c4c041 163 // Conversion from and to float
sravet 0:aa2871c4c041 164
sravet 0:aa2871c4c041 165 template <int p>
sravet 0:aa2871c4c041 166 float fix2float(int32_t f)
sravet 0:aa2871c4c041 167 {
sravet 0:aa2871c4c041 168 return (float)f / (1 << p);
sravet 0:aa2871c4c041 169 }
sravet 0:aa2871c4c041 170
sravet 0:aa2871c4c041 171 template <int p>
sravet 0:aa2871c4c041 172 int32_t float2fix(float f)
sravet 0:aa2871c4c041 173 {
sravet 0:aa2871c4c041 174 return (int32_t)(f * (1 << p));
sravet 0:aa2871c4c041 175 }
sravet 0:aa2871c4c041 176
sravet 0:aa2871c4c041 177 int32_t fixcos16(int32_t a);
sravet 0:aa2871c4c041 178 int32_t fixsin16(int32_t a);
sravet 0:aa2871c4c041 179 int32_t fixrsqrt16(int32_t a);
sravet 0:aa2871c4c041 180 int32_t fixsqrt16(int32_t a);
sravet 0:aa2871c4c041 181
sravet 0:aa2871c4c041 182 } // end namespace fixedpoint
sravet 0:aa2871c4c041 183
sravet 0:aa2871c4c041 184 #endif