1 | namespace Eigen {
|
---|
2 |
|
---|
3 | namespace internal {
|
---|
4 |
|
---|
5 | template<typename FunctorType, typename Scalar>
|
---|
6 | DenseIndex fdjac1(
|
---|
7 | const FunctorType &Functor,
|
---|
8 | Matrix< Scalar, Dynamic, 1 > &x,
|
---|
9 | Matrix< Scalar, Dynamic, 1 > &fvec,
|
---|
10 | Matrix< Scalar, Dynamic, Dynamic > &fjac,
|
---|
11 | DenseIndex ml, DenseIndex mu,
|
---|
12 | Scalar epsfcn)
|
---|
13 | {
|
---|
14 | using std::sqrt;
|
---|
15 | using std::abs;
|
---|
16 |
|
---|
17 | typedef DenseIndex Index;
|
---|
18 |
|
---|
19 | /* Local variables */
|
---|
20 | Scalar h;
|
---|
21 | Index j, k;
|
---|
22 | Scalar eps, temp;
|
---|
23 | Index msum;
|
---|
24 | int iflag;
|
---|
25 | Index start, length;
|
---|
26 |
|
---|
27 | /* Function Body */
|
---|
28 | const Scalar epsmch = NumTraits<Scalar>::epsilon();
|
---|
29 | const Index n = x.size();
|
---|
30 | eigen_assert(fvec.size()==n);
|
---|
31 | Matrix< Scalar, Dynamic, 1 > wa1(n);
|
---|
32 | Matrix< Scalar, Dynamic, 1 > wa2(n);
|
---|
33 |
|
---|
34 | eps = sqrt((std::max)(epsfcn,epsmch));
|
---|
35 | msum = ml + mu + 1;
|
---|
36 | if (msum >= n) {
|
---|
37 | /* computation of dense approximate jacobian. */
|
---|
38 | for (j = 0; j < n; ++j) {
|
---|
39 | temp = x[j];
|
---|
40 | h = eps * abs(temp);
|
---|
41 | if (h == 0.)
|
---|
42 | h = eps;
|
---|
43 | x[j] = temp + h;
|
---|
44 | iflag = Functor(x, wa1);
|
---|
45 | if (iflag < 0)
|
---|
46 | return iflag;
|
---|
47 | x[j] = temp;
|
---|
48 | fjac.col(j) = (wa1-fvec)/h;
|
---|
49 | }
|
---|
50 |
|
---|
51 | }else {
|
---|
52 | /* computation of banded approximate jacobian. */
|
---|
53 | for (k = 0; k < msum; ++k) {
|
---|
54 | for (j = k; (msum<0) ? (j>n): (j<n); j += msum) {
|
---|
55 | wa2[j] = x[j];
|
---|
56 | h = eps * abs(wa2[j]);
|
---|
57 | if (h == 0.) h = eps;
|
---|
58 | x[j] = wa2[j] + h;
|
---|
59 | }
|
---|
60 | iflag = Functor(x, wa1);
|
---|
61 | if (iflag < 0)
|
---|
62 | return iflag;
|
---|
63 | for (j = k; (msum<0) ? (j>n): (j<n); j += msum) {
|
---|
64 | x[j] = wa2[j];
|
---|
65 | h = eps * abs(wa2[j]);
|
---|
66 | if (h == 0.) h = eps;
|
---|
67 | fjac.col(j).setZero();
|
---|
68 | start = std::max<Index>(0,j-mu);
|
---|
69 | length = (std::min)(n-1, j+ml) - start + 1;
|
---|
70 | fjac.col(j).segment(start, length) = ( wa1.segment(start, length)-fvec.segment(start, length))/h;
|
---|
71 | }
|
---|
72 | }
|
---|
73 | }
|
---|
74 | return 0;
|
---|
75 | }
|
---|
76 |
|
---|
77 | } // end namespace internal
|
---|
78 |
|
---|
79 | } // end namespace Eigen
|
---|