36 #ifndef VIGRA_MATHUTIL_HXX
37 #define VIGRA_MATHUTIL_HXX
40 # pragma warning (disable: 4996) // hypot/_hypot confusion
48 #include "sized_int.hxx"
49 #include "numerictraits.hxx"
71 # define M_PI 3.14159265358979323846
75 # define M_SQRT2 1.41421356237309504880
97 #define VIGRA_DEFINE_UNSIGNED_ABS(T) \
98 inline T abs(T t) { return t; }
100 VIGRA_DEFINE_UNSIGNED_ABS(
bool)
101 VIGRA_DEFINE_UNSIGNED_ABS(
unsigned char)
102 VIGRA_DEFINE_UNSIGNED_ABS(
unsigned short)
103 VIGRA_DEFINE_UNSIGNED_ABS(
unsigned int)
104 VIGRA_DEFINE_UNSIGNED_ABS(
unsigned long)
105 VIGRA_DEFINE_UNSIGNED_ABS(
unsigned long long)
107 #undef VIGRA_DEFINE_UNSIGNED_ABS
109 #define VIGRA_DEFINE_MISSING_ABS(T) \
110 inline T abs(T t) { return t < 0 ? -t : t; }
112 VIGRA_DEFINE_MISSING_ABS(
signed char)
113 VIGRA_DEFINE_MISSING_ABS(
signed short)
115 #undef VIGRA_DEFINE_MISSING_ABS
132 inline double round(
double t)
139 inline long double round(
long double t)
210 static Int32 table[64];
214 Int32 IntLog2<T>::table[64] = {
215 -1, 0, -1, 15, -1, 1, 28, -1, 16, -1, -1, -1, 2, 21,
216 29, -1, -1, -1, 19, 17, 10, -1, 12, -1, -1, 3, -1, 6,
217 -1, 22, 30, -1, 14, -1, 27, -1, -1, -1, 20, -1, 18, 9,
218 11, -1, 5, -1, -1, 13, 26, -1, -1, 8, -1, 4, -1, 25,
219 -1, 7, 24, -1, 23, -1, 31, -1};
248 return detail::IntLog2<Int32>::table[x >> 26];
260 typename NumericTraits<T>::Promote
sq(T t)
271 static UInt32 sqq_table[];
276 UInt32 IntSquareRoot<T>::sqq_table[] = {
277 0, 16, 22, 27, 32, 35, 39, 42, 45, 48, 50, 53, 55, 57,
278 59, 61, 64, 65, 67, 69, 71, 73, 75, 76, 78, 80, 81, 83,
279 84, 86, 87, 89, 90, 91, 93, 94, 96, 97, 98, 99, 101, 102,
280 103, 104, 106, 107, 108, 109, 110, 112, 113, 114, 115, 116, 117, 118,
281 119, 120, 121, 122, 123, 124, 125, 126, 128, 128, 129, 130, 131, 132,
282 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 144, 145,
283 146, 147, 148, 149, 150, 150, 151, 152, 153, 154, 155, 155, 156, 157,
284 158, 159, 160, 160, 161, 162, 163, 163, 164, 165, 166, 167, 167, 168,
285 169, 170, 170, 171, 172, 173, 173, 174, 175, 176, 176, 177, 178, 178,
286 179, 180, 181, 181, 182, 183, 183, 184, 185, 185, 186, 187, 187, 188,
287 189, 189, 190, 191, 192, 192, 193, 193, 194, 195, 195, 196, 197, 197,
288 198, 199, 199, 200, 201, 201, 202, 203, 203, 204, 204, 205, 206, 206,
289 207, 208, 208, 209, 209, 210, 211, 211, 212, 212, 213, 214, 214, 215,
290 215, 216, 217, 217, 218, 218, 219, 219, 220, 221, 221, 222, 222, 223,
291 224, 224, 225, 225, 226, 226, 227, 227, 228, 229, 229, 230, 230, 231,
292 231, 232, 232, 233, 234, 234, 235, 235, 236, 236, 237, 237, 238, 238,
293 239, 240, 240, 241, 241, 242, 242, 243, 243, 244, 244, 245, 245, 246,
294 246, 247, 247, 248, 248, 249, 249, 250, 250, 251, 251, 252, 252, 253,
305 if (x >= 0x40000000) {
308 xn = sqq_table[x>>24] << 8;
310 xn = sqq_table[x>>22] << 7;
313 xn = sqq_table[x>>20] << 6;
315 xn = sqq_table[x>>18] << 5;
319 xn = sqq_table[x>>16] << 4;
321 xn = sqq_table[x>>14] << 3;
324 xn = sqq_table[x>>12] << 2;
326 xn = sqq_table[x>>10] << 1;
334 xn = (sqq_table[x>>8] >> 0) + 1;
336 xn = (sqq_table[x>>6] >> 1) + 1;
339 xn = (sqq_table[x>>4] >> 2) + 1;
341 xn = (sqq_table[x>>2] >> 3) + 1;
345 return sqq_table[x] >> 4;
349 xn = (xn + 1 + x / xn) / 2;
351 xn = (xn + 1 + x / xn) / 2;
374 throw std::domain_error(
"sqrti(Int32): negative argument.");
375 return (
Int32)detail::IntSquareRoot<UInt32>::exec((
UInt32)v);
387 return detail::IntSquareRoot<UInt32>::exec(v);
390 #ifdef VIGRA_NO_HYPOT
399 inline double hypot(
double a,
double b)
401 double absa = VIGRA_CSTD::fabs(a), absb = VIGRA_CSTD::fabs(b);
426 return t > NumericTraits<T>::zero()
427 ? NumericTraits<T>::one()
428 : t < NumericTraits<T>::zero()
429 ? -NumericTraits<T>::one()
430 : NumericTraits<T>::zero();
443 return t > NumericTraits<T>::zero()
445 : t < NumericTraits<T>::zero()
457 template <
class T1,
class T2>
460 return t2 >= NumericTraits<T2>::zero()
465 #define VIGRA_DEFINE_NORM(T) \
466 inline NormTraits<T>::SquaredNormType squaredNorm(T t) { return sq(t); } \
467 inline NormTraits<T>::NormType norm(T t) { return abs(t); }
469 VIGRA_DEFINE_NORM(
bool)
470 VIGRA_DEFINE_NORM(
signed char)
471 VIGRA_DEFINE_NORM(
unsigned char)
472 VIGRA_DEFINE_NORM(
short)
473 VIGRA_DEFINE_NORM(
unsigned short)
474 VIGRA_DEFINE_NORM(
int)
475 VIGRA_DEFINE_NORM(
unsigned int)
476 VIGRA_DEFINE_NORM(
long)
477 VIGRA_DEFINE_NORM(
unsigned long)
478 VIGRA_DEFINE_NORM(
float)
479 VIGRA_DEFINE_NORM(
double)
480 VIGRA_DEFINE_NORM(
long double)
482 #undef VIGRA_DEFINE_NORM
485 inline typename NormTraits<std::complex<T> >::SquaredNormType
488 return sq(t.real()) +
sq(t.imag());
491 #ifdef DOXYGEN // only for documentation
499 NormTraits<T>::SquaredNormType
squaredNorm(T
const & t);
512 inline typename NormTraits<T>::NormType
515 typedef typename NormTraits<T>::SquaredNormType SNT;
516 return sqrt(
static_cast<typename SquareRootTraits<SNT>::SquareRootArgument
>(
squaredNorm(t)));
534 template <
class Iterator>
535 Iterator
argMin(Iterator first, Iterator last)
537 typedef typename std::iterator_traits<Iterator>::value_type Value;
538 Value vopt = NumericTraits<Value>::max();
539 Iterator best = last;
540 for(; first != last; ++first)
566 template <
class Iterator>
567 Iterator
argMax(Iterator first, Iterator last)
569 typedef typename std::iterator_traits<Iterator>::value_type Value;
570 Value vopt = NumericTraits<Value>::min();
571 Iterator best = last;
572 for(; first != last; ++first)
604 template <
class Iterator,
class UnaryFunctor>
605 Iterator
argMinIf(Iterator first, Iterator last, UnaryFunctor condition)
607 typedef typename std::iterator_traits<Iterator>::value_type Value;
608 Value vopt = NumericTraits<Value>::max();
609 Iterator best = last;
610 for(; first != last; ++first)
612 if(condition(*first) && *first < vopt)
642 template <
class Iterator,
class UnaryFunctor>
643 Iterator
argMaxIf(Iterator first, Iterator last, UnaryFunctor condition)
645 typedef typename std::iterator_traits<Iterator>::value_type Value;
646 Value vopt = NumericTraits<Value>::min();
647 Iterator best = last;
648 for(; first != last; ++first)
650 if(condition(*first) && vopt < *first)
672 double d =
hypot(a00 - a11, 2.0*a01);
673 *r0 =
static_cast<T
>(0.5*(a00 + a11 + d));
674 *r1 =
static_cast<T
>(0.5*(a00 + a11 - d));
692 T * r0, T * r1, T * r2)
694 static double inv3 = 1.0 / 3.0, root3 =
std::sqrt(3.0);
696 double c0 = a00*a11*a22 + 2.0*a01*a02*a12 - a00*a12*a12 - a11*a02*a02 - a22*a01*a01;
697 double c1 = a00*a11 - a01*a01 + a00*a22 - a02*a02 + a11*a22 - a12*a12;
698 double c2 = a00 + a11 + a22;
699 double c2Div3 = c2*inv3;
700 double aDiv3 = (c1 - c2*c2Div3)*inv3;
703 double mbDiv2 = 0.5*(c0 + c2Div3*(2.0*c2Div3*c2Div3 - c1));
704 double q = mbDiv2*mbDiv2 + aDiv3*aDiv3*aDiv3;
711 *r0 =
static_cast<T
>(c2Div3 + 2.0*magnitude*cs);
712 *r1 =
static_cast<T
>(c2Div3 - magnitude*(cs + root3*sn));
713 *r2 =
static_cast<T
>(c2Div3 - magnitude*(cs - root3*sn));
725 T ellipticRD(T x, T y, T z)
727 double f = 1.0, s = 0.0, X, Y, Z, m;
730 m = (x + y + 3.0*z) / 5.0;
734 if(std::max(std::max(VIGRA_CSTD::fabs(X), VIGRA_CSTD::fabs(Y)), VIGRA_CSTD::fabs(Z)) < 0.01)
746 double d = a - 6.0*b;
747 double e = d + 2.0*c;
748 return 3.0*s + f*(1.0+d*(-3.0/14.0+d*9.0/88.0-Z*e*4.5/26.0)
753 T ellipticRF(T x, T y, T z)
758 m = (x + y + z) / 3.0;
762 if(std::max(std::max(VIGRA_CSTD::fabs(X), VIGRA_CSTD::fabs(Y)), VIGRA_CSTD::fabs(Z)) < 0.01)
769 double d = X*Y -
sq(Z);
796 return s*detail::ellipticRF(c2, 1.0 -
sq(k*s), 1.0);
821 return s*(detail::ellipticRF(c2, 1.0-k, 1.0) - k/3.0*detail::ellipticRD(c2, 1.0-k, 1.0));
831 double t = 1.0/(1.0+0.5*VIGRA_CSTD::fabs(x));
832 double ans = t*
VIGRA_CSTD::exp(-x*x-1.26551223+t*(1.00002368+t*(0.37409196+
833 t*(0.09678418+t*(-0.18628806+t*(0.27886807+
834 t*(-1.13520398+t*(1.48851587+t*(-0.82215223+
835 t*0.17087277)))))))));
859 inline double erf(
double x)
861 return detail::erfImpl(x);
873 double noncentralChi2CDFApprox(
unsigned int degreesOfFreedom, T noncentrality, T arg)
875 double a = degreesOfFreedom + noncentrality;
876 double b = (a + noncentrality) /
sq(a);
882 void noncentralChi2OneIteration(T arg, T & lans, T & dans, T & pans,
unsigned int & j)
892 dans = dans * arg / j;
899 std::pair<double, double> noncentralChi2CDF(
unsigned int degreesOfFreedom, T noncentrality, T arg, T eps)
901 vigra_precondition(noncentrality >= 0.0 && arg >= 0.0 && eps > 0.0,
902 "noncentralChi2P(): parameters must be positive.");
903 if (arg == 0.0 && degreesOfFreedom > 0)
904 return std::make_pair(0.0, 0.0);
907 double b1 = 0.5 * noncentrality,
910 lnrtpi2 = 0.22579135264473,
911 probability, density, lans, dans, pans, sum, am, hold;
912 unsigned int maxit = 500,
914 if(degreesOfFreedom % 2)
930 if(degreesOfFreedom == 0)
933 degreesOfFreedom = 2;
935 sum = 1.0 / ao - 1.0 - am;
937 probability = 1.0 + am * pans;
942 degreesOfFreedom = degreesOfFreedom - 1;
944 sum = 1.0 / ao - 1.0;
945 while(i < degreesOfFreedom)
946 detail::noncentralChi2OneIteration(arg, lans, dans, pans, i);
947 degreesOfFreedom = degreesOfFreedom + 1;
952 for(++m; m<maxit; ++m)
955 detail::noncentralChi2OneIteration(arg, lans, dans, pans, degreesOfFreedom);
957 density = density + am * dans;
959 probability = probability + hold;
960 if((pans * sum < eps2) && (hold < eps2))
964 vigra_fail(
"noncentralChi2P(): no convergence.");
965 return std::make_pair(0.5 * ao * density, std::min(1.0, std::max(0.0, ao * probability)));
979 inline double chi2(
unsigned int degreesOfFreedom,
double arg,
double accuracy = 1e-7)
981 return detail::noncentralChi2CDF(degreesOfFreedom, 0.0, arg, accuracy).first;
994 inline double chi2CDF(
unsigned int degreesOfFreedom,
double arg,
double accuracy = 1e-7)
996 return detail::noncentralChi2CDF(degreesOfFreedom, 0.0, arg, accuracy).second;
1011 double noncentrality,
double arg,
double accuracy = 1e-7)
1013 return detail::noncentralChi2CDF(degreesOfFreedom, noncentrality, arg, accuracy).first;
1029 double noncentrality,
double arg,
double accuracy = 1e-7)
1031 return detail::noncentralChi2CDF(degreesOfFreedom, noncentrality, arg, accuracy).second;
1050 return detail::noncentralChi2CDFApprox(degreesOfFreedom, noncentrality, arg);
1060 FPT safeFloatDivision( FPT f1, FPT f2 )
1062 return f2 < NumericTraits<FPT>::one() && f1 > f2 * NumericTraits<FPT>::max()
1063 ? NumericTraits<FPT>::max()
1064 : (f2 > NumericTraits<FPT>::one() && f1 < f2 * NumericTraits<FPT>::smallestPositive()) ||
1065 f1 == NumericTraits<FPT>::zero()
1066 ? NumericTraits<FPT>::zero()
1082 template <
class T1,
class T2>
1086 typedef typename PromoteTraits<T1, T2>::Promote T;
1088 return VIGRA_CSTD::fabs(r) <= epsilon;
1090 return VIGRA_CSTD::fabs(l) <= epsilon;
1091 T diff = VIGRA_CSTD::fabs( l - r );
1092 T d1 = detail::safeFloatDivision<T>( diff, VIGRA_CSTD::fabs( r ) );
1093 T d2 = detail::safeFloatDivision<T>( diff, VIGRA_CSTD::fabs( l ) );
1095 return (d1 <= epsilon && d2 <= epsilon);
1098 template <
class T1,
class T2>
1101 typedef typename PromoteTraits<T1, T2>::Promote T;