/**
* {{{ 删除列表
*/
int DeleteNode(Link to_delete)
{
Link curr, prev;
int i;
if (Head == NULL)
return 0;
for (prev = NULL, curr = Head; curr != NULL && (i = NodeCmp(to_delete, curr)) > 0; prev = curr, curr = curr->Next) {
// empty loop
}
if (curr != NULL && i == 0) {
if (prev) {
prev->Next = curr->Next;
} else {
Head = curr->Next;
}
FreeNode(curr);
NodeCount -= 1;
return 1;
}
return 0;
}
void RmSubst( NODE *n )
{
NODE *pn;
NODE *cn;
pn = 0;
cn = substList;
while( cn != 0 ) {
if( NodeCmp( n, cn ) )
break;
pn = cn;
cn = cn->left;
}
if( cn == 0 ) return;
FreeNode( cn->right );
if( pn )
pn->left = cn->left;
else
substList = cn->left;
cn->right = 0;
cn->left = 0;
FreeNode( cn );
}
NODE * LookUpSubst( NODE *n )
{
NODE *cn;
cn = substList;
while( cn != 0 ) {
if( NodeCmp( n, cn ) )
return cn;
cn = cn->left;
}
return 0;
}
// Initiallize a new search
void AStar::new_search(Point &sp, Point &gp)
{
navigraph.find_onoff_ramps(sp, gp, start->navidata, goal->navidata);
// Initialise the A* specific parts of the start node
start->g = 0.0;
start->h = estimate_cost_to_goal(start);
start->f = start->g + start->h;
start->parent = 0;
// Push the start node on the open list (heap)
open.push_back(start);
std::push_heap(open.begin(), open.end(), NodeCmp());
steps = 0;
search_state = SEARCH_STATE_SEARCHING;
}
/**
* {{{ 添加节点
*/
int AddNodeAscend(Link to_add)
{
Link pn, prev, curr;
struct Node dummy;
int i;
pn = (Link) malloc(sizeof(struct Node));
if (pn == NULL) {
return 0;
}
memcpy(pn, to_add, sizeof(struct Node));
// 设置虚拟节点
dummy.Next = Head;
prev = &dummy;
curr = Head;
for (;; prev = curr, curr = curr->Next) {
if (curr == NULL) { // 到达了链表的结尾
break;
}
i = NodeCmp(pn, curr);
if (i <= 0) {
break;
}
}
if (curr && i == 0) {
if (DuplicateNode(curr, pn) == 0) {
return 1;
}
}
prev->Next = pn;
pn->Next = curr;
Head = dummy.Next; // 回收虚拟节点
return 1;
}
IRenderSystem::RenderNode IRenderSystem::addRenderable( IRenderable* obj )
{
RNODE node;
node.obj = obj;
node.effect = NULL;
RenderNode iter = std::lower_bound( mRenderList.begin() , mRenderList.end() , node , NodeCmp() );
mRenderList.insert( iter , node );
#ifdef _DEBUG
obj->mbManaged = true;
return --iter;
#endif
}
// Take one step forward in the search and return the resulting state
int AStar::search_step(void)
{
// Break if the search is not initialised or the search is finished
if(search_state != SEARCH_STATE_SEARCHING)
return search_state;
// If the open list is empty, there is no solution,
// set state to failed and return
if(open.empty())
return search_state = SEARCH_STATE_FAILED;
steps++;
// Get the best node from the open list (pop the heap)
Node* n = open.front();
std::pop_heap(open.begin(), open.end(), NodeCmp());
open.pop_back();
// Check if this was the goal, if it was we're done
if(n == goal)
{
// If the goal and start is not the same node,
// reconstruct the solution path
if(n != start)
{
// Set the child pointers in each node
// (except goal which has no child)
Node* child_node = goal;
Node* parent_node = goal->parent;
do {
// set pointer
parent_node->child = child_node;
// move on to next
child_node = parent_node;
parent_node = parent_node->parent;
} while(child_node != start); // start is always the first node
}
return search_state = SEARCH_STATE_SUCCEEDED;
} else { // if not the goal
// We now need to get the successors (neigbours) of this node,
// the neighbours are placed in the list successors
get_successors(n);
// Handle each successor
for(NodeListIterator succ = successors.begin(); succ != successors.end(); succ++)
{
// Calculate the cost from start to this node
float newg = n->g + get_cost(n,(*succ));
// Now we need to find whether the node is already on the open or
// closed lists. If it is but the node that is already on them is
// better (lower g) then we can forget about this successor
// First linear search of open list to find node
// TODO: Improve to better complexity than linear
NodeListIterator open_it;
for(open_it = open.begin(); open_it != open.end(); open_it++){
if((*open_it) == (*succ))
break;
}
if((open_it != open.end()) && ((*open_it)->g <= newg))
continue;
// Now do the same check on closed list
NodeListIterator closed_it;
for(closed_it = closed.begin(); closed_it != closed.end(); closed_it++){
if((*closed_it) == (*succ))
break;
}
if((closed_it != closed.end()) && ((*closed_it)->g <= newg))
continue;
// This node is the best node so far so
// lets keep it and set up its A* data
(*succ)->parent = n;
(*succ)->g = newg;
(*succ)->h = estimate_cost_to_goal((*succ));
(*succ)->f = (*succ)->g + (*succ)->h;
(*succ)->visited = true;
// Remove successor from closed if it was on it
if(closed_it != closed.end())
closed.erase(closed_it);
// If not in open yet, push node onto open
// else make sure to keep the heap structure
if(open_it == open.end()){
open.push_back((*succ));
std::push_heap(open.begin(), open.end(), NodeCmp());
} else {
std::make_heap(open.begin(), open.end(), NodeCmp());
}
}
// Push n onto closed, as we have expanded it now
closed.push_back(n);
}
return search_state; // return the resulting state of this search step
}
