[136] | 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
|
---|