From 2b49711061d43d3f429cff08dace2cd15a7c8821 Mon Sep 17 00:00:00 2001 From: Charles Pigott Date: Thu, 1 Jan 2015 19:33:26 +0000 Subject: [PATCH 11/14] change pathing --- src/geometry.h | 12 +++++++++ src/path_finding.cpp | 76 +++++++++++++++++++++------------------------------- src/path_finding.h | 20 +++++++------- src/person.cpp | 25 +++++++++-------- 4 files changed, 66 insertions(+), 67 deletions(-) diff --git a/src/geometry.h b/src/geometry.h index 46a6f59..fecce30 100644 --- a/src/geometry.h +++ b/src/geometry.h @@ -143,6 +143,18 @@ inline bool operator!=(const XYZPoint &p, const XYZPoint &q) } /** + * Comparsion of two 3D points. + * @param p First point to compare. + * @param q Second point to compare. + * @return + */ +template +inline bool operator<(const XYZPoint &p, const XYZPoint &q) +{ + return p.x < q.x && p.y < q.y && p.z < q.z; +} + +/** * Add two 3D points together. * @param p First point to add. * @param q Second point to add. diff --git a/src/path_finding.cpp b/src/path_finding.cpp index 15244ea..2830c06 100644 --- a/src/path_finding.cpp +++ b/src/path_finding.cpp @@ -15,15 +15,13 @@ /** * Constructor of a walked position. - * @param x Current X position. - * @param y Current Y position. - * @param z Current Z position. + * @param cur_vox Current voxel position. * @param traveled Length of travel from the starting point so far. * @param estimate Estimated length of remaining travel to the destination (should be less or equal to the real value for optimal solutions). * @param prev_pos Previous walked position used to get at the new position, \c nullptr for the first position. */ -WalkedPosition::WalkedPosition(int x, int y, int z, uint32 traveled, uint32 estimate, const WalkedPosition *prev_pos) - : x(x), y(y), z(z), traveled(traveled), estimate(estimate), prev_pos(prev_pos) +WalkedPosition::WalkedPosition(const XYZPoint16 &cur_vox, uint32 traveled, uint32 estimate, const WalkedPosition *prev_pos) + : cur_vox(cur_vox), traveled(traveled), estimate(estimate), prev_pos(prev_pos) { } @@ -35,9 +33,7 @@ WalkedPosition::WalkedPosition(int x, int y, int z, uint32 traveled, uint32 esti */ bool operator<(const WalkedPosition &wp1, const WalkedPosition &wp2) { - if (wp1.x != wp2.x) return wp1.x < wp2.x; - if (wp1.y != wp2.y) return wp1.y < wp2.y; - return wp1.z < wp2.z; + return wp1.cur_vox < wp2.cur_vox; } /** @@ -66,55 +62,45 @@ bool operator<(const WalkedDistance &wd1, const WalkedDistance &wd2) /** * Constructor, find a path to (\a dest_x, \a dest_y, \a dest_z). Give starting points through PathSearcher::AddStart. - * @param dest_x X coordinate of the destination voxel. - * @param dest_y Y coordinate of the destination voxel. - * @param dest_z Z coordinate of the destination voxel. + * @param dest_vox Coordinate of the destination voxel. */ -PathSearcher::PathSearcher(int dest_x, int dest_y, int dest_z) +PathSearcher::PathSearcher(const XYZPoint16 &dest_vox) { - this->dest_x = dest_x; - this->dest_y = dest_y; - this->dest_z = dest_z; + this->dest_vox = dest_vox; this->dest_pos = nullptr; } /** * Add a starting point to the searcher. - * @param start_x X coordinate of the start voxel. - * @param start_y Y coordinate of the start voxel. - * @param start_z Z coordinate of the start voxel. + * @param start_vox Coordinate of the start voxel. */ -void PathSearcher::AddStart(int start_x, int start_y, int start_z) +void PathSearcher::AddStart(const XYZPoint16 &start_vox) { - this->AddOpen(start_x, start_y, start_z, 0, nullptr); + this->AddOpen(start_vox, 0, nullptr); } /** * Get an (optimistic) estimate of the path length to go to the destination voxel. - * @param x Current x position in voxels. - * @param y Current y position in voxels. - * @param z Current z position in voxels. + * @param vox Current position in voxels. * @return Estimate of the length of path still to go. */ -inline uint32 PathSearcher::GetEstimate(int x, int y, int z) +inline uint32 PathSearcher::GetEstimate(const XYZPoint16 &vox) { - int32 val = abs(x - this->dest_x) + abs(y - this->dest_y); - if (val < abs(z - this->dest_z)) return abs(z - this->dest_z); + int32 val = abs(vox.x - this->dest_vox.x) + abs(vox.y - this->dest_vox.y); + if (val < abs(vox.z - this->dest_vox.z)) return abs(vox.z - this->dest_vox.z); return val; } /** * Add a new open position to the set of open points, if it is better than already available. - * @param x X position of the current position. - * @param y Y position of the current position. - * @param z Z position of the current position. + * @param vox Position of the current position. * @param traveled Distance traveled to get to the current position. * @param prev_pos Previous position (\c nullptr for the start position). */ -void PathSearcher::AddOpen(int x, int y, int z, uint32 traveled, const WalkedPosition *prev_pos) +void PathSearcher::AddOpen(const XYZPoint16 &vox, uint32 traveled, const WalkedPosition *prev_pos) { - uint32 estimate = this->GetEstimate(x, y, z); - WalkedPosition wp(x, y, z, traveled, estimate, prev_pos); + uint32 estimate = this->GetEstimate(vox); + WalkedPosition wp(vox, traveled, estimate, prev_pos); /* Find the position. */ PositionSet::iterator pos_iter = this->positions.find(wp); @@ -149,13 +135,13 @@ bool PathSearcher::Search() /* Reached the destination? */ const WalkedPosition *wp = wd.pos; - if (wp->x == this->dest_x && wp->y == this->dest_y && wp->z == this->dest_z) { + if (wp->cur_vox == this->dest_vox) { this->dest_pos = wp; return true; } /* Add new open points. */ - const Voxel *v = _world.GetVoxel(XYZPoint16(wp->x, wp->y, wp->z)); + const Voxel *v = _world.GetVoxel(wp->cur_vox); if (v == nullptr) continue; // No voxel at the expected point, don't bother. uint8 exits = GetPathExits(v); @@ -164,29 +150,29 @@ bool PathSearcher::Search() /* There is an outgoing connection, is it also on the world? */ Point16 dxy = _tile_dxy[edge]; - if (dxy.x < 0 && wp->x == 0) continue; - if (dxy.x > 0 && wp->x + 1 == _world.GetXSize()) continue; - if (dxy.y < 0 && wp->y == 0) continue; - if (dxy.y > 0 && wp->y + 1 == _world.GetYSize()) continue; + if (dxy.x < 0 && wp->cur_vox.x == 0) continue; + if (dxy.x > 0 && wp->cur_vox.x + 1 == _world.GetXSize()) continue; + if (dxy.y < 0 && wp->cur_vox.y == 0) continue; + if (dxy.y > 0 && wp->cur_vox.y + 1 == _world.GetYSize()) continue; - int new_z = ((exits & (0x10 << edge)) != 0) ? wp->z + 1 : wp->z; - if (new_z < 0 || new_z >= WORLD_Z_SIZE) continue; + int extra_z = ((exits & (0x10 << edge)) != 0); + if (wp->cur_vox.z + extra_z < 0 || wp->cur_vox.z + extra_z >= WORLD_Z_SIZE) continue; /* Now check the other side, new_z is the voxel where the path should be at the bottom. */ - const Voxel *v2 = _world.GetVoxel(XYZPoint16(wp->x + dxy.x, wp->y + dxy.y, new_z)); + const Voxel *v2 = _world.GetVoxel(wp->cur_vox + XYZPoint16(dxy.x, dxy.y, extra_z)); if (v2 == nullptr) continue; uint8 other_exits = GetPathExits(v2); if ((other_exits & (1 << ((edge + 2) % 4))) == 0) { // No path here, try one voxel below - new_z--; - if (new_z < 0) continue; - v2 = _world.GetVoxel(XYZPoint16(wp->x + dxy.x, wp->y + dxy.y, new_z)); + extra_z--; + if (wp->cur_vox.z + extra_z < 0) continue; + v2 = _world.GetVoxel(wp->cur_vox + XYZPoint16(dxy.x, dxy.y, extra_z)); if (v2 == nullptr) continue; other_exits = GetPathExits(v2); if ((other_exits & (0x10 << ((edge + 2) % 4))) == 0) continue; } /* Add new open point to the path finder. */ - this->AddOpen(wp->x + dxy.x, wp->y + dxy.y, new_z, wp->traveled + 1, wp); + this->AddOpen(wp->cur_vox + XYZPoint16(dxy.x, dxy.y, extra_z), wp->traveled + 1, wp); } } return false; diff --git a/src/path_finding.h b/src/path_finding.h index c4f7bc3..d20fd91 100644 --- a/src/path_finding.h +++ b/src/path_finding.h @@ -14,14 +14,14 @@ #include +#include "geometry.h" + /** Intermediate position of a walk. */ class WalkedPosition { public: - WalkedPosition(int x, int y, int z, uint32 traveled, uint32 estimate, const WalkedPosition *prev_pos); + WalkedPosition(const XYZPoint16 &cur_vox, uint32 traveled, uint32 estimate, const WalkedPosition *prev_pos); - int x; ///< X coordinate of the current position. - int y; ///< Y coordinate of the current position. - int z; ///< Z coordinate of the current position. + XYZPoint16 cur_vox; ///< Coordinate of the current position. mutable uint32 traveled; ///< Length of the traveled path so far. mutable uint32 estimate; ///< Estimated distance to the destination. mutable const WalkedPosition *prev_pos; ///< Position coming from (\c nullptr for initial position). @@ -43,23 +43,21 @@ typedef std::multiset OpenPoints; ///< Points for further explor /** Class for searching (and hopefully finding) a path between tiles. */ class PathSearcher { public: - PathSearcher(int dest_x, int dest_y, int dest_z); + PathSearcher(const XYZPoint16 &dest_vox); - void AddStart(int start_x, int start_y, int start_z); + void AddStart(const XYZPoint16 &start_vox); bool Search(); void Clear(); - int dest_x; ///< X coordinate of the desired destination voxel. - int dest_y; ///< Y coordinate of the desired destination voxel. - int dest_z; ///< Z coordinate of the desired destination voxel. + XYZPoint16 dest_vox; ///< Coordinate of the desired destination voxel. const WalkedPosition *dest_pos; ///< If path was found, this points to the end-point of the walk. protected: PositionSet positions; ///< Examined positions. OpenPoints open_points; ///< Open points to examine further. - inline uint32 GetEstimate(int x, int y, int z); - void AddOpen(int x, int y, int z, uint32 traveled, const WalkedPosition *prev_pos); + inline uint32 GetEstimate(const XYZPoint16 &vox); + void AddOpen(const XYZPoint16 &vox, uint32 traveled, const WalkedPosition *prev_pos); }; #endif diff --git a/src/person.cpp b/src/person.cpp index fb9dacf..8df32dc 100644 --- a/src/person.cpp +++ b/src/person.cpp @@ -532,7 +532,7 @@ uint8 Guest::GetExitDirections(const Voxel *v, TileEdge start_edge, bool *seen_w */ static TileEdge GetParkEntryDirection(const XYZPoint16 &pos) { - PathSearcher ps(pos.x, pos.y, pos.z); // Current position is the destination. + PathSearcher ps(pos); // Current position is the destination. /* Add path tiles with a connection to outside the park to the initial starting points. */ for (int x = 0; x < _world.GetXSize() - 1; x++) { @@ -542,8 +542,9 @@ static TileEdge GetParkEntryDirection(const XYZPoint16 &pos) if (_world.GetStack(x + 1, y)->owner != OWN_PARK || _world.GetStack(x, y + 1)->owner != OWN_PARK) { int offset = vs->GetGroundOffset(); const Voxel *v = vs->voxels + offset; - if (HasValidPath(v) && GetImplodedPathSlope(v) < PATH_FLAT_COUNT) { - if ((GetPathExits(v) & ((1 << EDGE_SE) | (1 << EDGE_SW))) != 0) ps.AddStart(x, y, vs->base + offset); + if (HasValidPath(v) && GetImplodedPathSlope(v) < PATH_FLAT_COUNT && + (GetPathExits(v) & ((1 << EDGE_SE) | (1 << EDGE_SW))) != 0) { + ps.AddStart(XYZPoint16(x, y, vs->base + offset)); } } } else { @@ -551,8 +552,9 @@ static TileEdge GetParkEntryDirection(const XYZPoint16 &pos) if (vs->owner == OWN_PARK) { int offset = vs->GetGroundOffset(); const Voxel *v = vs->voxels + offset; - if (HasValidPath(v) && GetImplodedPathSlope(v) < PATH_FLAT_COUNT) { - if ((GetPathExits(v) & (1 << EDGE_NE)) != 0) ps.AddStart(x + 1, y, vs->base + offset); + if (HasValidPath(v) && GetImplodedPathSlope(v) < PATH_FLAT_COUNT && + (GetPathExits(v) & (1 << EDGE_NE)) != 0) { + ps.AddStart(XYZPoint16(x + 1, y, vs->base + offset)); } } @@ -560,8 +562,9 @@ static TileEdge GetParkEntryDirection(const XYZPoint16 &pos) if (vs->owner == OWN_PARK) { int offset = vs->GetGroundOffset(); const Voxel *v = vs->voxels + offset; - if (HasValidPath(v) && GetImplodedPathSlope(v) < PATH_FLAT_COUNT) { - if ((GetPathExits(v) & (1 << EDGE_NW)) != 0) ps.AddStart(x, y + 1, vs->base + offset); + if (HasValidPath(v) && GetImplodedPathSlope(v) < PATH_FLAT_COUNT && + (GetPathExits(v) & (1 << EDGE_NW)) != 0) { + ps.AddStart(XYZPoint16(x, y + 1, vs->base + offset)); } } } @@ -573,7 +576,7 @@ static TileEdge GetParkEntryDirection(const XYZPoint16 &pos) const WalkedPosition *prev = dest->prev_pos; if (prev == nullptr) return INVALID_EDGE; // Already at tile. - return GetAdjacentEdge(dest->x, dest->y, prev->x, prev->y); + return GetAdjacentEdge(dest->cur_vox.x, dest->cur_vox.y, prev->cur_vox.x, prev->cur_vox.y); } /** @@ -583,11 +586,11 @@ static TileEdge GetParkEntryDirection(const XYZPoint16 &pos) */ static TileEdge GetGoHomeDirection(const XYZPoint16 &pos) { - PathSearcher ps(pos.x, pos.y, pos.z); // Current position is the destination. + PathSearcher ps(pos); // Current position is the destination. int x = _guests.start_voxel.x; int y = _guests.start_voxel.y; - ps.AddStart(x, y, _world.GetGroundHeight(x, y)); + ps.AddStart(XYZPoint16(x, y, _world.GetGroundHeight(x, y))); if (!ps.Search()) return INVALID_EDGE; @@ -595,7 +598,7 @@ static TileEdge GetGoHomeDirection(const XYZPoint16 &pos) const WalkedPosition *prev = dest->prev_pos; if (prev == nullptr) return INVALID_EDGE; // Already at tile. - return GetAdjacentEdge(dest->x, dest->y, prev->x, prev->y); + return GetAdjacentEdge(dest->cur_vox.x, dest->cur_vox.y, prev->cur_vox.x, prev->cur_vox.y); } /** -- 2.2.1