typename traj_simu<scalar_type>::state_jump_results traj_simu<scalar_type>::state_jump_intervals_derivative(
const state_functor & f, const state_matrix_functor & Jac,
const state & xinit, scalar_type tinit, scalar_type tmax,
std::vector<typename lin_constraint_system::const_ptr> & guards,
typename lin_constraint_system::const_ptr orig_invariant) const {
// reorder_state_functor<scalar_type> f= reorder_state_functor<scalar_type>(f_i)
assert(xinit.domain()==f.map(xinit).domain());
using namespace math;
using namespace numeric;
unsigned int gn = guards.size();
state_jump_results res;
scalar_type tdeb_k[gn]; //if jump_ok[i] then tdeb_k[i] time the root occurred
unsigned int tdeb_index[gn]; // GF: the index in the current trajectory?
bool jump_ok[gn]; //is the jump OK at current time ?
scalar_type xp; //scalar used for initialisation
state xinitprim;
scalar_type zero(0);
ode_solver solver;
// ---------------------------------------------------------------------
// prepare the affine maps corresponding to the root functions
// ---------------------------------------------------------------------
// every guard consists of a set of constraints.
// every constraint is considered a root function.
// the state is in the guard if all root functions are nonpositive.
// z_map[0] is the invariant
// z_map[1] ... z_map[gn] are the guards
std::vector<typename affine_map::const_ptr> z_map(gn + 1);
// @todo choose a domain for the solver and stick with it (use it everywhere)
// current : domain of xinit chosen.
// to bring lin_constraint_system to domain of solver
// + convert to affine map form for functor
matrix<scalar_type> A;
vector<scalar_type> b;
positional_vdomain dom;
positional_vdomain codom;
// Note: The canonic matrix form is Ax<=b, to the associated root function
// must be the affine map Ax-b==0.
lin_constraint_system invariant(*orig_invariant);
invariant.reorder(xinit.domain());
canonic_matrix_form(A, b, dom, invariant);
codom = positional_vdomain();
for (int k = 0; k < invariant.size(); k++)
codom.add_variable(variable("traj_simu_dummy" + to_string(k)));
z_map[0] = typename affine_map::ptr(
new affine_map(vdom_matrix<scalar_type> (codom, dom, A),
state(codom, -b)));
// Reorder the guard constraints and prepare the root functions
for (int i = 0; i < gn; i++) {
typename lin_constraint_system::ptr reordered_guard =
typename lin_constraint_system::ptr(
new lin_constraint_system(*(guards[i])));
reordered_guard->reorder(xinit.domain());
guards[i] = reordered_guard;
canonic_matrix_form(A, b, dom, *(guards[i]));
codom = positional_vdomain();
for (int k = 0; k < guards[i]->size(); k++)
codom.add_variable(variable("traj_simu_dummy" + to_string(k)));
z_map[i + 1] = typename affine_map::ptr(
new affine_map(vdom_matrix<scalar_type> (codom, dom, A),
state(codom, -b)));
}
// ---------------------------------------------------------------------
// checking invariant + which guard is satified and which is'nt.
// guards are assumed of the form ax<=b (calcul = ax - b<=O)
// verification of invariant
bool invariant_satisfied = true;
bool invariant_atomic = false;
// Check for each constraint in the invariant whether it is satisfied or not
for (typename lin_constraint_system::const_iterator it = invariant.begin(); invariant_satisfied
&& it != invariant.end(); ++it) {
// Check whether x violates the constraint or is on the border
xp = it->normal_eval(xinit);
// For better numerics, don't compare xp to zero, but
// xp-inh_coeff to -inh_coeff. That way we take into account the relative error
scalar_type inh_coeff = it->get_canonic_inh_coeff();
if (definitely(is_GT(xp, -inh_coeff))) {
// The invariant constraint is violated.
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "inv violated with " << xp << " > " << -inh_coeff;
invariant_satisfied = false;
} else if (!definitely(is_LT(xp, -inh_coeff))) {
// The invariant constraint is on the border.
// Check the derivative to see the whether the invariant function is increasing or decreasing.
// The derivative is f(x). If a^Tf(x)>0, then the function is increasing, and so the invariant is atomic.
scalar_type dxp = it->normal_eval(f.map(xinit));
if (definitely(is_GT(dxp, scalar_type(0)))) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "inv from below, so atomic " << "\n";
invariant_atomic = true;
}
// Note: for now we don't check whether the derivative is zero,
// which could cause problems with the solver
}
}
// If the invariant is violated from the start, return an empty trajectory
if (!invariant_satisfied) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< ("Inv false at beginning !!!\n");
res.traj = trajectory(xinit.domain());
res.ji = std::vector<interval_list>(gn);
res.horizont_reached = false;
return res;
}
if (invariant_atomic)
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< ("Inv atomic at beginning !!!\n");
// We found our first point, let's add it to the result trajectory
res.traj = trajectory(xinit.domain(), tinit, xinit.get_vector());
res.ji = std::vector<interval_list>(gn);
// currently sought roots
// if z[i] < 0 then all constraints of inv/guard need to be evaluated
// otherwise only the constraint with index z[i]
std::vector<int> z(gn + 1);
// for the guard i, the root function values of the constraints in the
// interval [index_beg[i+1],index_end[i+1]-1] need to be verified
// These indices correspond to the position in the output vector of
// z_functor.
int index_beg[gn + 1];
int index_end[gn + 1];
// all constraints of the invariant need to be tracked
z[0] = -1;
index_beg[0] = 0;
index_end[0] = invariant.size();
// analyze all of the guards on their status, and find which
// constraints need to be evaluated
for (int i = 0; i < gn; i++) {
scalar_type max_val(0);
bool guard_satisfied = true;
bool guard_atomic = false;
int position = 0;
int pos_max = -1;
int pos_atom = -1;
// find out if the guard is satisfied, atomic, and the "farthest" constraint
for (typename lin_constraint_system::const_iterator it =
guards[i]->begin(); it != guards[i]->end(); ++it) {
scalar_type xp = it->normal_eval(xinit);
scalar_type inh_coeff = it->get_canonic_inh_coeff();
if (pos_max < 0 || max_val < xp + inh_coeff) {
pos_max = position;
max_val = xp + inh_coeff;
}
if (guard_satisfied) {
if (definitely(is_GT(xp, -inh_coeff))) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "guard " << i << " constraint " << *it << " violated by " << xp + inh_coeff << "\n";
guard_satisfied = false;
} else if (!definitely(is_LT(xp, -inh_coeff))) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "guard " << i << " constraint " << *it << " zero at " << xp + inh_coeff << "\n";
// Check the derivative to see the whether the guard function is increasing or decreasing.
// The derivative is f(x). If a^Tf(x)>0, then the function is increasing.
scalar_type dxp = it->normal_eval(f.map(xinit));
// if(approx_comparator::is_maybe_equal(xp-inh_coeff,-inh_coeff) )
// std::runtime_error("Root found with rooted derivative, don't know how to deal with !");
if (definitely(is_GT(dxp, scalar_type(0)))) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "from below, so atomic " << "\n";
guard_atomic = true;
pos_atom = position;
}
}
}
position++;
}
if (guard_satisfied && guard_atomic) {
jump_interval temp;
temp.interv.set_lower(tinit);
temp.interv.set_upper(tinit);
temp.index_low = 0;
temp.index_high = 0;
res.ji[i].push_back(temp);
z[i + 1] = pos_atom;
jump_ok[i] = false;
// register the position in the output vector
index_beg[i + 1] = index_end[i]; // continue after the index of the previous constraint
index_end[i + 1] = index_beg[i + 1] + 1;
} else if (guard_satisfied) {
z[i + 1] = -1;
jump_ok[i] = true;
tdeb_k[i] = tinit;
tdeb_index[i] = 0;
// register the position in the output vector
index_beg[i + 1] = index_end[i];
index_end[i + 1] = index_beg[i + 1] + guards[i]->size(); // reserve room for all constraints
} else {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "guard " << i << " violated, adding constraint " << pos_max << "\n";
z[i + 1] = pos_max;
jump_ok[i] = false;
// register the position in the output vector
index_beg[i + 1] = index_end[i]; // continue after the index of the previous constraint
index_end[i + 1] = index_beg[i + 1] + 1;
}
}
//lin_constraint_state_functor z_functor(z,(const math::state_functor<scalar_type> *)&f,indices[(gn<<1)+1]);
affine_map_vector_state_vector_functor z_functor(z_map, z, index_end[gn]);
solver.init_rootf(xinit, tinit, f, z_functor, par); // init solver
solver.specify_jacobian(Jac);
// end of init
typename ode_solver::rootf_result interm_result;
typedef typename ode_solver::rootf_result::root_status root_status;
root_status rsNO_ROOT = ode_solver::rootf_result::NO_ROOT;
root_status rsFROM_BELOW = ode_solver::rootf_result::FROM_BELOW;
root_status rsFROM_ABOVE = ode_solver::rootf_result::FROM_ABOVE;
//algorithm body
res.horizont_reached = false;
bool inv = !invariant_atomic;
scalar_type t = tinit;
bool at_least_one_from_below;
bool solver_crash = false;
scalar_type tmax_next;
// inv && tmax>=t
// while(inv && ( tmax<0 || !maybe(approx_comparator::is_LE(tmax,t) ) ) && !solver_crash )
while (inv && (tmax < 0 || definitely(approx_comparator::is_LE(t, tmax)))
&& !solver_crash) {
tmax_next = (tmax < 0) ? (2 * t + 1) : tmax;
z_functor.update_approx_size(index_end[gn]);
LOGGER_OS(DEBUG7,"state_jump_intervals_derivative")
<< "running solver with "<< "guards: " << z_map << "roots: " << z;
try {
solver.change_rootf(z_functor);
interm_result = solver.solve_firstrootf(tmax_next);
res.traj.insert(interm_result.traj);
t = interm_result.stop_time;
} catch (std::exception& e) {
basic_warning("state_jump_intervals_derivative",
"ODE solver crashed, result is incomplete.",
basic_warning::INCOMPLETE_OUTPUT);
solver_crash = true;
IFLOGGER(DEBUG4) {
LOGGER_OS(DEBUG4,"state_jump_intervals_derivative")
<< "guards: " << z_map << "roots: " << z;
throw e;
}
}
if (!(interm_result.root_info.size1() == 0)) {
LOGGER_OS(DEBUG7,"state_jump_intervals_derivative")
<< " stop_state : " << interm_result.stop_state
<< "and inv & g values : " << z_functor.map(
interm_result.stop_state);
{
typename lin_constraint_system::const_iterator it =
invariant.begin();
for (int i = index_beg[0]; i < index_end[0]; i++) {
xp = it->normal_eval(interm_result.stop_state);
scalar_type inh_coeff = it->get_canonic_inh_coeff();
scalar_type dxp = it->normal_eval(
f.map(interm_result.stop_state));
// if(approx_comparator::is_maybe_equal(xp-inh_coeff,-inh_coeff) && approx_comparator::is_maybe_equal(dxp-inh_coeff,-inh_coeff) )
// std::runtime_error("Root found with rooted derivative, don't know how to deal with !");
// The state is outside the invariant, or on the border of the invariant with derivative pointing outside
if (definitely(is_GT(xp, -inh_coeff)) || (is_MEQ(xp,
-inh_coeff) && definitely(
is_GT(dxp, scalar_type(0))))) {
inv = false;
}
++it;
}
}
for (int i = 0; i < gn; i++) //for all guards
{
typename lin_constraint_system::const_iterator it =
guards[i]->begin();
if (jump_ok[i]) {
// the state was previously inside the guard
int j, git, posgit;
git = 0; // index counter
posgit = -1; // index of the positive constraint
for (j = index_beg[i + 1]; jump_ok[i] && j < index_end[i
+ 1]; ++j) {
// here we could use j to access the output vector of z_functor.
// for now, we re-evaluate the constraints
xp = it->normal_eval(interm_result.stop_state);
scalar_type inh_coeff = it->get_canonic_inh_coeff();
scalar_type dxp = it->normal_eval(
f.map(interm_result.stop_state));
// if(approx_comparator::is_maybe_equal(xp-inh_coeff,-inh_coeff) && approx_comparator::is_maybe_equal(dxp-inh_coeff,-inh_coeff) )
// std::runtime_error("Root found with rooted derivative, don't know how to deal with !");
// the state is on the border of the guard, and the derivative is pointing outside
if (definitely(is_GT(xp, -inh_coeff)) || (is_MEQ(xp,
-inh_coeff) && definitely(
is_GT(dxp, scalar_type(0))))) {
jump_ok[i] = false;
posgit = git;
}
++git;
++it;
}
if (jump_ok[i] == false) // change tested constraint if violation has been found
{
z[i + 1] = posgit;
index_beg[i + 1] = index_end[i];
index_end[i + 1] = index_beg[i + 1] + 1;
jump_interval temp;
temp.interv.set_lower(tdeb_k[i]);
temp.interv.set_upper(t);
temp.index_low = tdeb_index[i];
temp.index_high = res.traj.size() - 1;
res.ji[i].push_back(temp);
} else // tested constraints remain the same, just updating indexes
{
index_beg[i + 1] = index_end[i];
index_end[i + 1] = index_beg[i + 1] + guards[i]->size();
}
} else {
// the state was outside of this guard
// check if the constraint is still violated
// move iterator to the constraint with index z[i+1]
typename lin_constraint_system::const_iterator it =
guards[i]->begin();
for (int kk = 0; kk < z[i + 1]; ++kk) {
++it;
}
scalar_type xp = it->normal_eval(interm_result.stop_state);
scalar_type inh_coeff = it->get_canonic_inh_coeff();
// if not violated, restart
if (maybe(is_LE(xp, -inh_coeff))) {
scalar_type max_val(0);
bool guard_satisfied = true;
bool guard_atomic = false;
int position = 0;
int pos_max = -1;
int pos_atom = -1;
// find out if the guard is satisfied, atomic, and the "farthest" constraint
for (typename lin_constraint_system::const_iterator it =
guards[i]->begin(); it != guards[i]->end(); ++it) {
scalar_type xp = it->normal_eval(
interm_result.stop_state);
scalar_type inh_coeff = it->get_canonic_inh_coeff();
if (pos_max < 0 || max_val < xp + inh_coeff) {
pos_max = position;
max_val = xp + inh_coeff;
}
if (guard_satisfied) {
if (definitely(is_GT(xp, -inh_coeff))) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "violated by " << xp + inh_coeff
<< "\n";
guard_satisfied = false;
} else if (!definitely(is_LT(xp, -inh_coeff))) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "zero at " << xp + inh_coeff << "\n";
// Check the derivative to see the whether the guard function is increasing or decreasing.
// The derivative is f(x). If a^Tf(x)>0, then the function is increasing.
scalar_type dxp = it->normal_eval(
f.map(interm_result.stop_state));
// if(approx_comparator::is_maybe_equal(xp-inh_coeff,-inh_coeff) )
// std::runtime_error("Root found with rooted derivative, don't know how to deal with !");
if (definitely(is_GT(dxp, scalar_type(0)))) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "from below, so atomic " << "\n";
guard_atomic = true;
pos_atom = position;
}
}
}
position++;
}
// Note: guard_atomic may be true if guard_satisfied is false, because
// guard_satisfied can change after guard_atomic has become true
if (guard_satisfied && guard_atomic) {
jump_interval temp;
temp.interv.set_lower(t);
temp.interv.set_upper(t);
temp.index_low = res.traj.size() - 1;
temp.index_high = temp.index_low;
res.ji[i].push_back(temp);
z[i + 1] = pos_atom;
jump_ok[i] = false;
// register the position in the output vector
index_beg[i + 1] = index_end[i]; // continue after the index of the previous constraint
index_end[i + 1] = index_beg[i + 1] + 1;
} else if (guard_satisfied) {
z[i + 1] = -1;
jump_ok[i] = true;
tdeb_k[i] = t;
tdeb_index[i] = res.traj.size() - 1;
// register the position in the output vector
index_beg[i + 1] = index_end[i];
index_end[i + 1] = index_beg[i + 1]
+ guards[i]->size(); // reserve room for all constraints
} else {
z[i + 1] = pos_max;
jump_ok[i] = false;
// register the position in the output vector
index_beg[i + 1] = index_end[i]; // continue after the index of the previous constraint
index_end[i + 1] = index_beg[i + 1] + 1;
}
} else // tested constraints remain the same, just updating indexes
{
index_beg[i + 1] = index_end[i];
index_end[i + 1] = index_beg[i + 1] + guards[i]->size();
}
} // else: state was outside of guard
} // for all guards
} // if there is a root
}
//if(t>=tmax) res.horizont_reached=true; -->tribool
if ((!(tmax < 0)) && maybe(is_LE(tmax, t)))
res.horizont_reached = true;
for (int i = 0; i < gn; i++) {
if (jump_ok[i]) {
jump_interval temp;
temp.interv.set_lower(tdeb_k[i]);
temp.interv.set_upper(t);
temp.index_low = tdeb_index[i];
temp.index_high = res.traj.size() - 1;
res.ji[i].push_back(temp);
}
}
return res;
}
typename traj_simu<scalar_type>::state_jump_results traj_simu<scalar_type>::state_jump_intervals_solver(
const state_functor & f, const state_matrix_functor & Jac,
const state & xinit, scalar_type tinit, scalar_type tmax,
std::vector<typename lin_constraint_system::const_ptr> & guards,
typename lin_constraint_system::const_ptr invariant) const {
// reorder_state_functor<scalar_type> f = reorder_state_functor<scalar_type> (
// f_i);
//used to store intermediate IVP and Root results returned by the solver
typename ode_solver::rootf_result interm_result;
// typedef for root finding
typedef typename math::ode::ode_solver<scalar_type>::rootf_result::root_matrix root_matrix;
typedef typename ode_solver::rootf_result::root_status root_status;
//solver constants for rootfinding
root_status rsNO_ROOT = ode_solver::rootf_result::NO_ROOT;
root_status rsFROM_BELOW = ode_solver::rootf_result::FROM_BELOW;
root_status rsFROM_ABOVE = ode_solver::rootf_result::FROM_ABOVE;
unsigned int gn = guards.size(); // number of guards
// variables used to store temporary/intermediate results results
jump_interval temp;
int temp_satisfied;
scalar_type xp; //scalar used for initialisation
state xinitprim;
scalar_type zero(0);
//the result of this function
state_jump_results res;
// variables used to determine which guard is satisfied or not, and if it is, since when, or if it's not, haw many
// constraints are not yet satisfied
scalar_type tdeb_k[gn]; //if jump_ok[i] then tdeb_k[i] time the root occurred
unsigned int tdeb_index[gn];
int remaining_to_satisf[gn];
bool jump_ok[gn]; //is the jump OK at current time ?
//variables used to pass constraints of guards (and invariant) to the solver and
//memorize indexes in order to assign results produced by the solver
//to the corresponding guard ( or the invariant)
lin_constraints_vector z(gn + 1); //current sought roots
int indices[2 * (gn + 1)];
//loop variable (iterators)
typename lin_constraint_system::const_iterator it;
typename lin_constraint_system::const_iterator git;
typename lin_constraint_system::const_iterator oit;
//boolean for guards
bool guard_satisfied = true;
bool guard_atomic = false;
// an instance of the SOLVER --> needs an initialisation (performed later)
ode_solver solver;
///checking invariant + which guard is sastified and which is'nt.
///guards are assumed of the form a*x+b<=0
/// Filling z and indices
/// with invariant and guards constraints
// to bring lin_constraint_system to domain of solver
typename lin_constraint_system::ptr reordered_invariant =
typename lin_constraint_system::ptr(
new lin_constraint_system(*invariant));
// @todo choose a domain for the solver and stick with it (use it everywhere)
reordered_invariant->reorder(xinit.domain());
invariant = reordered_invariant;
for (int i = 0; i < gn; i++) {
typename lin_constraint_system::ptr reordered_guard =
typename lin_constraint_system::ptr(
new lin_constraint_system(*guards[i]));
// @todo choose a domain for the solver and stick with it (use it everywhere)
reordered_guard->reorder(xinit.domain());
guards[i] = reordered_guard;
}
z[0].begin = invariant->begin();
z[0].end = invariant->end();
indices[0] = 0;
indices[1] = 2 * invariant->size();
for (int i = 0; i < gn; i++) {
LOGGER_OS(DEBUG7,"state_jump_intervals_solver")
<< "guard " << i << ":\n" << guards;
//setting z for this guard
z[i + 1].begin = guards[i]->begin();
z[i + 1].end = guards[i]->end();
indices[(i + 1) * 2] = indices[(i + 1) * 2 - 1];
indices[(i + 1) * 2 + 1] = indices[(i + 1) * 2] + 2 * guards[i]->size();
}
lin_constraint_state_functor z_functor(z,
(const math::state_functor<scalar_type> *) &f, indices[2 * gn + 1]);
/// solver initialisation
solver.init_rootf(xinit, tinit, f, z_functor, par);
solver.specify_jacobian(Jac);
///do solver adjustment :
/// a little backward then forward integration up to t = tinit in
/// order to gather information on roots of invariants and guards at t = tinit
/// (to detect and handle properly roots at the beginning of the algorithm)
interm_result = solver.solve_firstrootf(tinit - 1);
while (!approx_comparator::is_definitely_strictly_smaller(
interm_result.stop_time, tinit)) {
interm_result = solver.solve_firstrootf(tinit - 1);
}
solver.init_rootf(interm_result.stop_state, interm_result.stop_time, f,
z_functor, par);
while (approx_comparator::is_definitely_strictly_smaller(
interm_result.stop_time, tinit)) {
interm_result = solver.solve_firstrootf(tinit + par.max_timestep);
}
/// if solver has not stopped at t = tinit, there is no root at the beginning of the integration.
if (approx_comparator::is_definitely_strictly_larger(
interm_result.stop_time, tinit) && interm_result.root_info.size1()> 0) {
interm_result.root_info = root_matrix(1, indices[gn * 2 + 1], rsNO_ROOT);
}
///for verfication of invariant
/// sat is true if the invariant is true, atom is true is the invariant is atomic, e.g. only satisfied at t=tinit
bool invariant_satisfied = true;
bool invariant_atomic = false;
/// root_info matrix should not be empty in the next loops
if (interm_result.root_info.size1() == 0)
interm_result.root_info
= root_matrix(1, indices[gn * 2 + 1], rsNO_ROOT);
/// evaluating invariant and then guards values and directions (if roots) at t=tinit
/// Goal is to detect if a guard/ the invariant is satisfied and if it is the case, verify
/// if this situation is atomic, e.g. one of the constraint is rooted and goes up, which
/// means that the constraint set will not be satisfied after t = current time
/// The same sort of check is performed in the main iteration each time the solver stops with a root.
it = invariant->begin();
for (int i = 0; (2 * i) < indices[1]; i++) {
LOGGER_OS(DEBUG7,"state_jump_intervals_solver")
<< "initial check on invariant " << i << ":" << *it << " : ";
// Check whether x violates the constraint or is on the border
if (interm_result.root_info(0, 2 * i) == rsFROM_BELOW) {
invariant_atomic = true;
} else if (interm_result.root_info(0, 2 * i) == rsNO_ROOT) {
xp = lin_constraint_evaluation(*it, xinit);
if (approx_comparator::is_definitely_strictly_pos(xp)) {
invariant_satisfied = false;
} else if (!approx_comparator::is_definitely_strictly_neg(xp)) {
// The invariant constraint is on the border.
// Check the derivative to see the whether the invariant function is increasing or decreasing.
if (interm_result.root_info(0, 2 * i + 1) == rsFROM_BELOW) {
invariant_atomic = true;
}
/*else{
printf(
"WARNING : No root found by solver, but constraint %u is rooted",
i);
}*/
}
}
++it;
}
/// If the invariant is not satisfied at the beginning, no trajectory should be computed and no guards taken
/// (state is invalid !!!)
if (!invariant_satisfied) {
LOGGER_OS(DEBUG5,"state_jump_intervals_safe")
<< "Invariant is not satisfied at the beginning of the simulation !!!";
res.traj = trajectory(xinit.domain());
res.ji = std::vector<interval_list>(gn);
res.horizont_reached = false;
return res;
}
/// If the invariant is not atomic at the beginning, no further trajectory will be computed but guards may be taken.
/// See condition of main loop
if (invariant_atomic)
LOGGER_OS(DEBUG5,"state_jump_intervals_safe")
<< "Inv atomic at beginning !!!";
///now evaluating if guards are satisfied at beginning.
/// the atomic et satisfied booleans play the same role as atom & satisf play for the infariant
res.traj = trajectory(xinit.domain(), tinit, xinit.get_vector());
res.ji = std::vector<interval_list>(gn);
for (int i = 0; i < gn; i++) {
it = guards[i]->begin();
remaining_to_satisf[i] = 0;
guard_atomic = false;
guard_satisfied = true;
for (int j = 0; j < guards[i]->size(); j++) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "initial check on guard " << i << ":" << *it << " : ";
if (interm_result.root_info(0, indices[2 * (i + 1)] + 2 * j)
== rsFROM_BELOW) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "atomic" << "\n";
guard_atomic = true;
remaining_to_satisf[i]++;
} else if (interm_result.root_info(0, indices[2 * (i + 1)] + 2 * j)
== rsNO_ROOT) {
xp = lin_constraint_evaluation(*it, xinit);
scalar_type inh_coeff = it->get_canonic_inh_coeff();
if (definitely(is_GT(xp - inh_coeff, -inh_coeff))) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "violated by " << xp << "\n";
guard_satisfied = false;
remaining_to_satisf[i]++;
} else if (!definitely(is_LT(xp - inh_coeff, -inh_coeff))) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "zero at " << xp << "\n";
// Check the derivative to see the whether the guard function is increasing or decreasing.
// The derivative is f(x). If a^Tf(x)>0, then the function is increasing.
if (interm_result.root_info(0,
indices[2 * (i + 1)] + 2 * j + 1) == rsFROM_BELOW) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "from below, so atomic " << "\n";
guard_atomic = true;
// The constraint is violated for all t'>t, so
// it needs to be added to the constraints remaining to satisfy
remaining_to_satisf[i]++;
}
}
}
++it;
}
if (guard_satisfied && guard_atomic) {
jump_ok[i] = false;
temp.interv.set_lower(tinit);
temp.interv.set_upper(tinit);
temp.index_low = 0;
temp.index_high = 0;
res.ji[i].push_back(temp);
} else if (guard_satisfied) {
jump_ok[i] = true;
tdeb_k[i] = tinit;
tdeb_index[i] = 0;
} else
jump_ok[i] = false;
}
solver.init_rootf(xinit, tinit, f, z_functor, par); // init solver
solver.specify_jacobian(Jac);
//algorithm main loop
res.horizont_reached = false;
bool inv = !invariant_atomic;
scalar_type t = tinit;
scalar_type tmax_next;
scalar_type tprec;
// inv && tmax>=t
while (inv && (tmax < 0 || !maybe(approx_comparator::is_LE(tmax, t)))) {
//TODO find a better solution if tmax < 0
tmax_next = (tmax < 0) ? (2 * t + 1) : tmax;
interm_result = solver.solve_firstrootf(tmax_next);
res.traj.insert(interm_result.traj);
tprec = t;
t = interm_result.stop_time;
if (!(interm_result.root_info.size1() == 0)
&& approx_comparator::is_definitely_strictly_larger(t, tprec)) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "Root infos : NO_ROOT=" << (int) rsNO_ROOT
<< ", FROM_ABOVE=" << (int) rsFROM_ABOVE
<< ", FROM_BELOW=" << (int) rsFROM_BELOW << std::endl
<< "root matrix returned by solver "
<< interm_result.root_info << "and g values : "
<< z_functor.map(interm_result.stop_state);
it = invariant->begin();
for (int i = 0; (2 * i) < indices[1]; i++) {
xp = lin_constraint_evaluation(*it, interm_result.stop_state);
if (interm_result.root_info(0, 2 * i) == rsFROM_BELOW) {
inv = false;
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "invariant rooted, index = " << 2 * i
<< "with value " << xp;
} else LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "invariant not rooted, index = " << 2 * i
<< "with value " << xp;
++it;
}
for (int i = 0; i < gn; i++) //for all guards
{
it = guards[i]->begin();
guard_atomic = false;
temp_satisfied = 0;
for (int j = 0; j < guards[i]->size(); j++) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "check on guard " << i << ", index=" << (indices[2
* (i + 1)] + 2 * j) << " :" << *it << " : ";
// Note: The solver seems to have quite unreliable root info
// (timesteps too big?)
// So let's try using the derivative
bool below = false;
bool above = false;
below = interm_result.root_info(0,
indices[2 * (i + 1)] + 2 * j) == rsFROM_BELOW;
above = interm_result.root_info(0,
indices[2 * (i + 1)] + 2 * j) == rsFROM_ABOVE;
if (below) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "from below, so atomic";
guard_atomic = true;
remaining_to_satisf[i]++;
temp_satisfied++;
} else if (above) {
remaining_to_satisf[i]--;
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "from above, remaining:"
<< remaining_to_satisf[i];
} else {
xp = lin_constraint_evaluation(*it,
interm_result.stop_state);
scalar_type inh_coeff = it->get_canonic_inh_coeff();
double dxp = lin_constraint_evaluation(*it,
f.map(interm_result.stop_state));
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "solver says not root, guard value " << xp
<< ", checking derivative dxp=" << dxp;
if (interm_result.root_info(0,
indices[2 * (i + 1)] + 2 * j + 1)
== rsFROM_BELOW && is_MEQ(xp - inh_coeff,
-inh_coeff)) {
LOGGER_OS(DEBUG7,"state_jump_intervals_safe")
<< "is satisfied";
temp_satisfied++;
}
}
++it;
}
if (jump_ok[i] && remaining_to_satisf[i] != 0) {
temp.interv.set_lower(tdeb_k[i]);
temp.interv.set_upper(t);
temp.index_low = tdeb_index[i];
temp.index_high = res.traj.size() - 1;
res.ji[i].push_back(temp);
jump_ok[i] = false;
} else if (!jump_ok[i] && remaining_to_satisf[i] == 0) {
jump_ok[i] = true;
tdeb_k[i] = t;
tdeb_index[i] = res.traj.size() - 1;
} else if (!jump_ok[i] && remaining_to_satisf[i]
- temp_satisfied == 0) {
temp.interv.set_lower(t);
temp.interv.set_upper(t);
temp.index_low = res.traj.size() - 1;
temp.index_high = temp.index_low;
res.ji[i].push_back(temp);
}
}
}
}
//if(t>=tmax) res.horizont_reached=true; -->tribool
if ((!(tmax < 0)) && maybe(approx_comparator::is_LE(tmax, t)))
res.horizont_reached = true;
for (int i = 0; i < gn; i++) {
if (jump_ok[i]) {
temp.interv.set_lower(tdeb_k[i]);
temp.interv.set_upper(t);
temp.index_low = tdeb_index[i];
temp.index_high = res.traj.size() - 1;
res.ji[i].push_back(temp);
}
}
return res;
}
