mirror of
https://github.com/nzp-team/quakec.git
synced 2024-11-16 01:02:11 +00:00
490 lines
No EOL
10 KiB
C++
490 lines
No EOL
10 KiB
C++
/*
|
|
server/ai/pathfind_core.qc
|
|
|
|
generic waypoint pathfinding
|
|
|
|
Copyright (C) 2021 NZ:P Team
|
|
|
|
This program is free software; you can redistribute it and/or
|
|
modify it under the terms of the GNU General Public License
|
|
as published by the Free Software Foundation; either version 2
|
|
of the License, or (at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
|
|
See the GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program; if not, write to:
|
|
|
|
Free Software Foundation, Inc.
|
|
59 Temple Place - Suite 330
|
|
Boston, MA 02111-1307, USA
|
|
|
|
*/
|
|
|
|
void() Load_Waypoints_Legacy;
|
|
|
|
float Distance (vector from, vector to) {
|
|
return vlen(from - to);
|
|
}
|
|
|
|
float HeuristicCostEstimate (float start_way, float end_way)
|
|
{
|
|
//for now we will just look the distance between.
|
|
return Distance(waypoints[start_way].org, waypoints[end_way].org);
|
|
}
|
|
|
|
void ReconstructPath(entity z, float start_way, float end_way) {
|
|
|
|
}
|
|
|
|
float open_first;
|
|
float open;
|
|
float closed_init;
|
|
float closed;
|
|
float closed_first;
|
|
void SV_WP_ClearSet()
|
|
{
|
|
float i;
|
|
|
|
for (i = 0; i < MAX_WAYPOINTS; i = i + 1)
|
|
{
|
|
waypoints[i].set = 0;
|
|
}
|
|
}
|
|
|
|
float IsActive(float way) {
|
|
if (waypoints[way].targetdoor != "") {
|
|
entity door;
|
|
|
|
door = find(world, wayTarget, waypoints[way].targetdoor);
|
|
|
|
if (door != world) {
|
|
if (door.state == STATE_BOTTOM) {
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
void SV_WP_SetAdd(float wp, float isopen)
|
|
{
|
|
if (isopen)
|
|
{
|
|
// we only get added here from none, so no cleanup required
|
|
// gotta check if last entry was removed before this call
|
|
if (waypoints[open_first].set == SET_CLOSED)
|
|
{
|
|
//Con_Printf("Empty set: %i is new first\n", wp);
|
|
open_first = wp;
|
|
waypoints[wp].prev = wp;
|
|
}
|
|
else
|
|
{
|
|
waypoints[wp].prev = open;
|
|
waypoints[open].next = wp;
|
|
}
|
|
waypoints[wp].next = wp;
|
|
waypoints[wp].set = SET_OPEN;
|
|
open = wp;
|
|
}
|
|
else
|
|
{
|
|
// even if wp is the only one in the set, it doesn't really matter
|
|
if (wp == open_first)
|
|
open_first = waypoints[wp].next;
|
|
if (wp == open)
|
|
open = waypoints[wp].prev;
|
|
|
|
// update links so open set doesn't get fucked
|
|
waypoints[waypoints[wp].prev].next = waypoints[wp].next;
|
|
waypoints[waypoints[wp].next].prev = waypoints[wp].prev;
|
|
|
|
if (!closed_init)
|
|
{
|
|
closed_first = wp;
|
|
waypoints[wp].prev = wp;
|
|
closed_init = true;
|
|
}
|
|
else
|
|
{
|
|
waypoints[closed].next = wp;
|
|
waypoints[wp].prev = closed;
|
|
}
|
|
waypoints[wp].next = wp;
|
|
waypoints[wp].set = SET_CLOSED;
|
|
closed = wp;
|
|
}
|
|
}
|
|
|
|
float Pathfind (entity z, float s_wp, float e_wp) {
|
|
float current;
|
|
float i, j;
|
|
float bestf;
|
|
|
|
|
|
if (s_wp == e_wp) {
|
|
return s_wp;
|
|
}
|
|
|
|
SV_WP_ClearSet();
|
|
|
|
open_first = s_wp;
|
|
open = s_wp;
|
|
waypoints[s_wp].next = s_wp;
|
|
waypoints[s_wp].prev = s_wp;
|
|
waypoints[s_wp].set = SET_OPEN;
|
|
waypoints[s_wp].g = 0;
|
|
waypoints[s_wp].f = HeuristicCostEstimate(s_wp, e_wp);
|
|
waypoints[s_wp].step = s_wp;
|
|
current = s_wp;
|
|
|
|
closed_init = false;
|
|
|
|
// while openset is not empty
|
|
while (waypoints[open_first].set == SET_OPEN) {
|
|
i = open_first;
|
|
|
|
bestf = 320000;
|
|
//print("Current ", ftos(current), "\n");
|
|
|
|
// go from first open to last, pick the one with lowest f
|
|
do {
|
|
if (waypoints[i].f < bestf) {
|
|
current = i;
|
|
bestf = waypoints[i].f;
|
|
}
|
|
i = waypoints[i].next;
|
|
} while (i != open);
|
|
|
|
// mark node as visited
|
|
//print("Removing ", ftos(current), " from open\n");
|
|
SV_WP_SetAdd(current, false);
|
|
|
|
// we found a result, loop back with pathlength
|
|
if (current == e_wp) {
|
|
j = i = current;
|
|
// walk constructed path backwards
|
|
// keep 2 next steps with us
|
|
//print("Result\n");
|
|
//print(ftos(current), "\n");
|
|
while (waypoints[current].step != current) {
|
|
j = i; // 3rd waypoint along path
|
|
i = current; // 2nd waypoint along path
|
|
current = waypoints[current].step; // Start waypoint
|
|
}
|
|
|
|
// go to the furthest one on the path that we can actually see
|
|
// If within a radius of 6 qu of the first waypoint along the path,
|
|
// Always return the 2nd waypoint along the path
|
|
float is_at_first_waypoint = vlen(z.origin - waypoints[s_wp].org) <= 6;
|
|
|
|
|
|
|
|
// Check if we can tracemove to the 3rd waypoint along the path
|
|
if (tracemove(z.origin,VEC_HULL_MIN,VEC_HULL_MAX,waypoints[i].org,TRUE,z))
|
|
{
|
|
//VectorCopy(waypoints[i].origin, res);
|
|
//print("Giving third wp ", ftos(i), "\n");
|
|
return i;
|
|
}
|
|
// Check if we can tracemove to the 2nd waypoint along the path
|
|
else if (is_at_first_waypoint || tracemove(z.origin,VEC_HULL_MIN,VEC_HULL_MAX,waypoints[j].org,TRUE,z))
|
|
{
|
|
//VectorCopy(waypoints[j].origin, res);
|
|
//print("Giving second wp ", ftos(j), "\n");
|
|
return j;
|
|
}
|
|
else
|
|
{
|
|
//VectorCopy(waypoints[current].origin, res);
|
|
//print("Giving first wp ", ftos(current), "\n");
|
|
return current;
|
|
}
|
|
}
|
|
|
|
|
|
// check neighbours to add to openset
|
|
for (j = 0; j < MAX_WAY_TARGETS; j++)
|
|
{
|
|
i = waypoints[current].target_id[j];
|
|
// Skip empty neighbor slots
|
|
if(i == -1) {
|
|
continue;
|
|
}
|
|
|
|
if (waypoints[i].set != SET_CLOSED && i != current && IsActive(i))
|
|
{
|
|
bestf = waypoints[current].g + HeuristicCostEstimate(i, current);
|
|
if (waypoints[i].set == SET_NONE || bestf < waypoints[i].g)
|
|
{
|
|
waypoints[i].step = current;
|
|
waypoints[i].g = bestf;
|
|
waypoints[i].f = bestf + HeuristicCostEstimate(i, e_wp);
|
|
|
|
if (waypoints[i].set == SET_NONE)
|
|
{
|
|
//print("Adding ", ftos(i), " into open\n");
|
|
SV_WP_SetAdd(i, true);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
print("Waypointing failed\n");
|
|
return -1;
|
|
}
|
|
|
|
|
|
float Do_Pathfind(entity from, entity to) {
|
|
|
|
float i;
|
|
float TraceResult;
|
|
|
|
float dist, best_dist, best, best_target;
|
|
|
|
best = 0;
|
|
best_target = 0;
|
|
dist = 0;
|
|
best_dist = 100000000;
|
|
|
|
#ifdef FTE
|
|
for (i = 0; i < MAX_WAYPOINTS; i = i + 1) {
|
|
// Skip unused waypoint slots
|
|
if(waypoints[i].id < 0) {
|
|
continue;
|
|
}
|
|
|
|
if (IsActive(i)) {
|
|
TraceResult = tracemove (from.origin, VEC_HULL_MIN, VEC_HULL_MAX, waypoints[i].org, TRUE ,from);
|
|
if (TraceResult) {
|
|
dist = Distance(waypoints[i].org, from.origin);
|
|
|
|
if(dist < best_dist) {
|
|
best_dist = dist;
|
|
best = i;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
dist = 0;
|
|
best_dist = 100000000;
|
|
|
|
for (i = 0; i < MAX_WAYPOINTS; i = i + 1) {
|
|
// Skip unused waypoint slots
|
|
if(waypoints[i].id < 0) {
|
|
continue;
|
|
}
|
|
|
|
if (IsActive(i)) {
|
|
TraceResult = tracemove (to.origin, VEC_HULL_MIN, VEC_HULL_MAX, waypoints[i].org, TRUE ,to);
|
|
if (TraceResult) {
|
|
dist = Distance(waypoints[i].org, to.origin);
|
|
|
|
if(dist < best_dist) {
|
|
best_dist = dist;
|
|
best_target = i;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//print("Starting waypoint: ", ftos(best)," Ending waypoint: ", ftos(best_target), "\n");
|
|
|
|
|
|
|
|
|
|
return Pathfind(from, best, best_target);
|
|
#else
|
|
return 0;
|
|
#endif
|
|
}
|
|
|
|
void CalcDistances() {
|
|
float i, s;
|
|
|
|
for (i = 1; i < MAX_WAYPOINTS; i++) {
|
|
waypoint_ai way;
|
|
way = waypoints[i];
|
|
if (way.id > 0) {
|
|
for (s = 0; s < MAX_WAY_TARGETS; s++) {
|
|
float targ;
|
|
targ = way.target_id[s];
|
|
if (targ > 0) {
|
|
way.dist[s] = Distance(way.org, waypoints[targ].org);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void creator_way_touch();
|
|
void LoadWaypointData() {
|
|
float file, point;
|
|
string h;
|
|
float targetcount, loop, waycount;
|
|
entity new_way;
|
|
|
|
h = strcat(mappath, ".way");
|
|
file = fopen (h, FILE_READ);
|
|
|
|
if (file == -1)
|
|
{
|
|
dprint("Error: file not found \n");
|
|
return;
|
|
}
|
|
|
|
float i, s;
|
|
|
|
|
|
#ifdef FTE
|
|
for (i = 0; i < MAX_WAYPOINTS; i = i + 1) {
|
|
waypoint_ai way;
|
|
|
|
way = waypoints[i];
|
|
way.org = '0 0 0';
|
|
way.id = 0;
|
|
way.g = 0;
|
|
way.f = 0;
|
|
way.set = 0;
|
|
way.targetdoor = "";
|
|
|
|
for (s = 0; s < MAX_WAY_TARGETS; s = s + 1) {
|
|
way.target_id[s] = 0;
|
|
way.dist[s] = 0;
|
|
}
|
|
|
|
}
|
|
#endif
|
|
|
|
new_way = spawn();
|
|
point = 0;
|
|
waycount = 0;
|
|
targetcount = 0;
|
|
loop = 1;
|
|
while (loop)
|
|
{
|
|
string line;
|
|
line = fgets(file);
|
|
if not (line) {
|
|
//bprint(PRINT_HIGH, "End of file\n");
|
|
loop = 0;
|
|
break;
|
|
}
|
|
h = strtrim(line);
|
|
|
|
//print(h, "\n");
|
|
if (h == "") {
|
|
continue;
|
|
}
|
|
|
|
switch (point) {
|
|
case 0:
|
|
if (h == "waypoint") {
|
|
new_way = spawn();
|
|
new_way.solid = SOLID_TRIGGER;
|
|
|
|
setmodel(new_way, "models/way/normal_way.spr");
|
|
new_way.classname = "waypoint";
|
|
|
|
new_way.touch = creator_way_touch;
|
|
point = 1;
|
|
targetcount = 0;
|
|
} else if (h == "Waypoint") {
|
|
//bprint(PRINT_HIGH, "Identified .way as legacy..\n");
|
|
point = 99;
|
|
Load_Waypoints_Legacy();
|
|
} else {
|
|
bprint(PRINT_HIGH, strcat("Error: unknown point ", strcat(h, "\n")));
|
|
}
|
|
break;
|
|
case 1:
|
|
if (h == "{") {
|
|
point = 2;
|
|
} else {
|
|
bprint(PRINT_HIGH, strcat("Error: unknown variable ", strcat(h, " expected {\n")));
|
|
}
|
|
break;
|
|
case 2:
|
|
tokenize(h);
|
|
|
|
string value, variable;
|
|
variable = strtrim(argv(0));
|
|
value = strtrim(argv(2));
|
|
|
|
switch (variable) {
|
|
case "origin":
|
|
setorigin(new_way, stov(value));
|
|
break;
|
|
case "id":
|
|
new_way.waynum = value;
|
|
break;
|
|
case "door":
|
|
new_way.targetname = value;
|
|
break;
|
|
case "targets":
|
|
point = 3;
|
|
break;
|
|
case "}":
|
|
float id = stof(new_way.waynum);
|
|
waypoint_ai waypoint;
|
|
waypoint = waypoints[id];
|
|
|
|
waypoints[id].id = id;
|
|
waypoints[id].org = new_way.origin;
|
|
waypoints[id].targetdoor = new_way.targetname;
|
|
|
|
for (i = 0; i < MAX_WAY_TARGETS; i++) {
|
|
waypoints[id].target_id[i] = stof(new_way.targets[i]);
|
|
}
|
|
|
|
if (!cvar("waypoint_mode")) {
|
|
remove(new_way);
|
|
}
|
|
|
|
|
|
point = 0;
|
|
break;
|
|
default:
|
|
bprint(PRINT_HIGH, strcat("Error: unknown variable ", strcat(variable, "\n")));
|
|
break;
|
|
}
|
|
break;
|
|
case 3:
|
|
if (h == "[") {
|
|
point = 4;
|
|
} else {
|
|
bprint(PRINT_HIGH, strcat("Error: unknown variable ", strcat(h, " expected [\n")));
|
|
}
|
|
break;
|
|
case 4:
|
|
if (targetcount >= MAX_WAY_TARGETS) {
|
|
bprint(PRINT_HIGH, "Error: Target count too high for waypoint\n");
|
|
} else if (h == "]") {
|
|
point = 2;
|
|
} else {
|
|
new_way.targets[targetcount] = h;
|
|
targetcount++;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (new_way) {
|
|
if (!cvar("waypoint_mode")) {
|
|
remove(new_way);
|
|
}
|
|
}
|
|
|
|
fclose(file);
|
|
|
|
#ifdef FTE
|
|
CalcDistances();
|
|
#endif
|
|
} |