void plAvBrainCritter::IEvalGoal() { // TODO: Implement pathfinding logic here // (for now, this runs directly towards the goal) fImmediateGoalPos = fFinalGoalPos; // where am I relative to my goal? const plSceneObject* creatureObj = fArmature->GetTarget(0); hsVector3 view(creatureObj->GetCoordinateInterface()->GetLocalToWorld().GetAxis(hsMatrix44::kView)); hsPoint3 creaturePos; hsQuat creatureRot; fAvMod->GetPositionAndRotationSim(&creaturePos, &creatureRot); hsVector3 goalVec(creaturePos - fImmediateGoalPos); goalVec.Normalize(); fDotGoal = goalVec * view; // 1 = directly facing, 0 = 90 deg off, -1 = facing away // calculate a vector pointing to the creature's right hsQuat invRot = creatureRot.Conjugate(); hsPoint3 globRight = invRot.Rotate(&kAvatarRight); fAngRight = globRight.InnerProduct(goalVec); // dot product, 1 = goal is 90 to the right, 0 = goal is in front or behind, -1 = goal is 90 to the left if (fAvoidingAvatars) { // check to see we can see anyone in our way (if we can't see them, we can't avoid them) std::vector<plArmatureMod*> playersICanSee = IAvatarsICanSee(); for (unsigned i = 0; i < playersICanSee.size(); ++i) { hsPoint3 avPos; hsQuat avRot; playersICanSee[i]->GetPositionAndRotationSim(&avPos, &avRot); hsVector3 avVec(creaturePos - avPos); avVec.Normalize(); float dotAv = avVec * goalVec; if (dotAv > 0.5f) // within a 45deg angle in front of us { // a player is in the way, so we will change our "goal" to a 90deg angle from the player // then we stop searching, since any other players in the way will just produce the same (or similar) result avVec.fZ = goalVec.fZ = 0.f; goalVec = goalVec % avVec; fAngRight = globRight.InnerProduct(goalVec); break; } } } // are we at our final goal? if (AtGoal()) { if (RunningBehavior(kDefaultRunBehName)) // don't do anything if we're not running! { // we're close enough, stop running and pick an idle fNextMode = IPickBehavior(kIdle); // tell everyone who cares that we have arrived for (unsigned i = 0; i < fReceivers.size(); ++i) { plAIArrivedAtGoalMsg* msg = new plAIArrivedAtGoalMsg(fArmature->GetKey(), fReceivers[i]); msg->Goal(fFinalGoalPos); msg->Send(); } } } }
void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, Path& path) { UpdateGrid(); PROFILE("ComputePath"); PathfinderState state = { 0 }; // Convert the start/end coordinates to tile indexes u16 i0, j0; NearestTile(x0, z0, i0, j0); NearestTile(goal.x, goal.z, state.iGoal, state.jGoal); // If we're already at the goal tile, then move directly to the exact goal coordinates if (AtGoal(i0, j0, goal)) { Waypoint w = { goal.x, goal.z }; path.m_Waypoints.push_back(w); return; } // If the target is a circle, we want to aim for the edge of it (so e.g. if we're inside // a large circle then the heuristics will aim us directly outwards); // otherwise just aim at the center point. (We'll never try moving outwards to a square shape.) if (goal.type == Goal::CIRCLE) state.rGoal = (goal.hw / (int)CELL_SIZE).ToInt_RoundToZero(); else state.rGoal = 0; state.passClass = passClass; state.moveCosts = m_MoveCosts.at(costClass); state.steps = 0; state.tiles = new PathfindTileGrid(m_MapSize, m_MapSize); state.terrain = m_Grid; state.iBest = i0; state.jBest = j0; state.hBest = CalculateHeuristic(i0, j0, state.iGoal, state.jGoal, state.rGoal); PriorityQueue::Item start = { std::make_pair(i0, j0), 0 }; state.open.push(start); state.tiles->get(i0, j0).SetStatusOpen(); state.tiles->get(i0, j0).SetPred(i0, j0, i0, j0); state.tiles->get(i0, j0).cost = 0; // To prevent units getting very stuck, if they start on an impassable tile // surrounded entirely by impassable tiles, we ignore the impassability state.ignoreImpassable = !IS_PASSABLE(state.terrain->get(i0, j0), state.passClass); while (1) { ++state.steps; // Hack to avoid spending ages computing giant paths, particularly when // the destination is unreachable if (state.steps > 40000) break; // If we ran out of tiles to examine, give up if (state.open.empty()) break; #if PATHFIND_STATS state.sumOpenSize += state.open.size(); #endif // Move best tile from open to closed PriorityQueue::Item curr = state.open.pop(); u16 i = curr.id.first; u16 j = curr.id.second; state.tiles->get(i, j).SetStatusClosed(); // If we've reached the destination, stop if (AtGoal(i, j, goal)) { state.iBest = i; state.jBest = j; state.hBest = 0; break; } // As soon as we find an escape route from the impassable terrain, // take it and forbid any further use of impassable tiles if (state.ignoreImpassable) { if (i > 0 && IS_PASSABLE(state.terrain->get(i-1, j), state.passClass)) state.ignoreImpassable = false; else if (i < m_MapSize-1 && IS_PASSABLE(state.terrain->get(i+1, j), state.passClass)) state.ignoreImpassable = false; else if (j > 0 && IS_PASSABLE(state.terrain->get(i, j-1), state.passClass)) state.ignoreImpassable = false; else if (j < m_MapSize-1 && IS_PASSABLE(state.terrain->get(i, j+1), state.passClass)) state.ignoreImpassable = false; } u32 g = state.tiles->get(i, j).cost; if (i > 0) ProcessNeighbour(i, j, i-1, j, g, state); if (i < m_MapSize-1) ProcessNeighbour(i, j, i+1, j, g, state); if (j > 0) ProcessNeighbour(i, j, i, j-1, g, state); if (j < m_MapSize-1) ProcessNeighbour(i, j, i, j+1, g, state); } // Reconstruct the path (in reverse) u16 ip = state.iBest, jp = state.jBest; while (ip != i0 || jp != j0) { PathfindTile& n = state.tiles->get(ip, jp); entity_pos_t x, z; TileCenter(ip, jp, x, z); Waypoint w = { x, z }; path.m_Waypoints.push_back(w); // Follow the predecessor link ip = n.GetPredI(ip); jp = n.GetPredJ(jp); } // Save this grid for debug display delete m_DebugGrid; m_DebugGrid = state.tiles; m_DebugSteps = state.steps; #if PATHFIND_STATS printf("PATHFINDER: steps=%d avgo=%d proc=%d impc=%d impo=%d addo=%d\n", state.steps, state.sumOpenSize/state.steps, state.numProcessed, state.numImproveClosed, state.numImproveOpen, state.numAddToOpen); #endif }