void NPC::GoTo(Vector3 destination) { path.clear(); Waypoint* currLocation = new Waypoint(position, Waypoint::sizeH, Waypoint::sizeV); Waypoint* targetLocation = new Waypoint(destination, Waypoint::sizeH, Waypoint::sizeV); currLocation->position.y = Waypoint::sizeV / 2; targetLocation->position.y = Waypoint::sizeV / 2; if (currLocation->CheckLink(*targetLocation)){//if there is a clear path between location and destination path.push_back(targetLocation); } else{ //else follow Dijkstra currLocation->LinkWaypoints(); targetLocation->LinkWaypoints(); for (vector<Waypoint*>::iterator it = (targetLocation->reachableWaypoints).begin(); it != (targetLocation->reachableWaypoints).end(); ++it){ (*it)->target = targetLocation; } path = Dijkstra(currLocation, targetLocation); if (path.size() > 0){ path.pop_back(); // removes the last waypoint - it represents current position } } checkPoint = path.rbegin(); }