00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00046 priority_queue <mapsquare *, vector<mapsquare *>, compare_squarecost> sorted_nodes;
00047
00048 vector <mapsquare *> opened_nodes;
00049
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
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
00072 if (n->x () == goal.x && n->y () == goal.y)
00073 {
00074 while (n->parent != NULL)
00075 {
00076
00077 if (n->x () == n->parent->x ())
00078 {
00079
00080 if (n->y () - n->parent->y () < 0)
00081 moves_to_goal.push_back (WALK_NORTH);
00082
00083 else moves_to_goal.push_back (WALK_SOUTH);
00084 }
00085
00086 else
00087 {
00088
00089 if (n->x () - n->parent->x () < 0)
00090 moves_to_goal.push_back (WALK_WEST);
00091
00092 else moves_to_goal.push_back (WALK_EAST);
00093 }
00094 n = n->parent;
00095 }
00096 return true;
00097 }
00098
00099
00100 mapsquare * np;
00101
00102
00103
00104
00105
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
00118 if (!((in_opened || in_closed) && np->g <= newg))
00119
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
00127 if (in_closed)
00128 closed_nodes.erase (find (closed_nodes.begin (), closed_nodes.end (), np));
00129
00130
00131 if (!in_opened)
00132 {
00133 sorted_nodes.push (np);
00134 opened_nodes.push_back (np);
00135 }
00136 }
00137 }
00138 }
00139
00140
00141
00142
00143
00144
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
00157 if (!((in_opened || in_closed) && np->g <= newg))
00158
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
00166 if (in_closed)
00167 closed_nodes.erase (find (closed_nodes.begin (), closed_nodes.end (), np));
00168
00169
00170 if (!in_opened)
00171 {
00172 sorted_nodes.push (np);
00173 opened_nodes.push_back (np);
00174 }
00175 }
00176 }
00177 }
00178
00179
00180
00181
00182
00183
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
00196 if (!((in_opened || in_closed) && np->g <= newg))
00197
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
00205 if (in_closed)
00206 closed_nodes.erase (find (closed_nodes.begin (), closed_nodes.end (), np));
00207
00208
00209 if (!in_opened)
00210 {
00211 sorted_nodes.push (np);
00212 opened_nodes.push_back (np);
00213 }
00214 }
00215 }
00216 }
00217
00218
00219
00220
00221
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
00234 if (!((in_opened || in_closed) && np->g <= newg))
00235
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
00243 if (in_closed)
00244 closed_nodes.erase (find (closed_nodes.begin (), closed_nodes.end (), np));
00245
00246
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 }