bool Pathfinder::pathfind(std::vector<PathfindingAction>& path)
{
#if 0
pout << "Actor " << actor->getObjId();
if (targetitem) {
pout << " pathfinding to item: ";
targetitem->dumpInfo();
} else {
pout << " pathfinding to (" << targetx << "," << targety << "," << targetz << ")" << std::endl;
}
#endif
#ifdef DEBUG
if (actor->getObjId() == visualdebug_actor) {
RenderSurface* screen = GUIApp::get_instance()->getScreen();
screen->BeginPainting();
if (targetitem)
drawbox(targetitem);
else
drawdot(targetx, targety, targetz, 2, 0xFF0000FF);
screen->EndPainting();
}
#endif
path.clear();
PathNode* startnode = new PathNode();
startnode->state = start;
startnode->cost = 0;
startnode->parent = 0;
startnode->depth = 0;
startnode->stepsfromparent = 0;
nodelist.push_back(startnode);
nodes.push(startnode);
unsigned int expandednodes = 0;
const unsigned int NODELIMIT_MIN = 30; //! constant
const unsigned int NODELIMIT_MAX = 200; //! constant
bool found = false;
Uint32 starttime = SDL_GetTicks();
while (expandednodes < NODELIMIT_MAX && !nodes.empty() && !found) {
PathNode* node = nodes.top(); nodes.pop();
#if 0
pout << "Trying node: (" << node->state.x << "," << node->state.y
<< "," << node->state.z << ") target=(" << targetx << ","
<< targety << "," << targetz << ")" << std::endl;
#endif
if (checkTarget(node)) {
// done!
// find path length
PathNode* n = node;
unsigned int length = 0;
while (n->parent) {
n = n->parent;
length++;
}
#if 0
pout << "Pathfinder: path found (length = " << length << ")"
<< std::endl;
#endif
unsigned int i = length;
if (length > 0) length++; // add space for final 'stand' action
path.resize(length);
// now backtrack through the nodes to assemble the final animation
while (node->parent) {
PathfindingAction action;
action.action = node->state.lastanim;
action.direction = node->state.direction;
action.steps = node->stepsfromparent;
path[--i] = action;
#if 0
pout << "anim = " << node->state.lastanim << ", dir = "
<< node->state.direction << ", steps = "
<< node->stepsfromparent << std::endl;
#endif
//TODO: check how turns work
//TODO: append final 'stand' animation
node = node->parent;
}
if (length) {
if (node->state.combat)
path[length-1].action = Animation::combatStand;
else
path[length-1].action = Animation::stand;
path[length-1].direction = path[length-2].direction;
}
expandtime = SDL_GetTicks() - starttime;
return true;
}
expandNode(node);
expandednodes++;
if(expandednodes >= NODELIMIT_MIN && ((expandednodes) % 5) == 0)
{
Uint32 elapsed_ms = SDL_GetTicks() - starttime;
if(elapsed_ms > 350) break;
}
}
expandtime = SDL_GetTicks() - starttime;
#if 0
static sint32 pfcalls = 0;
static sint32 pftotaltime = 0;
pfcalls++;
pftotaltime += expandtime;
pout << "maxout average = " << (pftotaltime / pfcalls) << "ms." << std::endl;
#endif
return false;
}
void Pathfinder::newNode(PathNode* oldnode, PathfindingState& state,
unsigned int steps)
{
PathNode* newnode = new PathNode();
nodelist.push_back(newnode); // for garbage collection
newnode->state = state;
newnode->parent = oldnode;
newnode->depth = oldnode->depth + 1;
newnode->stepsfromparent = 0;
double sqrddist;
sqrddist = ((newnode->state.x - oldnode->state.x)*
(newnode->state.x - oldnode->state.x));
sqrddist += ((newnode->state.y - oldnode->state.y)*
(newnode->state.y - oldnode->state.y));
sqrddist += ((newnode->state.z - oldnode->state.z)*
(newnode->state.z - oldnode->state.z));
unsigned int dist;
dist = static_cast<unsigned int>(std::sqrt(sqrddist));
int turn = 0;
if (oldnode->depth > 0) {
turn = state.direction - oldnode->state.direction;
if (turn < 0) turn = -turn;
if (turn > 4) turn = 8 - turn;
}
newnode->cost = oldnode->cost + dist + 32*turn; //!! constant
bool done = checkTarget(newnode);
if (done)
newnode->heuristicTotalCost = 0;
else
costHeuristic(newnode);
#if 0
perr << "trying dir " << state.direction;
if (steps > 0) {
perr << ", " << steps << " steps";
}
perr << " from ("
<< oldnode->state.x << "," << oldnode->state.y << ") to ("
<< newnode->state.x << "," << newnode->state.y
<< "), cost = " << newnode->cost << ", heurtotcost = "
<< newnode->heuristicTotalCost << std::endl;
#endif
#ifdef DEBUG
if (actor->getObjId() == visualdebug_actor) {
RenderSurface* screen = GUIApp::get_instance()->getScreen();
screen->BeginPainting();
drawpath(newnode, 0xFFFFFF00, done);
screen->EndPainting();
SDL_Delay(250);
if (!done) {
screen->BeginPainting();
drawpath(newnode, 0xFFB0B000, done);
screen->EndPainting();
}
}
#endif
nodes.push(newnode);
}
