1 | #define chkder_log10e 0.43429448190325182765
|
---|
2 | #define chkder_factor 100.
|
---|
3 |
|
---|
4 | namespace Eigen {
|
---|
5 |
|
---|
6 | namespace internal {
|
---|
7 |
|
---|
8 | template<typename Scalar>
|
---|
9 | void chkder(
|
---|
10 | const Matrix< Scalar, Dynamic, 1 > &x,
|
---|
11 | const Matrix< Scalar, Dynamic, 1 > &fvec,
|
---|
12 | const Matrix< Scalar, Dynamic, Dynamic > &fjac,
|
---|
13 | Matrix< Scalar, Dynamic, 1 > &xp,
|
---|
14 | const Matrix< Scalar, Dynamic, 1 > &fvecp,
|
---|
15 | int mode,
|
---|
16 | Matrix< Scalar, Dynamic, 1 > &err
|
---|
17 | )
|
---|
18 | {
|
---|
19 | using std::sqrt;
|
---|
20 | using std::abs;
|
---|
21 | using std::log;
|
---|
22 |
|
---|
23 | typedef DenseIndex Index;
|
---|
24 |
|
---|
25 | const Scalar eps = sqrt(NumTraits<Scalar>::epsilon());
|
---|
26 | const Scalar epsf = chkder_factor * NumTraits<Scalar>::epsilon();
|
---|
27 | const Scalar epslog = chkder_log10e * log(eps);
|
---|
28 | Scalar temp;
|
---|
29 |
|
---|
30 | const Index m = fvec.size(), n = x.size();
|
---|
31 |
|
---|
32 | if (mode != 2) {
|
---|
33 | /* mode = 1. */
|
---|
34 | xp.resize(n);
|
---|
35 | for (Index j = 0; j < n; ++j) {
|
---|
36 | temp = eps * abs(x[j]);
|
---|
37 | if (temp == 0.)
|
---|
38 | temp = eps;
|
---|
39 | xp[j] = x[j] + temp;
|
---|
40 | }
|
---|
41 | }
|
---|
42 | else {
|
---|
43 | /* mode = 2. */
|
---|
44 | err.setZero(m);
|
---|
45 | for (Index j = 0; j < n; ++j) {
|
---|
46 | temp = abs(x[j]);
|
---|
47 | if (temp == 0.)
|
---|
48 | temp = 1.;
|
---|
49 | err += temp * fjac.col(j);
|
---|
50 | }
|
---|
51 | for (Index i = 0; i < m; ++i) {
|
---|
52 | temp = 1.;
|
---|
53 | if (fvec[i] != 0. && fvecp[i] != 0. && abs(fvecp[i] - fvec[i]) >= epsf * abs(fvec[i]))
|
---|
54 | temp = eps * abs((fvecp[i] - fvec[i]) / eps - err[i]) / (abs(fvec[i]) + abs(fvecp[i]));
|
---|
55 | err[i] = 1.;
|
---|
56 | if (temp > NumTraits<Scalar>::epsilon() && temp < eps)
|
---|
57 | err[i] = (chkder_log10e * log(temp) - epslog) / epslog;
|
---|
58 | if (temp >= eps)
|
---|
59 | err[i] = 0.;
|
---|
60 | }
|
---|
61 | }
|
---|
62 | }
|
---|
63 |
|
---|
64 | } // end namespace internal
|
---|
65 |
|
---|
66 | } // end namespace Eigen
|
---|