/*
* 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;
}
void CEnergyGrid::UpdateVis()
{
if (!isVis) {
return;
}
Map* map = circuit->GetMap();
Figure* fig = circuit->GetDrawer()->GetFigure();
fig->Remove(figureFinishedId);
fig->Remove(figureBuildId);
fig->Remove(figureInvalidId);
fig->Remove(figureGridId);
// create new figure groups
figureFinishedId = fig->DrawLine(ZeroVector, ZeroVector, 0.0f, false, FRAMES_PER_SEC * 300, 0);
figureBuildId = fig->DrawLine(ZeroVector, ZeroVector, 0.0f, false, FRAMES_PER_SEC * 300, 0);
figureInvalidId = fig->DrawLine(ZeroVector, ZeroVector, 0.0f, false, FRAMES_PER_SEC * 300, 0);
figureGridId = fig->DrawLine(ZeroVector, ZeroVector, 0.0f, false, FRAMES_PER_SEC * 300, 0);
for (const CEnergyLink& link : links) {
int figureId;
float height = 20.0f;
if (link.IsFinished()) {
figureId = figureFinishedId;
height = 18.0f;
} else if (link.IsBeingBuilt()) {
figureId = figureBuildId;
} else if (!link.IsValid()) {
figureId = figureInvalidId;
} else {
figureId = figureGridId;
height = 18.0f;
}
AIFloat3 pos0 = link.GetV0()->pos;
const AIFloat3 dir = (link.GetV1()->pos - pos0) / 10;
pos0.y = map->GetElevationAt(pos0.x, pos0.z) + height;
for (int i = 0; i < 10; ++i) {
AIFloat3 pos1 = pos0 + dir;
pos1.y = map->GetElevationAt(pos1.x, pos1.z) + height;
fig->DrawLine(pos0, pos1, 16.0f, false, FRAMES_PER_SEC * 300, figureId);
pos0 = pos1;
}
}
fig->SetColor(figureFinishedId, AIColor(0.1f, 0.3f, 1.0f), 255);
fig->SetColor(figureBuildId, AIColor(1.0f, 1.0f, 0.0f), 255);
fig->SetColor(figureInvalidId, AIColor(1.0f, 0.3f, 0.3f), 255);
fig->SetColor(figureGridId, AIColor(0.5f, 0.5f, 0.5f), 255);
// Draw planned Kruskal
fig->Remove(figureKruskalId);
figureKruskalId = fig->DrawLine(ZeroVector, ZeroVector, 0.0f, false, FRAMES_PER_SEC * 300, 0);
const CMetalData::Clusters& clusters = circuit->GetMetalManager()->GetClusters();
const CMetalData::Graph& clusterGraph = circuit->GetMetalManager()->GetGraph();
for (const CMetalData::EdgeDesc& edge : spanningTree) {
const AIFloat3& posFrom = clusters[boost::source(edge, clusterGraph)].geoCentr;
const AIFloat3& posTo = clusters[boost::target(edge, clusterGraph)].geoCentr;
AIFloat3 pos0 = posFrom;
const AIFloat3 dir = (posTo - pos0) / 10;
pos0.y = map->GetElevationAt(pos0.x, pos0.z) + 19.0f;
for (int i = 0; i < 10; ++i) {
AIFloat3 pos1 = pos0 + dir;
pos1.y = map->GetElevationAt(pos1.x, pos1.z) + 19.0f;
fig->DrawLine(pos0, pos1, 16.0f, false, FRAMES_PER_SEC * 300, figureKruskalId);
pos0 = pos1;
}
}
fig->SetColor(figureKruskalId, AIColor(0.0f, 1.0f, 1.0f), 255);
delete fig;
}
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;
}