176baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman#define chkder_log10e 0.43429448190325182765 276baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman#define chkder_factor 100. 376baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman 476baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkermannamespace Eigen { 54dc8a2aec63e4fb5ee2688544c4de323ed5de3efWichert Akkerman 676baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkermannamespace internal { 776baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman 876baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkermantemplate<typename Scalar> 976baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkermanvoid chkder( 1076baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman const Matrix< Scalar, Dynamic, 1 > &x, 1176baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman const Matrix< Scalar, Dynamic, 1 > &fvec, 1276baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman const Matrix< Scalar, Dynamic, Dynamic > &fjac, 1376baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman Matrix< Scalar, Dynamic, 1 > &xp, 1476baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman const Matrix< Scalar, Dynamic, 1 > &fvecp, 1576baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman int mode, 1676baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman Matrix< Scalar, Dynamic, 1 > &err 1776baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman ) 1876baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman{ 1976baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman using std::sqrt; 2076baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman using std::abs; 2176baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman using std::log; 2276baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman 2376baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman typedef DenseIndex Index; 2476baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman 2576baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman const Scalar eps = sqrt(NumTraits<Scalar>::epsilon()); 2676baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman const Scalar epsf = chkder_factor * NumTraits<Scalar>::epsilon(); 2776baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman const Scalar epslog = chkder_log10e * log(eps); 2876baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman Scalar temp; 2976baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman 3076baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman const Index m = fvec.size(), n = x.size(); 3176baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman 3276baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman if (mode != 2) { 3376baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman /* mode = 1. */ 3476baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman xp.resize(n); 3576baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman for (Index j = 0; j < n; ++j) { 360ed617bd66624cec6138102545d73b2e2346f1f6Dmitry V. Levin temp = eps * abs(x[j]); 3776baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman if (temp == 0.) 381945ccc3fbd5b56008c4a6b0cdd4611616201675Denys Vlasenko temp = eps; 39b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin xp[j] = x[j] + temp; 4076baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman } 41b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin } 4276baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman else { 43b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin /* mode = 2. */ 44b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin err.setZero(m); 451945ccc3fbd5b56008c4a6b0cdd4611616201675Denys Vlasenko for (Index j = 0; j < n; ++j) { 46b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin temp = abs(x[j]); 47b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin if (temp == 0.) 4876baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman temp = 1.; 49b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin err += temp * fjac.col(j); 5076baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman } 5176baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman for (Index i = 0; i < m; ++i) { 5276baf7c9f6dd61a15524ad43c1b690c252cf5b7Wichert Akkerman temp = 1.; 53ee81c8a57177ad6db9e5fae1666a7a996f90f159Andreas Schwab if (fvec[i] != 0. && fvecp[i] != 0. && abs(fvecp[i] - fvec[i]) >= epsf * abs(fvec[i])) 54b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin temp = eps * abs((fvecp[i] - fvec[i]) / eps - err[i]) / (abs(fvec[i]) + abs(fvecp[i])); 55ee81c8a57177ad6db9e5fae1666a7a996f90f159Andreas Schwab err[i] = 1.; 56b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin if (temp > NumTraits<Scalar>::epsilon() && temp < eps) 57b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin err[i] = (chkder_log10e * log(temp) - epslog) / epslog; 58b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin if (temp >= eps) 59ee81c8a57177ad6db9e5fae1666a7a996f90f159Andreas Schwab err[i] = 0.; 60ee81c8a57177ad6db9e5fae1666a7a996f90f159Andreas Schwab } 61b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin } 62b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin} 63ee81c8a57177ad6db9e5fae1666a7a996f90f159Andreas Schwab 64b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin} // end namespace internal 65b468f2320a8b8e245b481c78b58431ac56505849Dmitry V. Levin 66ee81c8a57177ad6db9e5fae1666a7a996f90f159Andreas Schwab} // end namespace Eigen 67ee81c8a57177ad6db9e5fae1666a7a996f90f159Andreas Schwab