/*
* radius is in full res.
* returns the path cost.
*/
float CPathFinder::MakePath(F3Vec& posPath, float3& startPos, float3& endPos, int radius) {
ai->math->TimerStart();
path.clear();
ai->math->F3MapBound(startPos);
ai->math->F3MapBound(endPos);
float totalcost = 0.0f;
int ex = int(endPos.x / (8 * resmodifier));
int ey = int(endPos.z / (8 * resmodifier));
int sy = int(startPos.z / (8 * resmodifier));
int sx = int(startPos.x / (8 * resmodifier));
radius /= int(8 * resmodifier);
if (micropather->FindBestPathToPointOnRadius(XY2Node(sx, sy), XY2Node(ex, ey), &path, &totalcost, radius) == MicroPather::SOLVED) {
posPath.reserve(path.size());
for (unsigned i = 0; i < path.size(); i++) {
float3 mypos = Node2Pos(path[i]);
mypos.y = ai->cb->GetElevation(mypos.x, mypos.z);
posPath.push_back(mypos);
}
}
return totalcost;
}
void CPathFinder::UpdateVis(const F3Vec& path)
{
if (!isVis) {
return;
}
Figure* fig = circuit->GetDrawer()->GetFigure();
int figId = fig->DrawLine(ZeroVector, ZeroVector, 16.0f, true, FRAMES_PER_SEC * 5, 0);
for (unsigned i = 1; i < path.size(); ++i) {
fig->DrawLine(path[i - 1], path[i], 16.0f, true, FRAMES_PER_SEC * 20, figId);
}
fig->SetColor(figId, AIColor((float)rand() / RAND_MAX, (float)rand() / RAND_MAX, (float)rand() / RAND_MAX), 255);
delete fig;
}
/*
* radius is in full res.
* returns the path cost.
*/
float CPathFinder::MakePath(F3Vec& posPath, AIFloat3& startPos, AIFloat3& endPos, int radius)
{
path.clear();
terrainData->CorrectPosition(startPos);
terrainData->CorrectPosition(endPos);
float pathCost = 0.0f;
const int ex = int(endPos.x / squareSize);
const int ey = int(endPos.z / squareSize);
const int sy = int(startPos.z / squareSize);
const int sx = int(startPos.x / squareSize);
radius /= squareSize;
if (micropather->FindBestPathToPointOnRadius(XY2Node(sx, sy), XY2Node(ex, ey), &path, &pathCost, radius) == CMicroPather::SOLVED) {
posPath.reserve(path.size());
// TODO: Consider performing transformations in place where move_along_path executed.
// Current task implementations recalc path every ~2 seconds,
// therefore only first few positions actually used.
Map* map = terrainData->GetMap();
for (void* node : path) {
float3 mypos = Node2Pos(node);
mypos.y = map->GetElevationAt(mypos.x, mypos.z);
posPath.push_back(mypos);
}
}
#ifdef DEBUG_VIS
UpdateVis(posPath);
#endif
return pathCost;
}
float CPathFinder::FindBestPathToRadius(F3Vec& posPath, AIFloat3& startPos, float radiusAroundTarget, const AIFloat3& target)
{
F3Vec posTargets;
posTargets.push_back(target);
return FindBestPath(posPath, startPos, radiusAroundTarget, posTargets);
}
float CPathFinder::FindBestPath(F3Vec& posPath, AIFloat3& startPos, float maxRange, F3Vec& possibleTargets)
{
float pathCost = 0.0f;
// <maxRange> must always be >= squareSize, otherwise
// <radius> will become 0 and the write to offsets[0]
// below is undefined
if (maxRange < float(squareSize)) {
return pathCost;
}
path.clear();
const unsigned int radius = maxRange / squareSize;
unsigned int offsetSize = 0;
std::vector<std::pair<int, int> > offsets;
std::vector<int> xend;
// make a list with the points that will count as end nodes
std::vector<void*> endNodes;
endNodes.reserve(possibleTargets.size() * radius * 10);
{
const unsigned int DoubleRadius = radius * 2;
const unsigned int SquareRadius = radius * radius;
xend.resize(DoubleRadius + 1);
offsets.resize(DoubleRadius * 5);
for (size_t a = 0; a < DoubleRadius + 1; a++) {
const float z = (int) (a - radius);
const float floatsqrradius = SquareRadius;
xend[a] = int(sqrt(floatsqrradius - z * z));
}
offsets[0].first = 0;
offsets[0].second = 0;
size_t index = 1;
size_t index2 = 1;
for (size_t a = 1; a < radius + 1; a++) {
int endPosIdx = xend[a];
int startPosIdx = xend[a - 1];
while (startPosIdx <= endPosIdx) {
assert(index < offsets.size());
offsets[index].first = startPosIdx;
offsets[index].second = a;
startPosIdx++;
index++;
}
startPosIdx--;
}
index2 = index;
for (size_t a = 0; a < index2 - 2; a++) {
assert(index < offsets.size());
assert(a < offsets.size());
offsets[index].first = offsets[a].first;
offsets[index].second = DoubleRadius - (offsets[a].second);
index++;
}
index2 = index;
for (size_t a = 0; a < index2; a++) {
assert(index < offsets.size());
assert(a < offsets.size());
offsets[index].first = -(offsets[a].first);
offsets[index].second = offsets[a].second;
index++;
}
for (size_t a = 0; a < index; a++) {
assert(a < offsets.size());
// offsets[a].first = offsets[a].first; // ??
offsets[a].second = offsets[a].second - radius;
}
offsetSize = index;
}
std::vector<void*> nodeTargets;
nodeTargets.reserve(possibleTargets.size());
for (unsigned int i = 0; i < possibleTargets.size(); i++) {
AIFloat3& f = possibleTargets[i];
terrainData->CorrectPosition(f);
void* node = Pos2Node(f);
NSMicroPather::PathNode* pn = micropather->GetNode((size_t)node);
if (pn->isTarget) {
continue;
}
pn->isTarget = 1;
nodeTargets.push_back(node);
int x, y;
Node2XY(node, &x, &y);
for (unsigned int j = 0; j < offsetSize; j++) {
const int sx = x + offsets[j].first;
const int sy = y + offsets[j].second;
if (sx >= 0 && sx < pathMapXSize && sy >= 0 && sy < pathMapYSize) {
endNodes.push_back(XY2Node(sx, sy));
}
}
}
for (void* node : nodeTargets) {
micropather->GetNode((size_t)node)->isTarget = 0;
}
terrainData->CorrectPosition(startPos);
if (micropather->FindBestPathToAnyGivenPoint(Pos2Node(startPos), endNodes, nodeTargets, &path, &pathCost) == CMicroPather::SOLVED) {
posPath.reserve(path.size());
Map* map = terrainData->GetMap();
for (unsigned i = 0; i < path.size(); i++) {
float3 mypos = Node2Pos(path[i]);
mypos.y = map->GetElevationAt(mypos.x, mypos.z);
posPath.push_back(mypos);
}
}
#ifdef DEBUG_VIS
UpdateVis(posPath);
#endif
return pathCost;
}
float CPathFinder::FindBestPath(F3Vec& posPath, float3& startPos, float myMaxRange, F3Vec& possibleTargets) {
ai->math->TimerStart();
path.clear();
// make a list with the points that will count as end nodes
float totalcost = 0.0f;
const unsigned int radius = int(myMaxRange / (8 * resmodifier));
int offsetSize = 0;
std::vector<std::pair<int, int> > offsets;
std::vector<int> xend;
std::vector<void*> endNodes;
endNodes.reserve(possibleTargets.size() * radius * 10);
{
const unsigned int DoubleRadius = radius * 2;
const unsigned int SquareRadius = radius * radius;
xend.resize(DoubleRadius + 1);
offsets.resize(DoubleRadius * 5);
for (size_t a = 0; a < DoubleRadius + 1; a++) {
const float z = (int) (a - radius);
const float floatsqrradius = SquareRadius;
xend[a] = int(sqrt(floatsqrradius - z * z));
}
offsets[0].first = 0;
offsets[0].second = 0;
size_t index = 1;
size_t index2 = 1;
for (size_t a = 1; a < radius + 1; a++) {
int endPosIdx = xend[a];
int startPosIdx = xend[a - 1];
while (startPosIdx <= endPosIdx) {
assert(index < offsets.size());
offsets[index].first = startPosIdx;
offsets[index].second = a;
startPosIdx++;
index++;
}
startPosIdx--;
}
index2 = index;
for (size_t a = 0; a < index2 - 2; a++) {
assert(index < offsets.size());
assert(a < offsets.size());
offsets[index].first = offsets[a].first;
offsets[index].second = DoubleRadius - (offsets[a].second);
index++;
}
index2 = index;
for (size_t a = 0; a < index2; a++) {
assert(index < offsets.size());
assert(a < offsets.size());
offsets[index].first = -(offsets[a].first);
offsets[index].second = offsets[a].second;
index++;
}
for (size_t a = 0; a < index; a++) {
assert(a < offsets.size());
offsets[a].first = offsets[a].first; // ??
offsets[a].second = offsets[a].second - radius;
}
offsetSize = index;
}
for (unsigned i = 0; i < possibleTargets.size(); i++) {
float3& f = possibleTargets[i];
int x, y;
// TODO: make the circle here
ai->math->F3MapBound(f);
Node2XY(Pos2Node(f), &x, &y);
for (int j = 0; j < offsetSize; j++) {
const int sx = x + offsets[j].first;
const int sy = y + offsets[j].second;
if (sx >= 0 && sx < PathMapXSize && sy >= 0 && sy < PathMapYSize) {
endNodes.push_back(XY2Node(sx, sy));
}
}
}
ai->math->F3MapBound(startPos);
if (micropather->FindBestPathToAnyGivenPoint(Pos2Node(startPos), endNodes, &path, &totalcost) == MicroPather::SOLVED) {
posPath.reserve(path.size());
for (unsigned i = 0; i < path.size(); i++) {
int x, y;
Node2XY(path[i], &x, &y);
float3 mypos = Node2Pos(path[i]);
mypos.y = ai->cb->GetElevation(mypos.x, mypos.z);
posPath.push_back(mypos);
}
}
return totalcost;
}