void VehicleInstance::setDestinationV(Mission *m, int x, int y, int z, int ox, int oy, int new_speed) { std::set < PathNode > open, closed; std::map < PathNode, PathNode > parent; m->adjXYZ(x, y, z); z = tile_z_; dest_path_.clear(); setSpeed(0); if (map_ == -1 || health_ <= 0 || !(walkable(x, y, z))) return; if (!walkable(tile_x_, tile_y_, tile_z_)) { float dBest = 100000, dCur; int xBest,yBest; // we got somewhere we shouldn't, we need to find somewhere that is walkable for (int j = 0; j < 5; j++) for (int i = 0; i < 5; i++) if (walkable(tile_x_ + i, tile_y_ + j, tile_z_)) { dCur = sqrt((float)(i*i + j*j)); if(dCur < dBest) { xBest = tile_x_ + i; yBest = tile_y_ + j; dBest = dCur; } } for (int j = 0; j < 5; j++) for (int i = 0; i > -5; --i) if (walkable(tile_x_ + i, tile_y_ + j, tile_z_)) { dCur = sqrt((float)(i*i + j*j)); if(dCur < dBest) { xBest = tile_x_ + i; yBest = tile_y_ + j; dBest = dCur; } } for (int j = 0; j > -5; --j) for (int i = 0; i > -5; --i) if (walkable(tile_x_ + i, tile_y_ + j, tile_z_)) { dCur = sqrt((float)(i*i + j*j)); if(dCur < dBest) { xBest = tile_x_ + i; yBest = tile_y_ + j; } } for (int j = 0; j > -5; --j) for (int i = 0; i < 5; i++) if (walkable(tile_x_ + i, tile_y_ + j, tile_z_)) { dCur = sqrt((float)(i*i + j*j)); if(dCur < dBest) { xBest = tile_x_ + i; yBest = tile_y_ + j; dBest = dCur; } } if(dBest == 100000) return; else { tile_x_ = xBest; tile_y_ = yBest; } } PathNode closest; float closest_dist = 100000; open.insert(PathNode(tile_x_, tile_y_, tile_z_, off_x_, off_y_)); int watchDog = 1000; while (!open.empty()) { watchDog--; float dist = 100000; PathNode p; std::set < PathNode >::iterator pit; for (std::set < PathNode >::iterator it = open.begin(); it != open.end(); it++) { float d = sqrt((float) (x - it->tileX()) * (x - it->tileX()) + (y - it->tileY()) * (y - it->tileY())); if (d < dist) { dist = d; p = *it; pit = it; // it cannot be const_iterator because of this assign } } if (dist < closest_dist) { closest = p; closest_dist = dist; } //printf("found best dist %f in %i nodes\n", dist, open.size()); open.erase(pit); closed.insert(p); if ((p.tileX() == x && p.tileY() == y && p.tileZ() == z) || watchDog < 0) { if (watchDog < 0) { p = closest; dest_path_. push_front(PathNode (p.tileX(), p.tileY(), p.tileZ(), ox, oy)); } else dest_path_.push_front(PathNode(x, y, z, ox, oy)); while (parent.find(p) != parent.end()) { p = parent[p]; if (p.tileX() == tile_x_ && p.tileY() == tile_y_ && p.tileZ() == tile_z_) break; dest_path_.push_front(p); } break; } std::list < PathNode > neighbours; uint32 goodDir = tileDir(p.tileX(), p.tileY(), p.tileZ()); if (p.tileX() > 0) { if (dirWalkable(&p,p.tileX() - 1, p.tileY(), p.tileZ()) && ((goodDir & 0xFF000000) == 0x06000000 || goodDir == 0xFFFFFFFF)) neighbours. push_back(PathNode(p.tileX() - 1, p.tileY(), p.tileZ())); } if (p.tileX() < g_App.maps().map(map())->maxX()) { if (dirWalkable(&p,p.tileX() + 1, p.tileY(), p.tileZ()) && ((goodDir & 0x0000FF00) == 0x00000200 || goodDir == 0xFFFFFFFF)) neighbours. push_back(PathNode(p.tileX() + 1, p.tileY(), p.tileZ())); } if (p.tileY() > 0) if (dirWalkable(&p,p.tileX(), p.tileY() - 1, p.tileZ()) && ((goodDir & 0x00FF0000) == 0x00040000 || goodDir == 0xFFFFFFFF)) neighbours. push_back(PathNode(p.tileX(), p.tileY() - 1, p.tileZ())); if (p.tileY() < g_App.maps().map(map())->maxY()) if (dirWalkable(&p,p.tileX(), p.tileY() + 1, p.tileZ()) && ((goodDir & 0x000000FF) == 0x0 || goodDir == 0xFFFFFFFF)) neighbours. push_back(PathNode(p.tileX(), p.tileY() + 1, p.tileZ())); for (std::list < PathNode >::iterator it = neighbours.begin(); it != neighbours.end(); it++) if (dirWalkable(&p,it->tileX(), it->tileY(), it->tileZ()) && open.find(*it) == open.end() && closed.find(*it) == closed.end()) { parent[*it] = p; open.insert(*it); } } if(!dest_path_.empty()) { // Adjusting offsets for correct positioning speed_ = new_speed; for(std::list < PathNode >::iterator it = dest_path_.begin(); it != dest_path_.end(); it++) { // TODO : requires testing for correct offsets per // every direction, because in some part of game // vehicle position on start of game can create incorrect // visual representation // maybe offsets depend on type or tileZ? switch(tileDir(it->tileX(), it->tileY(), it->tileZ())) { case 0xFFFFFF00: case 0xFFFF0200: it->setOffX(200); it->setOffY(32); break; case 0xFF04FFFF: it->setOffX(32); it->setOffY(200); break; case 0xFFFF02FF: case 0xFF0402FF: it->setOffX(32); it->setOffY(32); break; case 0x06FFFFFF: case 0x0604FFFF: it->setOffX(32); it->setOffY(200); break; case 0x06FFFF00: it->setOffX(200); it->setOffY(200); break; default: printf("hmm tileDir %X\n", (unsigned int)tileDir(it->tileX(), it->tileY(), it->tileZ())); break; } } } }