1 /*
2 ===============================================================================
3
4 FILE: util.hpp
5
6 CONTENTS:
7 Utility classes
8
9 PROGRAMMERS:
10
11 martin.isenburg@rapidlasso.com - http://rapidlasso.com
12 uday.karan@gmail.com - Hobu, Inc.
13
14 COPYRIGHT:
15
16 (c) 2007-2014, martin isenburg, rapidlasso - tools to catch reality
17 (c) 2014, Uday Verma, Hobu, Inc.
18
19 This is free software; you can redistribute and/or modify it under the
20 terms of the GNU Lesser General Licence as published by the Free Software
21 Foundation. See the COPYING file for more information.
22
23 This software is distributed WITHOUT ANY WARRANTY and without even the
24 implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
25
26 CHANGE HISTORY:
27
28 ===============================================================================
29 */
30
31 #ifndef __util_hpp__
32 #define __util_hpp__
33
34 #include <string.h>
35
36 #include <array>
37 #include <cstdint>
38 #include <cstdlib>
39 #include <limits>
40
41 #ifdef NDEBUG
42 #define LAZDEBUG(e) ((void)0)
43 #else
44 #define LAZDEBUG(e) (void)(e)
45 #endif
46
47 namespace lazperf
48 {
49 namespace utils
50 {
51
52 template<int BIT, typename T>
clearBit(T t)53 T clearBit(T t)
54 {
55 return t & ~(1 << BIT);
56 }
57
58 // Clamp the input value to the range of the output type.
59 template<typename T, typename U>
60 T clamp(U u)
61 {
62 constexpr T mn = (std::numeric_limits<T>::min)();
63 constexpr T mx = (std::numeric_limits<T>::max)();
64 if (u <= mn)
65 return mn;
66 else if (u >= mx)
67 return mx;
68 return u;
69 }
70
u2d(uint64_t u)71 inline double u2d(uint64_t u)
72 {
73 double d;
74 memcpy(&d, &u, sizeof(d));
75 return d;
76 }
77
i2d(int64_t i)78 inline double i2d(int64_t i)
79 {
80 double d;
81 memcpy(&d, &i, sizeof(d));
82 return d;
83 }
84
d2u(double d)85 inline uint64_t d2u(double d)
86 {
87 uint64_t u;
88 memcpy(&u, &d, sizeof(u));
89 return u;
90 }
91
d2i(double d)92 inline int64_t d2i(double d)
93 {
94 int64_t i;
95 memcpy(&i, &d, sizeof(i));
96 return i;
97 }
98
99 template<typename T>
unpack(const char *)100 T unpack(const char *)
101 {
102 static_assert(sizeof(T) != 0, "Only specialized instances of packers should be used");
103 };
104
105 //ABELL - All this junk should be replaced with (no-op) endian changes followed by a copy, which
106 // probably means one instruction in the end.
107 template<>
unpack(const char * in)108 inline uint64_t unpack(const char *in)
109 {
110 uint64_t b1 = in[0],
111 b2 = in[1],
112 b3 = in[2],
113 b4 = in[3],
114 b5 = in[4],
115 b6 = in[5],
116 b7 = in[6],
117 b8 = in[7];
118
119 return (b8 << 56) | (b7 << 48) | (b6 << 40) | (b5 << 32) |
120 (b4 << 24) | (b3 << 16) | (b2 << 8) | b1;
121 }
122
pack(uint64_t v,char * out)123 inline void pack(uint64_t v, char *out)
124 {
125 out[7] = (char)(v >> 56);
126 out[6] = (char)(v >> 48);
127 out[5] = (char)(v >> 40);
128 out[4] = (char)(v >> 32);
129 out[3] = (char)(v >> 24);
130 out[2] = (char)(v >> 16);
131 out[1] = (char)(v >> 8);
132 out[0] = (char)v;
133 }
134
135 template<>
unpack(const char * in)136 inline uint32_t unpack(const char *in)
137 {
138 uint32_t b1 = in[0],
139 b2 = in[1],
140 b3 = in[2],
141 b4 = in[3];
142
143 return ((b4 << 24) |
144 ((b3 & 0xFF) << 16) |
145 ((b2 & 0xFF) << 8) |
146 (b1 & 0xFF));
147 }
148
pack(const uint32_t v,char * out)149 inline void pack(const uint32_t v, char *out)
150 {
151 out[3] = (v >> 24) & 0xFF;
152 out[2] = (v >> 16) & 0xFF;
153 out[1] = (v >> 8) & 0xFF;
154 out[0] = v & 0xFF;
155 }
156
157 template<>
unpack(const char * in)158 inline double unpack(const char *in)
159 {
160 uint64_t lower = unpack<uint32_t>(in);
161 uint64_t upper = unpack<uint32_t>(in + 4);
162 return u2d((upper << 32) | lower);
163 }
164
pack(const double & d,char * buf)165 inline void pack(const double& d, char *buf)
166 {
167 uint64_t val = d2u(d);
168 pack(uint32_t(val & 0xFFFFFFFF), buf);
169 pack(uint32_t(val >> 32), buf + 4);
170 }
171
172 template<>
unpack(const char * in)173 inline uint16_t unpack(const char *in)
174 {
175 uint16_t b1 = in[0],
176 b2 = in[1];
177
178 return (((b2 & 0xFF) << 8) | (b1 & 0xFF));
179 }
180
pack(const uint16_t v,char * out)181 inline void pack(const uint16_t v, char *out)
182 {
183 out[1] = (v >> 8) & 0xFF;
184 out[0] = v & 0xFF;
185 }
186
187 template<>
unpack(const char * in)188 inline int64_t unpack(const char *in)
189 {
190 return static_cast<int64_t>(unpack<uint64_t>(in));
191 }
192
pack(int64_t t,char * out)193 inline void pack(int64_t t, char *out)
194 {
195 pack(static_cast<uint64_t>(t), out);
196 }
197
198 template<>
unpack(const char * in)199 inline int32_t unpack(const char *in)
200 {
201 return static_cast<uint32_t>(unpack<uint32_t>(in));
202 }
203
pack(int32_t t,char * out)204 inline void pack(int32_t t, char *out)
205 {
206 pack(static_cast<uint32_t>(t), out);
207 }
208
209 template<>
unpack(const char * in)210 inline int16_t unpack(const char *in)
211 {
212 return static_cast<uint16_t>(unpack<uint16_t>(in));
213 }
214
pack(int16_t t,char * out)215 inline void pack(int16_t t, char *out)
216 {
217 pack(static_cast<uint16_t>(t), out);
218 }
219
sum(const uint8_t * buf,uint32_t size)220 inline int32_t sum(const uint8_t *buf, uint32_t size)
221 {
222 int32_t total = 0;
223 while (size--)
224 total += *buf++;
225 return total;
226 }
227
228 struct Summer
229 {
Summerlazperf::utils::Summer230 Summer() : sum(0), cnt(0)
231 {}
232
233 template <typename T>
addlazperf::utils::Summer234 void add(const T& t)
235 {
236 const uint8_t *b = reinterpret_cast<const uint8_t *>(&t);
237 sum += utils::sum(b, sizeof(T));
238 cnt++;
239 }
240
addlazperf::utils::Summer241 void add(uint8_t *b, size_t size)
242 {
243 sum += utils::sum(b, size);
244 }
245
valuelazperf::utils::Summer246 uint32_t value()
247 {
248 uint32_t v = sum;
249 sum = 0;
250 return v;
251 }
252
countlazperf::utils::Summer253 uint32_t count()
254 {
255 uint32_t c = cnt;
256 cnt = 0;
257 return c;
258 }
259
260 uint32_t sum;
261 uint32_t cnt;
262 };
263
264 #define ALIGN 64
265
aligned_malloc(int size)266 inline void *aligned_malloc(int size)
267 {
268 void *mem = malloc(size+ALIGN+sizeof(void*));
269 void **ptr = (void**)(( ((uintptr_t)mem)+ALIGN+sizeof(void*) ) & ~(uintptr_t)(ALIGN-1) );
270 ptr[-1] = mem;
271 return ptr;
272 }
273
aligned_free(void * ptr)274 inline void aligned_free(void *ptr)
275 {
276 free(((void**)ptr)[-1]);
277 }
278
279 template<typename T>
280 class streaming_median
281 {
282 public:
283 std::array<T, 5> values;
284 bool high;
285
init()286 void init() {
287 values.fill(T(0));
288 high = true;
289 }
290
add(const T & v)291 inline void add(const T& v)
292 {
293 if (high) {
294 if (v < values[2]) {
295 values[4] = values[3];
296 values[3] = values[2];
297 if (v < values[0]) {
298 values[2] = values[1];
299 values[1] = values[0];
300 values[0] = v;
301 }
302 else if (v < values[1]) {
303 values[2] = values[1];
304 values[1] = v;
305 }
306 else {
307 values[2] = v;
308 }
309 }
310 else {
311 if (v < values[3]) {
312 values[4] = values[3];
313 values[3] = v;
314 }
315 else {
316 values[4] = v;
317 }
318 high = false;
319 }
320 }
321 else {
322 if (values[2] < v) {
323 values[0] = values[1];
324 values[1] = values[2];
325 if (values[4] < v) {
326 values[2] = values[3];
327 values[3] = values[4];
328 values[4] = v;
329 }
330 else if (values[3] < v) {
331 values[2] = values[3];
332 values[3] = v;
333 }
334 else {
335 values[2] = v;
336 }
337 }
338 else {
339 if (values[1] < v) {
340 values[0] = values[1];
341 values[1] = v;
342 }
343 else {
344 values[0] = v;
345 }
346 high = true;
347 }
348 }
349 }
350
get() const351 inline T get() const {
352 return values[2];
353 }
354
streaming_median()355 streaming_median() {
356 init();
357 }
358 };
359
360 } // namespace utils
361 } // namespace lazperf
362
363 #endif // __util_hpp__
364