void list_push(pList *head, pList w, pnext next_fn, Mutex *mutex)
{
ASSERT(head);
ASSERT(w);
ASSERT(next_fn);
lock(mutex);
_list_insert(head, w, next_fn(w));
unlock(mutex);
}
void list_append(pList *head, pList w, pnext next_fn, Mutex *mutex)
{
ASSERT(head);
ASSERT(w);
ASSERT(next_fn);
lock(mutex);
for (; *head; head = next_fn(*head))
{
;
}
_list_insert(head, w, next_fn(w));
unlock(mutex);
}
/**
* 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 list_add_sorted(pList *head, pList w, pnext next_fn, cmp_fn cmp, Mutex *mutex)
{
ASSERT(head);
ASSERT(w);
ASSERT(next_fn);
ASSERT(cmp);
lock(mutex);
for (; *head; head = next_fn(*head))
{
if (cmp(w, *head) >= 0)
{
break;
}
}
_list_insert(head, w, next_fn(w));
unlock(mutex);
}
void symbol__list_insert(SYMBOL *sym)
{
_list_insert(&sym_head.list, &sym->list);
}
