Adonthell 0.4
|
00001 /* 00002 $Id: path.cc,v 1.5 2001/10/10 18:18:43 gnurou Exp $ 00003 00004 Copyright (C) 2001 Alexandre Courbot 00005 Part of the Adonthell Project http://adonthell.linuxgames.com 00006 00007 This program is free software; you can redistribute it and/or modify 00008 it under the terms of the GNU General Public License. 00009 This program is distributed in the hope that it will be useful, 00010 but WITHOUT ANY WARRANTY. 00011 00012 See the COPYING file for more details. 00013 */ 00014 00015 00016 /** 00017 * @file path.cc 00018 * @author Alexandre Courbot <alexandrecourbot@linuxgames.com> 00019 * 00020 * @brief Defines the path class. 00021 * 00022 * 00023 */ 00024 00025 #include "path.h" 00026 #include "landmap.h" 00027 #include <queue> 00028 #include <algorithm> 00029 00030 00031 u_int16 path::goal_estimate (u_int16 x, u_int16 y) 00032 { 00033 u_int16 est; 00034 if (x > goal.x) est = x - goal.x; 00035 else est = goal.x - x; 00036 00037 if (y > goal.y) est += y - goal.y; 00038 else est += goal.y - y; 00039 00040 return est; 00041 } 00042 00043 bool path::calculate () 00044 { 00045 // Sorted Open nodes. 00046 priority_queue <mapsquare *, vector<mapsquare *>, compare_squarecost> sorted_nodes; 00047 // Open nodes. 00048 vector <mapsquare *> opened_nodes; 00049 // Closed nodes. 00050 vector <mapsquare *> closed_nodes; 00051 00052 moves_to_goal.clear (); 00053 00054 mapsquare_area * smap = refmap->get_submap (submap); 00055 00056 mapsquare * n = smap->get_square (start.x, start.y); 00057 // Origin node 00058 n->g = 0; 00059 n->h = goal_estimate (n->x (), n->y ()); 00060 n->f = n->g + n->h; 00061 n->parent = NULL; 00062 00063 sorted_nodes.push (n); 00064 opened_nodes.push_back (n); 00065 while (!sorted_nodes.empty ()) 00066 { 00067 n = sorted_nodes.top (); 00068 sorted_nodes.pop (); 00069 opened_nodes.erase (find (opened_nodes.begin (), opened_nodes.end (), n)); 00070 00071 // Have we reached the goal? 00072 if (n->x () == goal.x && n->y () == goal.y) 00073 { 00074 while (n->parent != NULL) 00075 { 00076 // Vertical move 00077 if (n->x () == n->parent->x ()) 00078 { 00079 // Go to north 00080 if (n->y () - n->parent->y () < 0) 00081 moves_to_goal.push_back (WALK_NORTH); 00082 // Go to south 00083 else moves_to_goal.push_back (WALK_SOUTH); 00084 } 00085 // Horizontal move 00086 else 00087 { 00088 // Go to west 00089 if (n->x () - n->parent->x () < 0) 00090 moves_to_goal.push_back (WALK_WEST); 00091 // Go to east 00092 else moves_to_goal.push_back (WALK_EAST); 00093 } 00094 n = n->parent; 00095 } 00096 return true; 00097 } 00098 00099 // Now proceeding with the successors of n 00100 mapsquare * np; 00101 00102 // East square 00103 // Make sure that the square is not at the edge of the submap 00104 // and is directly reachable. 00105 // If so, add it to the opened nodes list. 00106 if (n->x () + 1 < smap->area_length ()) 00107 { 00108 np = smap->get_square (n->x () + 1, n->y ()); 00109 if (n->is_walkable_east () && np->is_walkable_west () && np->is_free () && 00110 (np->can_use_for_pathfinding || (np->x () == goal.x && np->y () == goal.y))) 00111 { 00112 u_int16 newg = n->g + 1; 00113 bool in_opened, in_closed; 00114 in_opened = (find (opened_nodes.begin (), opened_nodes.end (), np) != opened_nodes.end ()); 00115 in_closed = (find (closed_nodes.begin (), closed_nodes.end (), np) != closed_nodes.end ()); 00116 00117 // If np is in opened_nodes or closed_nodes and np->g <= newg, don't do anything. 00118 if (!((in_opened || in_closed) && np->g <= newg)) 00119 // else add the node to the opened nodes list (if necessary) 00120 { 00121 np->g = newg; 00122 np->h = goal_estimate (np->x (), np->y ()); 00123 np->f = np->g + np->h; 00124 np->parent = n; 00125 00126 // if np is in closed_nodes, remove it 00127 if (in_closed) 00128 closed_nodes.erase (find (closed_nodes.begin (), closed_nodes.end (), np)); 00129 00130 // if np is not in opened_nodes yet, add it 00131 if (!in_opened) 00132 { 00133 sorted_nodes.push (np); 00134 opened_nodes.push_back (np); 00135 } 00136 } 00137 } 00138 } 00139 00140 00141 // West square 00142 // Make sure that the square is not at the edge of the submap 00143 // and is directly reachable. 00144 // If so, add it to the opened nodes list. 00145 if (n->x () > 0) 00146 { 00147 np = smap->get_square (n->x () - 1, n->y ()); 00148 if (n->is_walkable_west () && np->is_walkable_east () && np->is_free () && 00149 (np->can_use_for_pathfinding || (np->x () == goal.x && np->y () == goal.y))) 00150 { 00151 u_int16 newg = n->g + 1; 00152 bool in_opened, in_closed; 00153 in_opened = (find (opened_nodes.begin (), opened_nodes.end (), np) != opened_nodes.end ()); 00154 in_closed = (find (closed_nodes.begin (), closed_nodes.end (), np) != closed_nodes.end ()); 00155 00156 // If np is in opened_nodes or closed_nodes and np->g <= newg, don't do anything. 00157 if (!((in_opened || in_closed) && np->g <= newg)) 00158 // else add the node to the opened nodes list (if necessary) 00159 { 00160 np->g = newg; 00161 np->h = goal_estimate (np->x (), np->y ()); 00162 np->f = np->g + np->h; 00163 np->parent = n; 00164 00165 // if np is in closed_nodes, remove it 00166 if (in_closed) 00167 closed_nodes.erase (find (closed_nodes.begin (), closed_nodes.end (), np)); 00168 00169 // if np is not in opened_nodes yet, add it 00170 if (!in_opened) 00171 { 00172 sorted_nodes.push (np); 00173 opened_nodes.push_back (np); 00174 } 00175 } 00176 } 00177 } 00178 00179 00180 // North square 00181 // Make sure that the square is not at the edge of the submap 00182 // and is directly reachable. 00183 // If so, add it to the opened nodes list. 00184 if (n->y () > 0) 00185 { 00186 np = smap->get_square (n->x (), n->y () - 1); 00187 if (n->is_walkable_north () && np->is_walkable_south () && np->is_free () && 00188 (np->can_use_for_pathfinding || (np->x () == goal.x && np->y () == goal.y))) 00189 { 00190 u_int16 newg = n->g + 1; 00191 bool in_opened, in_closed; 00192 in_opened = (find (opened_nodes.begin (), opened_nodes.end (), np) != opened_nodes.end ()); 00193 in_closed = (find (closed_nodes.begin (), closed_nodes.end (), np) != closed_nodes.end ()); 00194 00195 // If np is in opened_nodes or closed_nodes and np->g <= newg, don't do anything. 00196 if (!((in_opened || in_closed) && np->g <= newg)) 00197 // else add the node to the opened nodes list (if necessary) 00198 { 00199 np->g = newg; 00200 np->h = goal_estimate (np->x (), np->y ()); 00201 np->f = np->g + np->h; 00202 np->parent = n; 00203 00204 // if np is in closed_nodes, remove it 00205 if (in_closed) 00206 closed_nodes.erase (find (closed_nodes.begin (), closed_nodes.end (), np)); 00207 00208 // if np is not in opened_nodes yet, add it 00209 if (!in_opened) 00210 { 00211 sorted_nodes.push (np); 00212 opened_nodes.push_back (np); 00213 } 00214 } 00215 } 00216 } 00217 00218 // South square 00219 // Make sure that the square is not at the edge of the submap 00220 // and is directly reachable. 00221 // If so, add it to the opened nodes list. 00222 if (n->y () + 1 < smap->area_height ()) 00223 { 00224 np = smap->get_square (n->x (), n->y () + 1); 00225 if (n->is_walkable_south () && np->is_walkable_north () && np->is_free () && 00226 (np->can_use_for_pathfinding || (np->x () == goal.x && np->y () == goal.y))) 00227 { 00228 u_int16 newg = n->g + 1; 00229 bool in_opened, in_closed; 00230 in_opened = (find (opened_nodes.begin (), opened_nodes.end (), np) != opened_nodes.end ()); 00231 in_closed = (find (closed_nodes.begin (), closed_nodes.end (), np) != closed_nodes.end ()); 00232 00233 // If np is in opened_nodes or closed_nodes and np->g <= newg, don't do anything. 00234 if (!((in_opened || in_closed) && np->g <= newg)) 00235 // else add the node to the opened nodes list (if necessary) 00236 { 00237 np->g = newg; 00238 np->h = goal_estimate (np->x (), np->y ()); 00239 np->f = np->g + np->h; 00240 np->parent = n; 00241 00242 // if np is in closed_nodes, remove it 00243 if (in_closed) 00244 closed_nodes.erase (find (closed_nodes.begin (), closed_nodes.end (), np)); 00245 00246 // if np is not in opened_nodes yet, add it 00247 if (!in_opened) 00248 { 00249 sorted_nodes.push (np); 00250 opened_nodes.push_back (np); 00251 } 00252 } 00253 } 00254 } 00255 00256 closed_nodes.push_back (n); 00257 } 00258 return false; 00259 } 00260 00261 s_int8 path::get_state (igzstream & file) 00262 { 00263 u_int16 nb_moves; 00264 00265 clear (); 00266 00267 submap << file; 00268 dir << file; 00269 start.x << file; 00270 start.y << file; 00271 goal.x << file; 00272 goal.y << file; 00273 00274 nb_moves << file; 00275 00276 for (u_int16 i = 0; i < nb_moves; i++) 00277 { 00278 u_int16 t; 00279 t << file; 00280 moves_to_goal.push_back (t); 00281 } 00282 return 0; 00283 } 00284 00285 s_int8 path::put_state (ogzstream & file) const 00286 { 00287 submap >> file; 00288 dir >> file; 00289 start.x >> file; 00290 start.y >> file; 00291 goal.x >> file; 00292 goal.y >> file; 00293 00294 nbr_moves () >> file; 00295 00296 for (u_int16 i = 0; i < nbr_moves (); i++) 00297 { 00298 get_move (i) >> file; 00299 } 00300 return 0; 00301 }