void SparseSystem::apply_givens(int row, int col, double* c_givens, double* s_givens) {
double c, s;
SparseMatrix::apply_givens(row, col, &c, &s);
// modify rhs
double r1 = _rhs(col);
double r2 = _rhs(row);
_rhs(col) = c*r1 - s*r2;
_rhs(row) = s*r1 + c*r2;
}
VectorXd SparseSystem::solve() const {
requireDebug(num_rows() >= num_cols(), "SparseSystem::solve: cannot solve system, not enough constraints");
requireDebug(num_rows() == num_cols(), "SparseSystem::solve: system not triangular");
int n = num_cols();
VectorXd result(n);
for (int row=n-1; row>=0; row--) {
const SparseVector& rowvec = get_row(row);
// start with rhs...
double terms = _rhs(row);
double diag;
// ... and subtract already solved variables multiplied with respective coefficient from R
// We assume that the SpareSystem is triangular
SparseVectorIter iter(rowvec);
iter.get(diag); // we can check return value it should be == row
iter.next();
for (; iter.valid(); iter.next()) {
double v;
int col = iter.get(v);
terms = terms - result(col)*v;
}
// divide result by diagonal entry of R
result(row) = terms / diag;
}
return result;
}
/**
* Calculates key value for cell.
*
* @param Map::Cell* cell to calculate for
* @return pair<double,double> key value
*/
pair<double,double> Planner::_k(Map::Cell* u)
{
double g = _g(u);
double rhs = _rhs(u);
double min = (g < rhs) ? g : rhs;
return pair<double,double>((min + _h(_start, u) + _km), min);
}
void SparseSystem::add_row(const SparseVector& new_row, double new_r) {
ensure_num_cols(new_row.last()+1);
append_new_rows(1);
int row = num_rows() - 1;
_rhs(row) = new_r;
set_row(row, new_row);
}
double CNCPoint::YZDistance( const CNCPoint & rhs ) const
{
gp_Pnt _lhs(*this);
gp_Pnt _rhs(rhs);
_lhs.SetX(0.0);
_rhs.SetX(0.0);
return(_lhs.Distance(_rhs));
}
/**
* Updates cell.
*
* @param Map::Cell* cell to update
* @return void
*/
void Planner::_update(Map::Cell* u)
{
bool diff = _g(u) != _rhs(u);
bool exists = (_open_hash.find(u) != _open_hash.end());
if (diff && exists)
{
_list_update(u, _k(u));
}
else if (diff && ! exists)
{
_list_insert(u, _k(u));
}
else if ( ! diff && exists)
{
_list_remove(u);
}
}
/**
* Constructor.
*
* @param Map* map
* @param Map::Cell* start cell
* @param Map::Cell* goal cell
*/
Planner::Planner(Map* map, Map::Cell* start, Map::Cell* goal)
{
// Clear lists
_open_list.clear();
_open_hash.clear();
_path.clear();
_km = 0;
_map = map;
_start = start;
_goal = goal;
_last = _start;
_rhs(_goal, 0.0);
_list_insert(_goal, pair<double,double>(_h(_start, _goal), 0));
}
void OsiIF::_row(int i, Row &r) const
{
const CoinPackedMatrix* coinMatrix;
CoinPackedVector coinVector;
int coinNumEl;
const int* coinIndices;
const double* coinElements;
coinMatrix = osiLP_->getMatrixByRow();
coinVector = coinMatrix->getVector(i);
coinNumEl = coinVector.getNumElements();
coinIndices = coinVector.getIndices();
coinElements = coinVector.getElements();
r.clear();
for (int j = 0; j < coinNumEl; j++)
r.insert(coinIndices[j], coinElements[j]);
r.sense(osi2csense(rowsense_[i]));
r.rhs(_rhs(i));
}
/**
* Computes shortest path.
*
* @return bool successful
*/
bool Planner::_compute()
{
if (_open_list.empty())
return false;
KeyCompare key_compare;
int attempts = 0;
Map::Cell* u;
pair<double,double> k_old;
pair<double,double> k_new;
Map::Cell** nbrs;
double g_old;
double tmp_g, tmp_rhs;
while (( ! _open_list.empty() && key_compare(_open_list.begin()->first, _k(_start))) || ! Math::equals(_rhs(_start), _g(_start)))
{
// Reached max steps, quit
if (++attempts > Planner::MAX_STEPS)
return false;
u = _open_list.begin()->second;
k_old = _open_list.begin()->first;
k_new = _k(u);
tmp_rhs = _rhs(u);
tmp_g = _g(u);
if (key_compare(k_old, k_new))
{
_list_update(u, k_new);
}
else if (Math::greater(tmp_g, tmp_rhs))
{
_g(u, tmp_rhs);
tmp_g = tmp_rhs;
_list_remove(u);
nbrs = u->nbrs();
for (unsigned int i = 0; i < Map::Cell::NUM_NBRS; i++)
{
if (nbrs[i] != NULL)
{
if (nbrs[i] != _goal)
{
_rhs(nbrs[i], min(_rhs(nbrs[i]), _cost(nbrs[i], u) + tmp_g));
}
_update(nbrs[i]);
}
}
}
else
{
g_old = tmp_g;
_g(u, Math::INF);
// Perform action for u
if (u != _goal)
{
_rhs(u, _min_succ(u).second);
}
_update(u);
nbrs = u->nbrs();
// Perform action for neighbors
for (unsigned int i = 0; i < Map::Cell::NUM_NBRS; i++)
{
if (nbrs[i] != NULL)
{
if (Math::equals(_rhs(nbrs[i]), (_cost(nbrs[i], u) + g_old)))
{
if (nbrs[i] != _goal)
{
_rhs(nbrs[i], _min_succ(nbrs[i]).second);
}
}
_update(nbrs[i]);
}
}
}
}
return true;
}
/**
* Update map.
*
* @param Map::Cell* cell to update
* @param double new cost of the cell
* @return void
*/
void Planner::update(Map::Cell* u, double cost)
{
if (u == _goal)
return;
// Update km
_km += _h(_last, _start);
_last = _start;
_cell(u);
double cost_old = u->cost;
double cost_new = cost;
u->cost = cost;
Map::Cell** nbrs = u->nbrs();
double tmp_cost_old, tmp_cost_new;
double tmp_rhs, tmp_g;
// Update u
for (unsigned int i = 0; i < Map::Cell::NUM_NBRS; i++)
{
if (nbrs[i] != NULL)
{
u->cost = cost_old;
tmp_cost_old = _cost(u, nbrs[i]);
u->cost = cost_new;
tmp_cost_new = _cost(u, nbrs[i]);
tmp_rhs = _rhs(u);
tmp_g = _g(nbrs[i]);
if (Math::greater(tmp_cost_old, tmp_cost_new))
{
if (u != _goal)
{
_rhs(u, min(tmp_rhs, (tmp_cost_new + tmp_g)));
}
}
else if (Math::equals(tmp_rhs, (tmp_cost_old + tmp_g)))
{
if (u != _goal)
{
_rhs(u, _min_succ(u).second);
}
}
}
}
_update(u);
// Update neighbors
for (unsigned int i = 0; i < Map::Cell::NUM_NBRS; i++)
{
if (nbrs[i] != NULL)
{
u->cost = cost_old;
tmp_cost_old = _cost(u, nbrs[i]);
u->cost = cost_new;
tmp_cost_new = _cost(u, nbrs[i]);
tmp_rhs = _rhs(nbrs[i]);
tmp_g = _g(u);
if (Math::greater(tmp_cost_old, tmp_cost_new))
{
if (nbrs[i] != _goal)
{
_rhs(nbrs[i], min(tmp_rhs, (tmp_cost_new + tmp_g)));
}
}
else if (Math::equals(tmp_rhs, (tmp_cost_old + tmp_g)))
{
if (nbrs[i] != _goal)
{
_rhs(nbrs[i], _min_succ(nbrs[i]).second);
}
}
_update(nbrs[i]);
}
}
}
