12
const double M_PI = 3.1415926535897932384626433832795;
25
inline T sqr(const T& x)
29
inline T cube(const T& x)
33
inline T min(T a1, T a2, T a3)
34
{ return min(a1, min(a2, a3)); }
37
inline T min(T a1, T a2, T a3, T a4)
38
{ return min(min(a1, a2), min(a3, a4)); }
41
inline T min(T a1, T a2, T a3, T a4, T a5)
42
{ return min(min(a1, a2), min(a3, a4), a5); }
45
inline T min(T a1, T a2, T a3, T a4, T a5, T a6)
46
{ return min(min(a1, a2), min(a3, a4), min(a5, a6)); }
49
inline T max(T a1, T a2, T a3)
50
{ return max(a1, max(a2, a3)); }
53
inline T max(T a1, T a2, T a3, T a4)
54
{ return max(max(a1, a2), max(a3, a4)); }
57
inline T max(T a1, T a2, T a3, T a4, T a5)
58
{ return max(max(a1, a2), max(a3, a4), a5); }
61
inline T max(T a1, T a2, T a3, T a4, T a5, T a6)
62
{ return max(max(a1, a2), max(a3, a4), max(a5, a6)); }
65
inline void minmax(T a1, T a2, T& amin, T& amax)
77
inline void minmax(T a1, T a2, T a3, T& amin, T& amax)
102
inline void minmax(T a1, T a2, T a3, T a4, T& amin, T& amax)
124
inline void minmax(T a1, T a2, T a3, T a4, T a5, T& amin, T& amax)
126
//@@@ the logic could be shortcircuited a lot!
127
amin=min(a1,a2,a3,a4,a5);
128
amax=max(a1,a2,a3,a4,a5);
132
inline void minmax(T a1, T a2, T a3, T a4, T a5, T a6, T& amin, T& amax)
134
//@@@ the logic could be shortcircuited a lot!
135
amin=min(a1,a2,a3,a4,a5,a6);
136
amax=max(a1,a2,a3,a4,a5,a6);
140
inline void update_minmax(T a1, T& amin, T& amax)
143
else if(a1>amax) amax=a1;
147
inline void sort(T &a, T &b, T &c)
156
temp=c;c=b;b=a;a=temp;
163
temp=b;b=c;c=a;a=temp;
172
inline T clamp(T a, T lower, T upper)
174
if(a<lower) return lower;
175
else if(a>upper) return upper;
179
// only makes sense with T=float or double
181
inline T smooth_step(T r)
184
else if(r>1) return 1;
185
return r*r*r*(10+r*(-15+r*6));
188
// only makes sense with T=float or double
190
inline T smooth_step(T r, T r_lower, T r_upper, T value_lower, T value_upper)
191
{ return value_lower + smooth_step((r-r_lower)/(r_upper-r_lower)) * (value_upper-value_lower); }
193
// only makes sense with T=float or double
196
{ return smooth_step((r+1)/2)*2-1; }
199
inline int lround(double x)
202
return (x-floor(x)<0.5) ? (int)floor(x) : (int)ceil(x);
204
return (x-floor(x)<=0.5) ? (int)floor(x) : (int)ceil(x);
207
inline double remainder(double x, double y)
209
return x-std::floor(x/y+0.5)*y;
213
inline unsigned int round_up_to_power_of_two(unsigned int n)
224
inline unsigned int round_down_to_power_of_two(unsigned int n)
234
// Transforms even the sequence 0,1,2,3,... into reasonably good random numbers
235
// Challenge: improve on this in speed and "randomness"!
236
// This seems to pass several statistical tests, and is a bijective map (of 32-bit unsigned ints)
238
inline unsigned int randhash(unsigned int seed)
240
unsigned int i=(seed^12345391u)*2654435769u;
247
// the inverse of randhash
248
inline unsigned int unhash(unsigned int h)
259
// returns repeatable stateless pseudo-random number in [0,1]
260
inline double randhashd(unsigned int seed)
261
{ return randhash(seed)/(double)UINT_MAX; }
262
inline float randhashf(unsigned int seed)
263
{ return randhash(seed)/(float)UINT_MAX; }
265
// returns repeatable stateless pseudo-random number in [a,b]
266
inline double randhashd(unsigned int seed, double a, double b)
267
{ return (b-a)*randhash(seed)/(double)UINT_MAX + a; }
268
inline float randhashf(unsigned int seed, float a, float b)
269
{ return ( (b-a)*randhash(seed)/(float)UINT_MAX + a); }
271
inline int intlog2(int x)
282
inline void get_barycentric(T x, ssize_t& i, T& f, ssize_t i_low, ssize_t i_high)
289
}else if(i>i_high-2){
296
template<class S, class T>
297
inline S lerp(const S& value0, const S& value1, T f)
298
{ return (1-f)*value0 + f*value1; }
300
template<class S, class T>
301
inline S bilerp(const S& v00, const S& v10,
302
const S& v01, const S& v11,
305
return lerp(lerp(v00, v10, fx),
310
template<class S, class T>
311
inline S trilerp(const S& v000, const S& v100,
312
const S& v010, const S& v110,
313
const S& v001, const S& v101,
314
const S& v011, const S& v111,
317
return lerp(bilerp(v000, v100, v010, v110, fx, fy),
318
bilerp(v001, v101, v011, v111, fx, fy),
322
template<class S, class T>
323
inline S quadlerp(const S& v0000, const S& v1000,
324
const S& v0100, const S& v1100,
325
const S& v0010, const S& v1010,
326
const S& v0110, const S& v1110,
327
const S& v0001, const S& v1001,
328
const S& v0101, const S& v1101,
329
const S& v0011, const S& v1011,
330
const S& v0111, const S& v1111,
331
T fx, T fy, T fz, T ft)
333
return lerp(trilerp(v0000, v1000, v0100, v1100, v0010, v1010, v0110, v1110, fx, fy, fz),
334
trilerp(v0001, v1001, v0101, v1101, v0011, v1011, v0111, v1111, fx, fy, fz),
338
// f should be between 0 and 1, with f=0.5 corresponding to balanced weighting between w0 and w2
340
inline void quadratic_bspline_weights(T f, T& w0, T& w1, T& w2)
343
w1=T(0.75)-sqr(f-T(0.5));;
347
// f should be between 0 and 1
349
inline void cubic_interp_weights(T f, T& wneg1, T& w0, T& w1, T& w2)
352
wneg1=-T(1./3)*f+T(1./2)*f2-T(1./6)*f3;
353
w0=1-f2+T(1./2)*(f3-f);
354
w1=f+T(1./2)*(f2-f3);
358
template<class S, class T>
359
inline S cubic_interp(const S& value_neg1, const S& value0, const S& value1, const S& value2, T f)
362
cubic_interp_weights(f, wneg1, w0, w1, w2);
363
return wneg1*value_neg1 + w0*value0 + w1*value1 + w2*value2;
366
template<class S, class T>
367
inline S bicubic_interp(const S& v00, const S& v10, const S& v20, const S& v30,
368
const S& v01, const S& v11, const S& v21, const S& v31,
369
const S& v02, const S& v12, const S& v22, const S& v32,
370
const S& v03, const S& v13, const S& v23, const S& v33,
373
return cubic_interp( cubic_interp( v00, v10, v20, v30, fx ),
374
cubic_interp( v01, v11, v21, v31, fx ),
375
cubic_interp( v02, v12, v22, v32, fx ),
376
cubic_interp( v03, v13, v23, v33, fx ),
380
template<class S, class T>
381
inline S tricubic_interp( const Array3<S, Array1<S> >& v, ssize_t i, ssize_t j, ssize_t k, T fx, T fy, T fz )
383
return cubic_interp( bicubic_interp( v(i+0,j+0,k+0), v(i+1,j+0,k+0), v(i+2,j+0,k+0), v(i+3,j+0,k+0),
384
v(i+0,j+1,k+0), v(i+1,j+1,k+0), v(i+2,j+1,k+0), v(i+3,j+1,k+0),
385
v(i+0,j+2,k+0), v(i+1,j+2,k+0), v(i+2,j+2,k+0), v(i+3,j+2,k+0),
386
v(i+0,j+3,k+0), v(i+1,j+3,k+0), v(i+2,j+3,k+0), v(i+3,j+3,k+0), fx, fy ),
387
bicubic_interp( v(i+0,j+0,k+1), v(i+1,j+0,k+1), v(i+2,j+0,k+1), v(i+3,j+0,k+1),
388
v(i+0,j+1,k+1), v(i+1,j+1,k+1), v(i+2,j+1,k+1), v(i+3,j+1,k+1),
389
v(i+0,j+2,k+1), v(i+1,j+2,k+1), v(i+2,j+2,k+1), v(i+3,j+2,k+1),
390
v(i+0,j+3,k+1), v(i+1,j+3,k+1), v(i+2,j+3,k+1), v(i+3,j+3,k+1), fx, fy ),
391
bicubic_interp( v(i+0,j+0,k+2), v(i+1,j+0,k+2), v(i+2,j+0,k+2), v(i+3,j+0,k+2),
392
v(i+0,j+1,k+2), v(i+1,j+1,k+2), v(i+2,j+1,k+2), v(i+3,j+1,k+2),
393
v(i+0,j+2,k+2), v(i+1,j+2,k+2), v(i+2,j+2,k+2), v(i+3,j+2,k+2),
394
v(i+0,j+3,k+2), v(i+1,j+3,k+2), v(i+2,j+3,k+2), v(i+3,j+3,k+2), fx, fy ),
395
bicubic_interp( v(i+0,j+0,k+3), v(i+1,j+0,k+3), v(i+2,j+0,k+3), v(i+3,j+0,k+3),
396
v(i+0,j+1,k+3), v(i+1,j+1,k+3), v(i+2,j+1,k+3), v(i+3,j+1,k+3),
397
v(i+0,j+2,k+3), v(i+1,j+2,k+3), v(i+2,j+2,k+3), v(i+3,j+2,k+3),
398
v(i+0,j+3,k+3), v(i+1,j+3,k+3), v(i+2,j+3,k+3), v(i+3,j+3,k+3), fx, fy ),
405
void zero(std::vector<T>& v)
406
{ for(int i=(int)v.size()-1; i>=0; --i) v[i]=0; }
409
T abs_max(const std::vector<T>& v)
412
for(int i=(int)v.size()-1; i>=0; --i){
413
if(std::fabs(v[i])>m)
420
bool contains(const std::vector<T>& a, T e)
422
for(unsigned int i=0; i<a.size(); ++i)
423
if(a[i]==e) return true;
428
void add_unique(std::vector<T>& a, T e)
430
for(unsigned int i=0; i<a.size(); ++i)
436
void insert(std::vector<T>& a, unsigned int index, T e)
438
a.push_back(a.back());
439
for(unsigned int i=(unsigned int)a.size()-1; i>index; --i)
445
void erase(std::vector<T>& a, unsigned int index)
447
for(unsigned int i=index; i<a.size()-1; ++i)
453
void erase_swap(std::vector<T>& a, unsigned int index)
455
for(unsigned int i=index; i<a.size()-1; ++i)
461
void erase_unordered(std::vector<T>& a, unsigned int index)
468
void erase_unordered_swap(std::vector<T>& a, unsigned int index)
470
swap(a[index], a.back());
475
void find_and_erase_unordered(std::vector<T>& a, const T& doomed_element)
477
for(unsigned int i=0; i<a.size(); ++i)
478
if(a[i]==doomed_element){
479
erase_unordered(a, i);
485
void replace_once(std::vector<T>& a, const T& old_element, const T& new_element)
487
for(unsigned int i=0; i<a.size(); ++i)
488
if(a[i]==old_element){
495
void write_matlab(std::ostream& output, const std::vector<T>& a, const char *variable_name, bool column_vector=true, int significant_digits=18)
497
output<<variable_name<<"=[";
498
std::streamsize old_precision=output.precision();
499
output.precision(significant_digits);
500
for(unsigned int i=0; i<a.size(); ++i){
506
output<<";"<<std::endl;
507
output.precision(old_precision);