38
+ − 1 ///////////////////////////////////////////////////////////////////////////////
+ − 2 /// -*- coding: UTF-8 -*-
+ − 3 ///
+ − 4 /// \file Discovery/Inc/Utils.h
+ − 5 /// \brief Various Utilities...
+ − 6 /// \author Heinrichs Weikamp
+ − 7 /// \date 2018
+ − 8 ///
+ − 9 /// $Id$
+ − 10 ///////////////////////////////////////////////////////////////////////////////
+ − 11 /// \par Copyright (c) 2014-2018 Heinrichs Weikamp gmbh
+ − 12 ///
+ − 13 /// This program is free software: you can redistribute it and/or modify
+ − 14 /// it under the terms of the GNU General Public License as published by
+ − 15 /// the Free Software Foundation, either version 3 of the License, or
+ − 16 /// (at your option) any later version.
+ − 17 ///
+ − 18 /// This program is distributed in the hope that it will be useful,
+ − 19 /// but WITHOUT ANY WARRANTY; without even the implied warranty of
+ − 20 /// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ − 21 /// GNU General Public License for more details.
+ − 22 ///
+ − 23 /// You should have received a copy of the GNU General Public License
+ − 24 /// along with this program. If not, see <http://www.gnu.org/licenses/>.
+ − 25 //////////////////////////////////////////////////////////////////////////////
+ − 26
+ − 27 #ifndef UTILS_H
+ − 28 #define UTILS_H
+ − 29
+ − 30 #include <stdint.h>
+ − 31 #include <stdbool.h>
+ − 32 #include <math.h>
+ − 33
+ − 34 #ifdef FloatToFixed
+ − 35 #undef FloatToFixed
+ − 36 #undef IntToFixed
+ − 37 #undef FixedToFloat
+ − 38 #undef FixedToInt
+ − 39 #endif
+ − 40
+ − 41 //
+ − 42 // 20.12 fixed-point operations
+ − 43 //
+ − 44
+ − 45 #define Fix(val) ((val)*4096)
+ − 46 #define F(val) Fix(val)
+ − 47
+ − 48 #define FixSqrt2 5793
+ − 49 #define FixInvSqrt2 2896
+ − 50 #define FixSinPiDiv4 FixInvSqrt2
+ − 51 #define FixCosPiDiv4 FixInvSqrt2
+ − 52 #define FixSinPiDiv8 1567
+ − 53 #define FixCosPiDiv8 3784
+ − 54
+ − 55 static inline int32_t FloatToFixed(float val) { return val*4096.0f; }
+ − 56 static inline int32_t IntToFixed(int val) { return val<<12; }
+ − 57 static inline int32_t IntToFixedPlusHalf(int val) { return (val<<12)+0x800; }
+ − 58
+ − 59 static inline float FixedToFloat(int32_t val) { return (float)val/4096.0f; }
+ − 60 static inline int32_t FixedToInt(int32_t val) { return val>>12; }
+ − 61 static inline int32_t FixedToRoundedInt(int32_t val) { return (val+0x800)>>12; }
+ − 62
+ − 63 //static inline int32_t FixedMul(int32_t a,int32_t b) { return ((int64_t)a*(int64_t)b)>>16; }
+ − 64
+ − 65 static inline int32_t imin(int32_t a,int32_t b) { return a<b?a:b; }
+ − 66 static inline int32_t imax(int32_t a,int32_t b) { return a>b?a:b; }
+ − 67 static inline int32_t iabs(int32_t a) { return a<0?-a:a; }
+ − 68 static inline int32_t isign(int32_t a) { return a>0?1:a<0?-1:0; }
+ − 69
+ − 70 static inline int64_t imul64(int64_t a, int64_t b) { return (a*b)>>12; }
+ − 71 static inline int32_t imul(int32_t a, int32_t b) { return (int32_t)imul64(a,b); }
+ − 72
+ − 73 static inline int64_t idiv64(int64_t num,int64_t den) { return (num<<12)/den; }
+ − 74 static inline int32_t idiv(int32_t num,int32_t den) { return (int32_t)idiv64(num,den); }
+ − 75
+ − 76 static inline int64_t isq64(int64_t val) { return imul64(val,val); }
+ − 77 static inline int32_t isq(int32_t val) { return imul(val,val); }
+ − 78
+ − 79 static uint32_t sqrti(uint32_t n)
+ − 80 {
+ − 81 uint32_t s,t;
+ − 82
+ − 83 #define sqrtBit(k) \
+ − 84 t = s+(1UL<<(k-1)); t <<= k+1; if (n >= t) { n -= t; s |= 1UL<<k; }
+ − 85
+ − 86 s=0;
+ − 87 if(n>=1<<30) { n-=1<<30; s=1<<15; }
+ − 88 sqrtBit(14); sqrtBit(13); sqrtBit(12); sqrtBit(11); sqrtBit(10);
+ − 89 sqrtBit(9); sqrtBit(8); sqrtBit(7); sqrtBit(6); sqrtBit(5);
+ − 90 sqrtBit(4); sqrtBit(3); sqrtBit(2); sqrtBit(1);
+ − 91 if(n>s<<1) s|=1;
+ − 92
+ − 93 #undef sqrtBit
+ − 94
+ − 95 return s;
+ − 96 }
+ − 97
+ − 98 static inline int64_t isqrt64(int64_t val) { return sqrti((uint64_t)val<<12); }
+ − 99 static inline int32_t isqrt(int32_t val) { return sqrti((uint64_t)val<<12); }
+ − 100
+ − 101 static inline int64_t inorm64(int64_t a,int64_t b) { return sqrt((double)a*(double)a+(double)b*(double)b); }
+ − 102 static inline int32_t inorm(int32_t a,int32_t b) { return sqrt((double)a*(double)a+(double)b*(double)b); }
+ − 103
+ − 104 static inline int32_t ifloor(int a) { return a&~0xfff; }
+ − 105 static inline int32_t ifrac(int a) { return a&0xfff; }
+ − 106
+ − 107 int32_t isin(int a);
+ − 108 static inline int32_t icos(int a) { return isin(a+1024); }
+ − 109 static inline int32_t itan(int a) { return idiv(isin(a),icos(a)); } // TODO: use a table
+ − 110 static inline int32_t idegrees(int degrees) { return degrees*4096.0f/360.0f; }
+ − 111 static inline int32_t iradians(float radians) { return radians*4096.0f/2/3.1415926535897932f; }
+ − 112
+ − 113 static inline int32_t ilerp(int32_t a,int32_t b,int32_t t)
+ − 114 {
+ − 115 return a+imul(b-a,t);
+ − 116 }
+ − 117
+ − 118 /*static inline int32_t islerp(int32_t a,int32_t b,int32_t t)
+ − 119 {
+ − 120 return a;
+ − 121 }*/
+ − 122
+ − 123 //
+ − 124 // Bitwise operations
+ − 125 //
+ − 126
+ − 127 static inline int CountBits32(uint32_t val)
+ − 128 {
+ − 129 val=(val&0x55555555)+((val&0xaaaaaaaa)>>1);
+ − 130 val=(val&0x33333333)+((val&0xcccccccc)>>2);
+ − 131 val=(val&0x0f0f0f0f)+((val&0xf0f0f0f0)>>4);
+ − 132 val=(val&0x00ff00ff)+((val&0xff00ff00)>>8);
+ − 133 val=(val&0x0000ffff)+((val&0xffff0000)>>16);
+ − 134 return val;
+ − 135 }
+ − 136
+ − 137 static inline int CountBits16(uint16_t val)
+ − 138 {
+ − 139 val=(val&0x5555)+((val&0xaaaa)>>1);
+ − 140 val=(val&0x3333)+((val&0xcccc)>>2);
+ − 141 val=(val&0x0f0f)+((val&0xf0f0)>>4);
+ − 142 val=(val&0x00ff)+((val&0xff00)>>8);
+ − 143 return val;
+ − 144 }
+ − 145
+ − 146 static inline int CountBits8(uint8_t val)
+ − 147 {
+ − 148 val=(val&0x55)+((val&0xaa)>>1);
+ − 149 val=(val&0x33)+((val&0xcc)>>2);
+ − 150 val=(val&0x0f)+((val&0xf0)>>4);
+ − 151 return val;
+ − 152 }
+ − 153
+ − 154 static inline uint32_t ReverseBits32(uint32_t val)
+ − 155 {
+ − 156 val=((val>>1)&0x55555555)|((val<<1)&0xaaaaaaaa);
+ − 157 val=((val>>2)&0x33333333)|((val<<2)&0xcccccccc);
+ − 158 val=((val>>4)&0x0f0f0f0f)|((val<<4)&0xf0f0f0f0);
+ − 159 val=((val>>8)&0x00ff00ff)|((val<<8)&0xff00ff00);
+ − 160 val=((val>>16)&0x0000ffff)|((val<<16)&0xffff0000);
+ − 161 return val;
+ − 162 }
+ − 163
+ − 164 static inline uint16_t ReverseBits16(uint16_t val)
+ − 165 {
+ − 166 val=((val>>1)&0x5555)|((val<<1)&0xaaaa);
+ − 167 val=((val>>2)&0x3333)|((val<<2)&0xcccc);
+ − 168 val=((val>>4)&0x0f0f)|((val<<4)&0xf0f0);
+ − 169 val=((val>>8)&0x00ff)|((val<<8)&0xff00);
+ − 170 return val;
+ − 171 }
+ − 172
+ − 173 static inline uint8_t ReverseBits8(uint8_t val)
+ − 174 {
+ − 175 val=((val>>1)&0x55)|((val<<1)&0xaa);
+ − 176 val=((val>>2)&0x33)|((val<<2)&0xcc);
+ − 177 val=((val>>4)&0x0f)|((val<<4)&0xf0);
+ − 178 return val;
+ − 179 }
+ − 180
+ − 181 static int LowestBitSet32(uint32_t val)
+ − 182 {
+ − 183 if(val==0) return -1;
+ − 184 return CountBits32(val^val-1)-1;
+ − 185 }
+ − 186
+ − 187 static int LowestBitSet16(uint16_t val)
+ − 188 {
+ − 189 if(val==0) return -1;
+ − 190 return CountBits16(val^val-1)-1;
+ − 191 }
+ − 192
+ − 193 static int LowestBitSet8(uint8_t val)
+ − 194 {
+ − 195 if(val==0) return -1;
+ − 196 return CountBits8(val^val-1)-1;
+ − 197 }
+ − 198
+ − 199 #endif // UTILS_H