float GetProgressForBidirectedAlgo(m2::PointD const & point, m2::PointD const & target)
{
if (target == m_finalPoint)
m_forwardDistance = MercatorBounds::DistanceOnEarth(point, target);
else if (target == m_startPoint)
m_backwardDistance = MercatorBounds::DistanceOnEarth(point, target);
else
ASSERT(false, ());
return CheckConstraints(2.0 - (m_forwardDistance + m_backwardDistance) / m_initialDistance);
}
float GetProgressForDirectedAlgo(m2::PointD const & point)
{
return CheckConstraints(
1.0 - MercatorBounds::DistanceOnEarth(point, m_finalPoint) / m_initialDistance);
}
unsigned int MethodGear<ODESystemKernel>::FindCorrection(const double t, const Eigen::VectorXd& errorWeights)
{
// Current solution to be updated
vb_ = v_[0];
// Check Constraints
CheckConstraints(vb_);
// Call the equations
this->Equations(vb_, t, f_);
numberOfFunctionCalls_++;
// Force recalculation of Jacobian matrix (every maxIterationsJacobian_)
if (numberOfSteps_ >= stepOfLastJacobian_ + maxIterationsJacobian_)
jacobianStatus_ = JACOBIAN_STATUS_HAS_TO_BE_CHANGED;
if (jacobianStatus_ == JACOBIAN_STATUS_HAS_TO_BE_CHANGED)
{
// The Jacobian is evaluated at the current step
stepOfLastJacobian_ = numberOfSteps_;
// Evaluation of the Jacobian matrix
switch (jacobianType_)
{
case JACOBIAN_TYPE_CONST:
jacobianStatus_ = JACOBIAN_STATUS_OK;
break;
case JACOBIAN_TYPE_USERDEFINED:
jacobianStatus_ = JACOBIAN_STATUS_MODIFIED;
this->UserDefinedJacobian(vb_, t);
numberOfJacobians_++;
factorizationStatus_ = MATRIX_HAS_TO_BE_FACTORIZED;
break;
case JACOBIAN_TYPE_NUMERICAL:
jacobianStatus_ = JACOBIAN_STATUS_MODIFIED;
this->NumericalJacobian(vb_, t, f_, h_, errorWeights, max_constraints_, max_values_);
numberOfJacobians_++;
factorizationStatus_ = MATRIX_HAS_TO_BE_FACTORIZED;
break;
}
}
// The hr0 value is used to build the G matrix (see Eq. 29.214)
const double hr0 = h_*r_[0](p_ - 1);
// If the order was changed, the matrix is factorized (of course!)
if (factorizationStatus_ == MATRIX_FACTORIZED)
{
if (iterOrder_ == 0)
factorizationStatus_ = MATRIX_HAS_TO_BE_FACTORIZED;
}
// The matrix is assembled and factorized
if (factorizationStatus_ == MATRIX_HAS_TO_BE_FACTORIZED)
{
stepOfLastFactorization_ = numberOfSteps_;
numberOfMatrixFactorizations_++;
this->BuildAndFactorizeMatrixG(hr0);
factorizationStatus_ = MATRIX_FACTORIZED;
}
// The firs iteration of the Newton's method is performed
// The first guess value is assumed b=0, which means that the following linear system is solved: G*d = -v1+h*f
// The solution d, since b=0 on first guess, is equal to the new b
{
deltab_ = f_*h_;
deltab_ -= v_[1];
this->SolveLinearSystem(deltab_);
numberOfLinearSystemSolutions_++;
b_ = deltab_;
}
// Newton's iterations
double oldCorrectionControl;
for (unsigned int k = 1; k <= maxConvergenceIterations_; k++)
{
// Estimation of the error
const double correctionControl = r_[0](p_ - 1)*OpenSMOKE::ErrorControl(deltab_, errorWeights);
// If the error is sufficiently small, the convergence is ok
if (correctionControl < 1.)
{
convergenceStatus_ = CONVERGENCE_STATUS_OK;
return k;
}
// If the new error is 2 times larger than the previous one, it is better to stop the Newton's method
// and try to restart with a smaller step
if (k > 1 && correctionControl > 2.*oldCorrectionControl)
{
convergenceStatus_ = CONVERGENCE_STATUS_FAILURE;
return k;
}
// Store the error
oldCorrectionControl = correctionControl;
// Update the solution by adding the vector b: v0 = v0+r0*b
va_ = b_*r_[0](p_ - 1);
va_ += v_[0];
// Check constraints for minimum and maximum values
{
// Minimum constraints
if (min_constraints_ == true)
{
for (unsigned int j = 0; j < this->ne_; j++)
if (va_(j) < min_values_(j))
{
va_(j) = min_values_(j);
b_(j) = (min_values_(j) - z_[0](j)) / r_[0](p_ - 1);
}
}
// Maximum constraints
if (max_constraints_ == true)
{
for (unsigned int j = 0; j < this->ne_; j++)
if (va_(j) > max_values_(j))
{
va_(j) = max_values_(j);
b_(j) = (max_values_(j) - z_[0](j)) / r_[0](p_ - 1);
}
}
}
// Call the equations
this->Equations(va_, t, f_);
numberOfFunctionCalls_++;
// Apply the Newton's iteration
{
deltab_ = f_*h_;
deltab_ -= v_[1];
deltab_ -= b_;
this->SolveLinearSystem(deltab_);
numberOfLinearSystemSolutions_++;
b_ += deltab_;
}
}
convergenceStatus_ = CONVERGENCE_STATUS_FAILURE;
return maxConvergenceIterations_;
}
