Array<double,2> NumericalJacobian<F>::Jacobian(Array<double,1>& x)
{
int i,j;
Array<double,2> result(f.retdim(),f.argdim());
Array<double,1> fx(f(x).copy());
Array<double,1> dx(f.argdim());
dx = 0.0;
for (j=0; j<f.argdim(); j++) {
dx(j) = d;
Array<double,1> xdx(x + dx);
Array<double,1> fxdx(f(xdx).copy());
for (i=0; i<f.retdim(); i++) result(i,j) = (fxdx(i)-fx(i))/d;
dx(j) = 0.0;
}
return result;
}
bool ChIntegrableIIorder::StateSolveA(ChStateDelta& Dvdt, // result: computed a for a=dv/dt
ChVectorDynamic<>& L, // result: computed lagrangian multipliers, if any
const ChState& x, // current state, x
const ChStateDelta& v, // current state, v
const double T, // current time T
const double dt, // timestep (if needed)
bool force_state_scatter // if false, x,v and T are not scattered to the system
) {
if (force_state_scatter)
StateScatter(x, v, T);
ChVectorDynamic<> R(GetNcoords_v());
ChVectorDynamic<> Qc(GetNconstr());
const double Delta = 1e-6;
LoadResidual_F(R, 1.0);
LoadConstraint_C(Qc, -2.0 / (Delta * Delta));
// numerical differentiation to get the Qc term in constraints
ChStateDelta dx(v);
dx *= Delta;
ChState xdx(x.GetRows(), this);
StateIncrement(xdx, x, dx);
StateScatter(xdx, v, T + Delta);
LoadConstraint_C(Qc, 1.0 / (Delta * Delta));
StateIncrement(xdx, x, -dx);
StateScatter(xdx, v, T - Delta);
LoadConstraint_C(Qc, 1.0 / (Delta * Delta));
StateScatter(x, v, T); // back to original state
bool success = StateSolveCorrection(Dvdt, L, R, Qc, 1.0, 0, 0, x, v, T, false, true);
return success;
}
