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.h@1:ac99b6f474a0, 2011-11-27 (annotated)

Committer:
madcowswe
Date:
Sun Nov 27 05:10:45 2011 +0000
Revision:
1:ac99b6f474a0
Parent:
0:ca79fb4a8165 
Made it possible to cast to double

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 #ifndef FIXEDP_FUNC_H_INCLUDED
madcowswe 0:ca79fb4a8165 37 #define FIXEDP_FUNC_H_INCLUDED
madcowswe 0:ca79fb4a8165 38
madcowswe 0:ca79fb4a8165 39 #ifdef _MSC_VER
madcowswe 0:ca79fb4a8165 40 #pragma once
madcowswe 0:ca79fb4a8165 41 #endif
madcowswe 0:ca79fb4a8165 42
madcowswe 0:ca79fb4a8165 43 #ifndef _MSC_VER
madcowswe 0:ca79fb4a8165 44 #include <stdint.h>
madcowswe 0:ca79fb4a8165 45 #else
madcowswe 0:ca79fb4a8165 46 #include "stdint.h"
madcowswe 0:ca79fb4a8165 47 #endif
madcowswe 0:ca79fb4a8165 48
madcowswe 0:ca79fb4a8165 49 namespace fixedpoint {
madcowswe 0:ca79fb4a8165 50
madcowswe 0:ca79fb4a8165 51 // The template argument p in all of the following functions refers to the
madcowswe 0:ca79fb4a8165 52 // fixed point precision (e.g. p = 8 gives 24.8 fixed point functions).
madcowswe 0:ca79fb4a8165 53
madcowswe 0:ca79fb4a8165 54 // Perform a fixed point multiplication without a 64-bit intermediate result.
madcowswe 0:ca79fb4a8165 55 // This is fast but beware of overflow!
madcowswe 0:ca79fb4a8165 56 template <int p>
madcowswe 0:ca79fb4a8165 57 inline int32_t fixmulf(int32_t a, int32_t b)
madcowswe 0:ca79fb4a8165 58 {
madcowswe 0:ca79fb4a8165 59 return (a * b) >> p;
madcowswe 0:ca79fb4a8165 60 }
madcowswe 0:ca79fb4a8165 61
madcowswe 0:ca79fb4a8165 62 // Perform a fixed point multiplication using a 64-bit intermediate result to
madcowswe 0:ca79fb4a8165 63 // prevent overflow problems.
madcowswe 0:ca79fb4a8165 64 template <int p>
madcowswe 0:ca79fb4a8165 65 inline int32_t fixmul(int32_t a, int32_t b)
madcowswe 0:ca79fb4a8165 66 {
madcowswe 0:ca79fb4a8165 67 return (int32_t)(((int64_t)a * b) >> p);
madcowswe 0:ca79fb4a8165 68 }
madcowswe 0:ca79fb4a8165 69
madcowswe 0:ca79fb4a8165 70 // Fixed point division
madcowswe 0:ca79fb4a8165 71 template <int p>
madcowswe 0:ca79fb4a8165 72 inline int fixdiv(int32_t a, int32_t b)
madcowswe 0:ca79fb4a8165 73 {
madcowswe 0:ca79fb4a8165 74 #if 0
madcowswe 0:ca79fb4a8165 75 return (int32_t)((((int64_t)a) << p) / b);
madcowswe 0:ca79fb4a8165 76 #else
madcowswe 0:ca79fb4a8165 77 // The following produces the same results as the above but gcc 4.0.3
madcowswe 0:ca79fb4a8165 78 // generates fewer instructions (at least on the ARM processor).
madcowswe 0:ca79fb4a8165 79 union {
madcowswe 0:ca79fb4a8165 80 int64_t a;
madcowswe 0:ca79fb4a8165 81 struct {
madcowswe 0:ca79fb4a8165 82 int32_t l;
madcowswe 0:ca79fb4a8165 83 int32_t h;
madcowswe 0:ca79fb4a8165 84 };
madcowswe 0:ca79fb4a8165 85 } x;
madcowswe 0:ca79fb4a8165 86
madcowswe 0:ca79fb4a8165 87 x.l = a << p;
madcowswe 0:ca79fb4a8165 88 x.h = a >> (sizeof(int32_t) * 8 - p);
madcowswe 0:ca79fb4a8165 89 return (int32_t)(x.a / b);
madcowswe 0:ca79fb4a8165 90 #endif
madcowswe 0:ca79fb4a8165 91 }
madcowswe 0:ca79fb4a8165 92
madcowswe 0:ca79fb4a8165 93 namespace detail {
madcowswe 0:ca79fb4a8165 94 inline uint32_t CountLeadingZeros(uint32_t x)
madcowswe 0:ca79fb4a8165 95 {
madcowswe 0:ca79fb4a8165 96 uint32_t exp = 31;
madcowswe 0:ca79fb4a8165 97
madcowswe 0:ca79fb4a8165 98 if (x & 0xffff0000) {
madcowswe 0:ca79fb4a8165 99 exp -= 16;
madcowswe 0:ca79fb4a8165 100 x >>= 16;
madcowswe 0:ca79fb4a8165 101 }
madcowswe 0:ca79fb4a8165 102
madcowswe 0:ca79fb4a8165 103 if (x & 0xff00) {
madcowswe 0:ca79fb4a8165 104 exp -= 8;
madcowswe 0:ca79fb4a8165 105 x >>= 8;
madcowswe 0:ca79fb4a8165 106 }
madcowswe 0:ca79fb4a8165 107
madcowswe 0:ca79fb4a8165 108 if (x & 0xf0) {
madcowswe 0:ca79fb4a8165 109 exp -= 4;
madcowswe 0:ca79fb4a8165 110 x >>= 4;
madcowswe 0:ca79fb4a8165 111 }
madcowswe 0:ca79fb4a8165 112
madcowswe 0:ca79fb4a8165 113 if (x & 0xc) {
madcowswe 0:ca79fb4a8165 114 exp -= 2;
madcowswe 0:ca79fb4a8165 115 x >>= 2;
madcowswe 0:ca79fb4a8165 116 }
madcowswe 0:ca79fb4a8165 117
madcowswe 0:ca79fb4a8165 118 if (x & 0x2) {
madcowswe 0:ca79fb4a8165 119 exp -= 1;
madcowswe 0:ca79fb4a8165 120 }
madcowswe 0:ca79fb4a8165 121
madcowswe 0:ca79fb4a8165 122 return exp;
madcowswe 0:ca79fb4a8165 123 }
madcowswe 0:ca79fb4a8165 124 }
madcowswe 0:ca79fb4a8165 125
madcowswe 0:ca79fb4a8165 126 // q is the precision of the input
madcowswe 0:ca79fb4a8165 127 // output has 32-q bits of fraction
madcowswe 0:ca79fb4a8165 128 template <int q>
madcowswe 0:ca79fb4a8165 129 inline int fixinv(int32_t a)
madcowswe 0:ca79fb4a8165 130 {
madcowswe 0:ca79fb4a8165 131 int32_t x;
madcowswe 0:ca79fb4a8165 132
madcowswe 0:ca79fb4a8165 133 bool sign = false;
madcowswe 0:ca79fb4a8165 134
madcowswe 0:ca79fb4a8165 135 if (a < 0) {
madcowswe 0:ca79fb4a8165 136 sign = true;
madcowswe 0:ca79fb4a8165 137 a = -a;
madcowswe 0:ca79fb4a8165 138 }
madcowswe 0:ca79fb4a8165 139
madcowswe 0:ca79fb4a8165 140 static const uint16_t rcp_tab[] = {
madcowswe 0:ca79fb4a8165 141 0x8000, 0x71c7, 0x6666, 0x5d17, 0x5555, 0x4ec4, 0x4924, 0x4444
madcowswe 0:ca79fb4a8165 142 };
madcowswe 0:ca79fb4a8165 143
madcowswe 0:ca79fb4a8165 144 int32_t exp = detail::CountLeadingZeros(a);
madcowswe 0:ca79fb4a8165 145 x = ((int32_t)rcp_tab[(a>>(28-exp))&0x7]) << 2;
madcowswe 0:ca79fb4a8165 146 exp -= 16;
madcowswe 0:ca79fb4a8165 147
madcowswe 0:ca79fb4a8165 148 if (exp <= 0)
madcowswe 0:ca79fb4a8165 149 x >>= -exp;
madcowswe 0:ca79fb4a8165 150 else
madcowswe 0:ca79fb4a8165 151 x <<= exp;
madcowswe 0:ca79fb4a8165 152
madcowswe 0:ca79fb4a8165 153 /* two iterations of newton-raphson x = x(2-ax) */
madcowswe 0:ca79fb4a8165 154 x = fixmul<(32-q)>(x,((2<<(32-q)) - fixmul<q>(a,x)));
madcowswe 0:ca79fb4a8165 155 x = fixmul<(32-q)>(x,((2<<(32-q)) - fixmul<q>(a,x)));
madcowswe 0:ca79fb4a8165 156
madcowswe 0:ca79fb4a8165 157 if (sign)
madcowswe 0:ca79fb4a8165 158 return -x;
madcowswe 0:ca79fb4a8165 159 else
madcowswe 0:ca79fb4a8165 160 return x;
madcowswe 0:ca79fb4a8165 161 }
madcowswe 0:ca79fb4a8165 162
madcowswe 0:ca79fb4a8165 163 // Conversion from and to float
madcowswe 0:ca79fb4a8165 164
madcowswe 0:ca79fb4a8165 165 template <int p>
madcowswe 0:ca79fb4a8165 166 float fix2float(int32_t f)
madcowswe 0:ca79fb4a8165 167 {
madcowswe 0:ca79fb4a8165 168 return (float)f / (1 << p);
madcowswe 0:ca79fb4a8165 169 }
madcowswe 0:ca79fb4a8165 170
madcowswe 0:ca79fb4a8165 171 template <int p>
madcowswe 0:ca79fb4a8165 172 int32_t float2fix(float f)
madcowswe 0:ca79fb4a8165 173 {
madcowswe 0:ca79fb4a8165 174 return (int32_t)(f * (1 << p));
madcowswe 0:ca79fb4a8165 175 }
madcowswe 0:ca79fb4a8165 176
madcowswe 0:ca79fb4a8165 177 int32_t fixcos16(int32_t a);
madcowswe 0:ca79fb4a8165 178 int32_t fixsin16(int32_t a);
madcowswe 0:ca79fb4a8165 179 int32_t fixrsqrt16(int32_t a);
madcowswe 0:ca79fb4a8165 180 int32_t fixsqrt16(int32_t a);
madcowswe 0:ca79fb4a8165 181
madcowswe 0:ca79fb4a8165 182 } // end namespace fixedpoint
madcowswe 0:ca79fb4a8165 183
madcowswe 0:ca79fb4a8165 184 #endif