Sector *Set::findPointSector(const Math::Vector3d &p, Sector::SectorType type) { for (int i = 0; i < _numSectors; i++) { Sector *sector = _sectors[i]; if (sector && (sector->getType() & type) && sector->isVisible() && sector->isPointInSector(p)) return sector; } return NULL; }
void Set::findClosestSector(const Math::Vector3d &p, Sector **sect, Math::Vector3d *closestPoint) { Sector *resultSect = NULL; Math::Vector3d resultPt = p; float minDist = 0.0; for (int i = 0; i < _numSectors; i++) { Sector *sector = _sectors[i]; if ((sector->getType() & Sector::WalkType) == 0 || !sector->isVisible()) continue; Math::Vector3d closestPt = sector->getClosestPoint(p); float thisDist = (closestPt - p).getMagnitude(); if (!resultSect || thisDist < minDist) { resultSect = sector; resultPt = closestPt; minDist = thisDist; } } if (sect) *sect = resultSect; if (closestPoint) *closestPoint = resultPt; }
void Actor::walkTo(const Math::Vector3d &p) { if (p == _pos) _walking = false; else { _walking = true; _destPos = p; _path.clear(); if (_constrain) { g_grim->getCurrSet()->findClosestSector(p, NULL, &_destPos); Common::List<PathNode *> openList; Common::List<PathNode *> closedList; PathNode *start = new PathNode; start->parent = NULL; start->pos = _pos; start->dist = 0.f; start->cost = 0.f; openList.push_back(start); g_grim->getCurrSet()->findClosestSector(_pos, &start->sect, NULL); Common::List<Sector *> sectors; for (int i = 0; i < g_grim->getCurrSet()->getSectorCount(); ++i) { Sector *s = g_grim->getCurrSet()->getSectorBase(i); int type = s->getType(); if ((type == Sector::WalkType || type == Sector::HotType || type == Sector::FunnelType) && s->isVisible()) { sectors.push_back(s); } } Sector *endSec = NULL; g_grim->getCurrSet()->findClosestSector(_destPos, &endSec, NULL); do { PathNode *node = NULL; float cost = -1.f; for (Common::List<PathNode *>::iterator j = openList.begin(); j != openList.end(); ++j) { PathNode *n = *j; float c = n->dist + n->cost; if (cost < 0.f || c < cost) { cost = c; node = n; } } closedList.push_back(node); openList.remove(node); Sector *sector = node->sect; if (sector == endSec) { PathNode *n = closedList.back(); // Don't put the start position in the list, or else // the first angle calculated in updateWalk() will be // meaningless. The only node without parent is the start // one. while (n->parent) { _path.push_back(n->pos); n = n->parent; } break; } for (Common::List<Sector *>::iterator i = sectors.begin(); i != sectors.end(); ++i) { Sector *s = *i; bool inClosed = false; for (Common::List<PathNode *>::iterator j = closedList.begin(); j != closedList.end(); ++j) { if ((*j)->sect == s) { inClosed = true; break; } } if (inClosed) continue; Common::List<Math::Line3d> bridges = sector->getBridgesTo(s); if (bridges.empty()) continue; // The sectors are not adjacent. Math::Vector3d closestPoint = s->getClosestPoint(_destPos); Math::Vector3d best; float bestDist = 1e6f; Math::Line3d l(node->pos, closestPoint); while (!bridges.empty()) { Math::Line3d bridge = bridges.back(); Math::Vector3d pos; const bool useXZ = (g_grim->getGameType() == GType_MONKEY4); if (!bridge.intersectLine2d(l, &pos, useXZ)) { pos = bridge.middle(); } float dist = (pos - closestPoint).getMagnitude(); if (dist < bestDist) { bestDist = dist; best = pos; } bridges.pop_back(); } best = handleCollisionTo(node->pos, best); PathNode *n = NULL; for (Common::List<PathNode *>::iterator j = openList.begin(); j != openList.end(); ++j) { if ((*j)->sect == s) { n = *j; break; } } if (n) { float newCost = node->cost + (best - node->pos).getMagnitude(); if (newCost < n->cost) { n->cost = newCost; n->parent = node; n->pos = best; n->dist = (n->pos - _destPos).getMagnitude(); } } else { n = new PathNode; n->parent = node; n->sect = s; n->pos = best; n->dist = (n->pos - _destPos).getMagnitude(); n->cost = node->cost + (n->pos - node->pos).getMagnitude(); openList.push_back(n); } } } while (!openList.empty()); for (Common::List<PathNode *>::iterator j = closedList.begin(); j != closedList.end(); ++j) { delete *j; } for (Common::List<PathNode *>::iterator j = openList.begin(); j != openList.end(); ++j) { delete *j; } } _path.push_front(_destPos); } }