36 #ifndef VIGRA_MATHUTIL_HXX
37 #define VIGRA_MATHUTIL_HXX
40 # pragma warning (disable: 4996) // hypot/_hypot confusion
49 #include "sized_int.hxx"
50 #include "numerictraits.hxx"
51 #include "algorithm.hxx"
90 # define M_PI 3.14159265358979323846
94 # define M_2_PI 0.63661977236758134308
98 # define M_PI_2 1.57079632679489661923
102 # define M_PI_4 0.78539816339744830962
106 # define M_SQRT2 1.41421356237309504880
109 #ifndef M_EULER_GAMMA
110 # define M_EULER_GAMMA 0.5772156649015329
122 using VIGRA_CSTD::pow;
134 #define VIGRA_DEFINE_UNSIGNED_ABS(T) \
135 inline T abs(T t) { return t; }
137 VIGRA_DEFINE_UNSIGNED_ABS(
bool)
138 VIGRA_DEFINE_UNSIGNED_ABS(
unsigned char)
139 VIGRA_DEFINE_UNSIGNED_ABS(
unsigned short)
140 VIGRA_DEFINE_UNSIGNED_ABS(
unsigned int)
141 VIGRA_DEFINE_UNSIGNED_ABS(
unsigned long)
142 VIGRA_DEFINE_UNSIGNED_ABS(
unsigned long long)
144 #undef VIGRA_DEFINE_UNSIGNED_ABS
146 #define VIGRA_DEFINE_MISSING_ABS(T) \
147 inline T abs(T t) { return t < 0 ? -t : t; }
149 VIGRA_DEFINE_MISSING_ABS(
signed char)
150 VIGRA_DEFINE_MISSING_ABS(
signed short)
152 #if defined(_MSC_VER) && _MSC_VER < 1600
153 VIGRA_DEFINE_MISSING_ABS(
signed long long)
156 #undef VIGRA_DEFINE_MISSING_ABS
166 #ifdef DOXYGEN // only for documentation
170 inline float round(
float t)
177 inline double round(
double t)
184 inline long double round(
long double t)
255 static Int32 table[64];
259 Int32 IntLog2<T>::table[64] = {
260 -1, 0, -1, 15, -1, 1, 28, -1, 16, -1, -1, -1, 2, 21,
261 29, -1, -1, -1, 19, 17, 10, -1, 12, -1, -1, 3, -1, 6,
262 -1, 22, 30, -1, 14, -1, 27, -1, -1, -1, 20, -1, 18, 9,
263 11, -1, 5, -1, -1, 13, 26, -1, -1, 8, -1, 4, -1, 25,
264 -1, 7, 24, -1, 23, -1, 31, -1};
293 return detail::IntLog2<Int32>::table[x >> 26];
305 typename NumericTraits<T>::Promote
sq(T t)
316 static UInt32 sqq_table[];
321 UInt32 IntSquareRoot<T>::sqq_table[] = {
322 0, 16, 22, 27, 32, 35, 39, 42, 45, 48, 50, 53, 55, 57,
323 59, 61, 64, 65, 67, 69, 71, 73, 75, 76, 78, 80, 81, 83,
324 84, 86, 87, 89, 90, 91, 93, 94, 96, 97, 98, 99, 101, 102,
325 103, 104, 106, 107, 108, 109, 110, 112, 113, 114, 115, 116, 117, 118,
326 119, 120, 121, 122, 123, 124, 125, 126, 128, 128, 129, 130, 131, 132,
327 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 144, 145,
328 146, 147, 148, 149, 150, 150, 151, 152, 153, 154, 155, 155, 156, 157,
329 158, 159, 160, 160, 161, 162, 163, 163, 164, 165, 166, 167, 167, 168,
330 169, 170, 170, 171, 172, 173, 173, 174, 175, 176, 176, 177, 178, 178,
331 179, 180, 181, 181, 182, 183, 183, 184, 185, 185, 186, 187, 187, 188,
332 189, 189, 190, 191, 192, 192, 193, 193, 194, 195, 195, 196, 197, 197,
333 198, 199, 199, 200, 201, 201, 202, 203, 203, 204, 204, 205, 206, 206,
334 207, 208, 208, 209, 209, 210, 211, 211, 212, 212, 213, 214, 214, 215,
335 215, 216, 217, 217, 218, 218, 219, 219, 220, 221, 221, 222, 222, 223,
336 224, 224, 225, 225, 226, 226, 227, 227, 228, 229, 229, 230, 230, 231,
337 231, 232, 232, 233, 234, 234, 235, 235, 236, 236, 237, 237, 238, 238,
338 239, 240, 240, 241, 241, 242, 242, 243, 243, 244, 244, 245, 245, 246,
339 246, 247, 247, 248, 248, 249, 249, 250, 250, 251, 251, 252, 252, 253,
350 if (x >= 0x40000000) {
353 xn = sqq_table[x>>24] << 8;
355 xn = sqq_table[x>>22] << 7;
358 xn = sqq_table[x>>20] << 6;
360 xn = sqq_table[x>>18] << 5;
364 xn = sqq_table[x>>16] << 4;
366 xn = sqq_table[x>>14] << 3;
369 xn = sqq_table[x>>12] << 2;
371 xn = sqq_table[x>>10] << 1;
379 xn = (sqq_table[x>>8] >> 0) + 1;
381 xn = (sqq_table[x>>6] >> 1) + 1;
384 xn = (sqq_table[x>>4] >> 2) + 1;
386 xn = (sqq_table[x>>2] >> 3) + 1;
390 return sqq_table[x] >> 4;
394 xn = (xn + 1 + x / xn) / 2;
396 xn = (xn + 1 + x / xn) / 2;
419 throw std::domain_error(
"sqrti(Int32): negative argument.");
420 return (
Int32)detail::IntSquareRoot<UInt32>::exec((
UInt32)v);
432 return detail::IntSquareRoot<UInt32>::exec(v);
435 #ifdef VIGRA_NO_HYPOT
444 inline double hypot(
double a,
double b)
446 double absa = VIGRA_CSTD::fabs(a), absb = VIGRA_CSTD::fabs(b);
471 return t > NumericTraits<T>::zero()
472 ? NumericTraits<T>::one()
473 : t < NumericTraits<T>::zero()
474 ? -NumericTraits<T>::one()
475 : NumericTraits<T>::zero();
488 return t > NumericTraits<T>::zero()
490 : t < NumericTraits<T>::zero()
502 template <
class T1,
class T2>
505 return t2 >= NumericTraits<T2>::zero()
511 #ifdef DOXYGEN // only for documentation
526 #define VIGRA_DEFINE_ODD_EVEN(T) \
527 inline bool even(T t) { return (t&1) == 0; } \
528 inline bool odd(T t) { return (t&1) == 1; }
530 VIGRA_DEFINE_ODD_EVEN(
char)
531 VIGRA_DEFINE_ODD_EVEN(
short)
532 VIGRA_DEFINE_ODD_EVEN(
int)
533 VIGRA_DEFINE_ODD_EVEN(
long)
534 VIGRA_DEFINE_ODD_EVEN(
long long)
535 VIGRA_DEFINE_ODD_EVEN(
unsigned char)
536 VIGRA_DEFINE_ODD_EVEN(
unsigned short)
537 VIGRA_DEFINE_ODD_EVEN(
unsigned int)
538 VIGRA_DEFINE_ODD_EVEN(
unsigned long)
539 VIGRA_DEFINE_ODD_EVEN(
unsigned long long)
541 #undef VIGRA_DEFINE_ODD_EVEN
543 #define VIGRA_DEFINE_NORM(T) \
544 inline NormTraits<T>::SquaredNormType squaredNorm(T t) { return sq(t); } \
545 inline NormTraits<T>::NormType norm(T t) { return abs(t); }
547 VIGRA_DEFINE_NORM(
bool)
548 VIGRA_DEFINE_NORM(
signed char)
549 VIGRA_DEFINE_NORM(
unsigned char)
550 VIGRA_DEFINE_NORM(
short)
551 VIGRA_DEFINE_NORM(
unsigned short)
552 VIGRA_DEFINE_NORM(
int)
553 VIGRA_DEFINE_NORM(
unsigned int)
554 VIGRA_DEFINE_NORM(
long)
555 VIGRA_DEFINE_NORM(
unsigned long)
556 VIGRA_DEFINE_NORM(
long long)
557 VIGRA_DEFINE_NORM(
unsigned long long)
558 VIGRA_DEFINE_NORM(
float)
559 VIGRA_DEFINE_NORM(
double)
560 VIGRA_DEFINE_NORM(
long double)
562 #undef VIGRA_DEFINE_NORM
565 inline typename NormTraits<std::complex<T> >::SquaredNormType
568 return sq(t.real()) +
sq(t.imag());
571 #ifdef DOXYGEN // only for documentation
579 NormTraits<T>::SquaredNormType
squaredNorm(T
const & t);
592 inline typename NormTraits<T>::NormType
595 typedef typename NormTraits<T>::SquaredNormType SNT;
596 return sqrt(
static_cast<typename SquareRootTraits<SNT>::SquareRootArgument
>(
squaredNorm(t)));
612 double d =
hypot(a00 - a11, 2.0*a01);
613 *r0 =
static_cast<T
>(0.5*(a00 + a11 + d));
614 *r1 =
static_cast<T
>(0.5*(a00 + a11 - d));
632 T * r0, T * r1, T * r2)
634 static double inv3 = 1.0 / 3.0, root3 =
std::sqrt(3.0);
636 double c0 = a00*a11*a22 + 2.0*a01*a02*a12 - a00*a12*a12 - a11*a02*a02 - a22*a01*a01;
637 double c1 = a00*a11 - a01*a01 + a00*a22 - a02*a02 + a11*a22 - a12*a12;
638 double c2 = a00 + a11 + a22;
639 double c2Div3 = c2*inv3;
640 double aDiv3 = (c1 - c2*c2Div3)*inv3;
643 double mbDiv2 = 0.5*(c0 + c2Div3*(2.0*c2Div3*c2Div3 - c1));
644 double q = mbDiv2*mbDiv2 + aDiv3*aDiv3*aDiv3;
651 *r0 =
static_cast<T
>(c2Div3 + 2.0*magnitude*cs);
652 *r1 =
static_cast<T
>(c2Div3 - magnitude*(cs + root3*sn));
653 *r2 =
static_cast<T
>(c2Div3 - magnitude*(cs - root3*sn));
665 T ellipticRD(T x, T y, T z)
667 double f = 1.0, s = 0.0, X, Y, Z, m;
670 m = (x + y + 3.0*z) / 5.0;
674 if(
std::max(
std::max(VIGRA_CSTD::fabs(X), VIGRA_CSTD::fabs(Y)), VIGRA_CSTD::fabs(Z)) < 0.01)
686 double d = a - 6.0*b;
687 double e = d + 2.0*c;
688 return 3.0*s + f*(1.0+d*(-3.0/14.0+d*9.0/88.0-Z*e*4.5/26.0)
689 +Z*(e/6.0+Z*(-c*9.0/22.0+a*Z*3.0/26.0))) / VIGRA_CSTD::pow(m,1.5);
693 T ellipticRF(T x, T y, T z)
698 m = (x + y + z) / 3.0;
702 if(
std::max(
std::max(VIGRA_CSTD::fabs(X), VIGRA_CSTD::fabs(Y)), VIGRA_CSTD::fabs(Z)) < 0.01)
709 double d = X*Y -
sq(Z);
736 return s*detail::ellipticRF(c2, 1.0 -
sq(k*s), 1.0);
761 return s*(detail::ellipticRF(c2, 1.0-k, 1.0) - k/3.0*detail::ellipticRD(c2, 1.0-k, 1.0));
771 double t = 1.0/(1.0+0.5*VIGRA_CSTD::fabs(x));
772 double ans = t*
VIGRA_CSTD::exp(-x*x-1.26551223+t*(1.00002368+t*(0.37409196+
773 t*(0.09678418+t*(-0.18628806+t*(0.27886807+
774 t*(-1.13520398+t*(1.48851587+t*(-0.82215223+
775 t*0.17087277)))))))));
799 inline double erf(
double x)
801 return detail::erfImpl(x);
815 double a = degreesOfFreedom + noncentrality;
816 double b = (a + noncentrality) /
sq(a);
817 double t = (VIGRA_CSTD::pow((
double)arg / a, 1.0/3.0) - (1.0 - 2.0 / 9.0 * b)) /
VIGRA_CSTD::sqrt(2.0 / 9.0 * b);
822 void noncentralChi2OneIteration(T arg, T & lans, T & dans, T & pans,
unsigned int & j)
832 dans = dans * arg / j;
839 std::pair<double, double>
noncentralChi2CDF(
unsigned int degreesOfFreedom, T noncentrality, T arg, T eps)
841 vigra_precondition(noncentrality >= 0.0 && arg >= 0.0 && eps > 0.0,
842 "noncentralChi2P(): parameters must be positive.");
843 if (arg == 0.0 && degreesOfFreedom > 0)
844 return std::make_pair(0.0, 0.0);
847 double b1 = 0.5 * noncentrality,
850 lnrtpi2 = 0.22579135264473,
851 probability, density, lans, dans, pans,
sum, am, hold;
852 unsigned int maxit = 500,
854 if(degreesOfFreedom % 2)
870 if(degreesOfFreedom == 0)
873 degreesOfFreedom = 2;
875 sum = 1.0 / ao - 1.0 - am;
877 probability = 1.0 + am * pans;
882 degreesOfFreedom = degreesOfFreedom - 1;
884 sum = 1.0 / ao - 1.0;
885 while(i < degreesOfFreedom)
886 detail::noncentralChi2OneIteration(arg, lans, dans, pans, i);
887 degreesOfFreedom = degreesOfFreedom + 1;
892 for(++m; m<maxit; ++m)
895 detail::noncentralChi2OneIteration(arg, lans, dans, pans, degreesOfFreedom);
897 density = density + am * dans;
899 probability = probability + hold;
900 if((pans * sum < eps2) && (hold < eps2))
904 vigra_fail(
"noncentralChi2P(): no convergence.");
905 return std::make_pair(0.5 * ao * density,
std::min(1.0,
std::max(0.0, ao * probability)));
919 inline double chi2(
unsigned int degreesOfFreedom,
double arg,
double accuracy = 1e-7)
934 inline double chi2CDF(
unsigned int degreesOfFreedom,
double arg,
double accuracy = 1e-7)
951 double noncentrality,
double arg,
double accuracy = 1e-7)
969 double noncentrality,
double arg,
double accuracy = 1e-7)
1000 T tmp = NumericTraits<T>::one();
1001 for(T f = l-m+1; f <= l+m; ++f)
1018 template <
class REAL>
1021 vigra_precondition(
abs(x) <= 1.0,
"legendre(): x must be in [-1.0, 1.0].");
1028 return legendre(l,m,x) * s / detail::facLM<REAL>(l,m);
1033 REAL r =
std::sqrt( (1.0-x) * (1.0+x) );
1035 for (
int i=1; i<=m; i++)
1044 REAL result_1 = x * (2.0 * m + 1.0) * result;
1048 for(
unsigned int i = m+2; i <= l; ++i)
1050 other = ( (2.0*i-1.0) * x * result_1 - (i+m-1.0)*result) / (i-m);
1065 template <
class REAL>
1080 template <
class REAL>
1088 bool invert =
false;
1102 rem = NumericTraits<REAL>::one();
1118 template <
class REAL>
1126 template <
class REAL>
1127 REAL gammaImpl(REAL x)
1129 int i, k, m, ix = (int)x;
1130 double ga = 0.0, gr = 0.0, r = 0.0, z = 0.0;
1132 static double g[] = {
1135 -0.6558780715202538,
1136 -0.420026350340952e-1,
1138 -0.421977345555443e-1,
1139 -0.9621971527877e-2,
1141 -0.11651675918591e-2,
1142 -0.2152416741149e-3,
1159 vigra_precondition(x <= 171.0,
1160 "gamma(): argument cannot exceed 171.0.");
1167 for (i=2; i<ix; ++i)
1174 vigra_precondition(
false,
1175 "gamma(): gamma function is undefined for 0 and negative integers.");
1185 for (k=1; k<=m; ++k)
1196 for (k=23; k>=0; --k)
1206 ga = -M_PI/(x*ga*
sin_pi(x));
1226 template <
class REAL>
1227 REAL loggammaImpl(REAL x)
1229 vigra_precondition(x > 0.0,
1230 "loggamma(): argument must be positive.");
1232 vigra_precondition(x <= 1.0e307,
1233 "loggamma(): argument must not exceed 1e307.");
1237 if (x < 4.2351647362715017e-22)
1241 else if ((x == 2.0) || (x == 1.0))
1247 static const double a[] = { 7.72156649015328655494e-02,
1248 3.22467033424113591611e-01,
1249 6.73523010531292681824e-02,
1250 2.05808084325167332806e-02,
1251 7.38555086081402883957e-03,
1252 2.89051383673415629091e-03,
1253 1.19270763183362067845e-03,
1254 5.10069792153511336608e-04,
1255 2.20862790713908385557e-04,
1256 1.08011567247583939954e-04,
1257 2.52144565451257326939e-05,
1258 4.48640949618915160150e-05 };
1259 static const double t[] = { 4.83836122723810047042e-01,
1260 -1.47587722994593911752e-01,
1261 6.46249402391333854778e-02,
1262 -3.27885410759859649565e-02,
1263 1.79706750811820387126e-02,
1264 -1.03142241298341437450e-02,
1265 6.10053870246291332635e-03,
1266 -3.68452016781138256760e-03,
1267 2.25964780900612472250e-03,
1268 -1.40346469989232843813e-03,
1269 8.81081882437654011382e-04,
1270 -5.38595305356740546715e-04,
1271 3.15632070903625950361e-04,
1272 -3.12754168375120860518e-04,
1273 3.35529192635519073543e-04 };
1274 static const double u[] = { -7.72156649015328655494e-02,
1275 6.32827064025093366517e-01,
1276 1.45492250137234768737e+00,
1277 9.77717527963372745603e-01,
1278 2.28963728064692451092e-01,
1279 1.33810918536787660377e-02 };
1280 static const double v[] = { 0.0,
1281 2.45597793713041134822e+00,
1282 2.12848976379893395361e+00,
1283 7.69285150456672783825e-01,
1284 1.04222645593369134254e-01,
1285 3.21709242282423911810e-03 };
1286 static const double tc = 1.46163214496836224576e+00;
1287 static const double tf = -1.21486290535849611461e-01;
1288 static const double tt = -3.63867699703950536541e-18;
1296 double p1 = a[0]+z*(a[2]+z*(a[4]+z*(a[6]+z*(a[8]+z*a[10]))));
1297 double p2 = z*(a[1]+z*(a[3]+z*(a[5]+z*(a[7]+z*(a[9]+z*a[11])))));
1301 else if (x >= 0.23164)
1303 double y = x-(tc-1.0);
1306 double p1 = t[0]+w*(t[3]+w*(t[6]+w*(t[9] +w*t[12])));
1307 double p2 = t[1]+w*(t[4]+w*(t[7]+w*(t[10]+w*t[13])));
1308 double p3 = t[2]+w*(t[5]+w*(t[8]+w*(t[11]+w*t[14])));
1309 double p = z*p1-(tt-w*(p2+y*p3));
1315 double p1 = y*(u[0]+y*(u[1]+y*(u[2]+y*(u[3]+y*(u[4]+y*u[5])))));
1316 double p2 = 1.0+y*(v[1]+y*(v[2]+y*(v[3]+y*(v[4]+y*v[5]))));
1317 res += (-0.5*y + p1/p2);
1327 double p1 = a[0]+z*(a[2]+z*(a[4]+z*(a[6]+z*(a[8]+z*a[10]))));
1328 double p2 = z*(a[1]+z*(a[3]+z*(a[5]+z*(a[7]+z*(a[9]+z*a[11])))));
1332 else if(x >= 1.23164)
1337 double p1 = t[0]+w*(t[3]+w*(t[6]+w*(t[9] +w*t[12])));
1338 double p2 = t[1]+w*(t[4]+w*(t[7]+w*(t[10]+w*t[13])));
1339 double p3 = t[2]+w*(t[5]+w*(t[8]+w*(t[11]+w*t[14])));
1340 double p = z*p1-(tt-w*(p2+y*p3));
1346 double p1 = y*(u[0]+y*(u[1]+y*(u[2]+y*(u[3]+y*(u[4]+y*u[5])))));
1347 double p2 = 1.0+y*(v[1]+y*(v[2]+y*(v[3]+y*(v[4]+y*v[5]))));
1348 res += (-0.5*y + p1/p2);
1354 static const double s[] = { -7.72156649015328655494e-02,
1355 2.14982415960608852501e-01,
1356 3.25778796408930981787e-01,
1357 1.46350472652464452805e-01,
1358 2.66422703033638609560e-02,
1359 1.84028451407337715652e-03,
1360 3.19475326584100867617e-05 };
1361 static const double r[] = { 0.0,
1362 1.39200533467621045958e+00,
1363 7.21935547567138069525e-01,
1364 1.71933865632803078993e-01,
1365 1.86459191715652901344e-02,
1366 7.77942496381893596434e-04,
1367 7.32668430744625636189e-06 };
1370 double p = y*(s[0]+y*(s[1]+y*(s[2]+y*(s[3]+y*(s[4]+y*(s[5]+y*s[6]))))));
1371 double q = 1.0+y*(r[1]+y*(r[2]+y*(r[3]+y*(r[4]+y*(r[5]+y*r[6])))));
1381 else if (x < 2.8823037615171174e+17)
1383 static const double w[] = { 4.18938533204672725052e-01,
1384 8.33333333333329678849e-02,
1385 -2.77777777728775536470e-03,
1386 7.93650558643019558500e-04,
1387 -5.95187557450339963135e-04,
1388 8.36339918996282139126e-04,
1389 -1.63092934096575273989e-03 };
1393 double yy = w[0]+z*(w[1]+y*(w[2]+y*(w[3]+y*(w[4]+y*(w[5]+y*w[6])))));
1394 res = (x-0.5)*(t-1.0)+yy;
1420 return detail::gammaImpl(x);
1436 return detail::loggammaImpl(x);
1445 FPT safeFloatDivision( FPT f1, FPT f2 )
1449 : (f2 > NumericTraits<FPT>::one() && f1 < f2 * NumericTraits<FPT>::smallestPositive()) ||
1450 f1 == NumericTraits<FPT>::zero()
1451 ? NumericTraits<FPT>::zero()
1467 template <
class T1,
class T2>
1471 typedef typename PromoteTraits<T1, T2>::Promote T;
1473 return VIGRA_CSTD::fabs(r) <= epsilon;
1475 return VIGRA_CSTD::fabs(l) <= epsilon;
1476 T diff = VIGRA_CSTD::fabs( l - r );
1477 T d1 = detail::safeFloatDivision<T>( diff, VIGRA_CSTD::fabs( r ) );
1478 T d2 = detail::safeFloatDivision<T>( diff, VIGRA_CSTD::fabs( l ) );
1480 return (d1 <= epsilon && d2 <= epsilon);
1483 template <
class T1,
class T2>
1486 typedef typename PromoteTraits<T1, T2>::Promote T;