mirror of
https://github.com/nzp-team/quakec.git
synced 2025-02-25 05:01:05 +00:00
5e8cd1e655
Adds traversal logic for climbing ledge Adds traversal logic for climbing short ledge Adds traversal logic for dropping down from a ledge
1305 lines
48 KiB
C++
1305 lines
48 KiB
C++
/*
|
|
server/ai/pathfind_core.qc
|
|
|
|
Navmesh-based 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
|
|
|
|
*/
|
|
|
|
//===========================================================================================================
|
|
//==================================== Navmesh-based Pathfinding Functions ==================================
|
|
//===========================================================================================================
|
|
|
|
// Calculates world-space coordinate of traversal endpoint
|
|
vector sv_navmesh_get_traversal_midpoint_pos(float traversal_index) {
|
|
vector start_pos = sv_navmesh_traversals[traversal_index].start_pos;
|
|
vector midpoint_pos = sv_navmesh_traversals[traversal_index].midpoint_pos;
|
|
// vector end_pos = sv_navmesh_traversals[traversal_index].end_pos;
|
|
float angle = sv_navmesh_traversals[traversal_index].angle;
|
|
// Midpoint / endpoint pos are relative to start_pos:
|
|
makevectors([0, angle, 0]);
|
|
midpoint_pos = start_pos + (v_right * midpoint_pos.x) + (v_forward * midpoint_pos.y) + (v_up * midpoint_pos.z);
|
|
// end_pos = start_pos + (v_right * end_pos.x) + (v_forward * end_pos.y) + (v_up * end_pos.z);
|
|
return midpoint_pos;
|
|
}
|
|
|
|
// Calculates world-space coordinate of traversal midpoint
|
|
vector sv_navmesh_get_traversal_end_pos(float traversal_index) {
|
|
vector start_pos = sv_navmesh_traversals[traversal_index].start_pos;
|
|
// vector midpoint_pos = sv_navmesh_traversals[traversal_index].midpoint_pos;
|
|
vector end_pos = sv_navmesh_traversals[traversal_index].end_pos;
|
|
float angle = sv_navmesh_traversals[traversal_index].angle;
|
|
// Midpoint / endpoint pos are relative to start_pos:
|
|
makevectors([0, angle, 0]);
|
|
// midpoint_pos = start_pos + (v_right * midpoint_pos.x) + (v_forward * midpoint_pos.y) + (v_up * midpoint_pos.z);
|
|
end_pos = start_pos + (v_right * end_pos.x) + (v_forward * end_pos.y) + (v_up * end_pos.z);
|
|
return end_pos;
|
|
}
|
|
|
|
// Calculates the heuristic h score value for a navmesh polygon (Our best guess for how far this node is from the goal node)
|
|
float sv_pathfind_calc_poly_h_score(float current_poly_idx, float goal_poly_idx) {
|
|
//FIXME: we could just as easily return vlen()^2 for comparisons... (saves a sqrt operation)
|
|
return vlen(sv_navmesh_polies[goal_poly_idx].center - sv_navmesh_polies[current_poly_idx].center);
|
|
}
|
|
|
|
// Calculates the heuristic h score value for a navmesh traversal (Our best guess for how far this node is from the goal node)
|
|
float sv_pathfind_calc_traversal_h_score(float current_traversal_idx, float goal_poly_idx) {
|
|
//FIXME: we could just as easily return vlen()^2 for comparisons... (saves a sqrt operation)
|
|
vector traversal_end_pos = sv_navmesh_get_traversal_end_pos(current_traversal_idx);
|
|
return vlen(sv_navmesh_polies[goal_poly_idx].center - traversal_end_pos);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//Returns the polygon with the lowest f score from polygons the open set
|
|
float sv_pathfind_get_lowest_f_score_poly(navmesh_pathfind_result* res) {
|
|
//TODO: implement a better algorithm for finding the lowest score
|
|
float best_score = 100000;
|
|
float best_score_index = -1;
|
|
|
|
for(float i = 0; i < sv_navmesh_poly_count; i++) {
|
|
if(res->poly_set[i] == PATHFIND_POLY_SET_OPEN) {
|
|
if(res->poly_f_score[i] < best_score) {
|
|
best_score = res->poly_f_score[i];
|
|
best_score_index = i;
|
|
}
|
|
}
|
|
}
|
|
return best_score_index;
|
|
}
|
|
|
|
|
|
//Returns the traversal with the lowest f score from traversals the open set
|
|
float sv_pathfind_get_lowest_f_score_traversal(navmesh_pathfind_result* res) {
|
|
//TODO: implement a better algorithm for finding the lowest score
|
|
float best_score = 100000;
|
|
float best_score_index = -1;
|
|
|
|
for(float i = 0; i < sv_navmesh_traversal_count; i++) {
|
|
if(res->traversal_set[i] == PATHFIND_POLY_SET_OPEN) {
|
|
if(res->traversal_f_score[i] < best_score) {
|
|
best_score = res->traversal_f_score[i];
|
|
best_score_index = i;
|
|
}
|
|
}
|
|
}
|
|
return best_score_index;
|
|
}
|
|
|
|
|
|
void sv_pathfind_clear_result_data(navmesh_pathfind_result* res) {
|
|
//Technically we only need to iterate over navmesh_poly_count...
|
|
for(float i = 0; i < NAV_MAX_POLIES; i++) {
|
|
res->poly_set[i] = PATHFIND_POLY_SET_NONE;
|
|
res->poly_prev_poly[i] = -1;
|
|
res->poly_prev_traversal[i] = -1;
|
|
res->poly_g_score[i] = 0;
|
|
res->poly_f_score[i] = 0;
|
|
|
|
res->path_polygons[i] = -1;
|
|
res->path_traversals[i] = -1;
|
|
|
|
res->portals_left_pos[i].x = 0;
|
|
res->portals_left_pos[i].y = 0;
|
|
res->portals_left_pos[i].z = 0;
|
|
res->portals_right_pos[i].x = 0;
|
|
res->portals_right_pos[i].y = 0;
|
|
res->portals_right_pos[i].z = 0;
|
|
res->portals_traversal[i] = -1;
|
|
|
|
res->point_path_points[i].x = 0;
|
|
res->point_path_points[i].y = 0;
|
|
res->point_path_points[i].y = 0;
|
|
res->point_path_traversals[i] = -1;
|
|
}
|
|
for(float i = 0;i < NAV_MAX_TRAVERSALS; i++) {
|
|
res->traversal_set[i] = PATHFIND_POLY_SET_NONE;
|
|
res->traversal_prev_poly[i] = -1;
|
|
res->traversal_g_score[i] = 0;
|
|
res->traversal_f_score[i] = 0;
|
|
}
|
|
res->path_length = 0;
|
|
res->portals_length = 0;
|
|
res->point_path_length = 0;
|
|
}
|
|
|
|
//Applies a funnel algorithm to the path defined by the array res->result_node_path
|
|
// and populates pathfind result path
|
|
|
|
void sv_pathfind_smooth_path(vector start_point, vector goal_point, navmesh_pathfind_result* res) {
|
|
|
|
float cur_poly;
|
|
float cur_traversal;
|
|
float prev_poly;
|
|
float prev_traversal;
|
|
vector cur_portal_left_pos = '0 0 0';
|
|
vector cur_portal_right_pos = '0 0 0';
|
|
float cur_portal_traversal = -1;
|
|
vector traversal_start_pos;
|
|
vector traversal_end_pos;
|
|
|
|
// Build the list of portals
|
|
for(float i = 1; i < res->path_length; i++) {
|
|
cur_poly = res->path_polygons[i];
|
|
cur_traversal = res->path_traversals[i];
|
|
prev_poly = res->path_polygons[i-1];
|
|
prev_traversal = res->path_traversals[i-1];
|
|
|
|
// If polygon and came from polygon, portal = shared edge
|
|
if(cur_poly != -1 && prev_poly != -1) {
|
|
// Find which of prev_poly's edges connects to cur_poly
|
|
float prev_poly_edge_index = -1;
|
|
for(float j = 0; j < sv_navmesh_polies[prev_poly].connected_polies_count; j++) {
|
|
if(sv_navmesh_polies[prev_poly].connected_polies[j] == cur_poly) {
|
|
prev_poly_edge_index = j;
|
|
break;
|
|
}
|
|
}
|
|
cur_portal_left_pos = sv_navmesh_verts[sv_navmesh_polies[prev_poly].connected_polies_left_vert[prev_poly_edge_index]].pos;
|
|
cur_portal_right_pos = sv_navmesh_verts[sv_navmesh_polies[prev_poly].connected_polies_right_vert[prev_poly_edge_index]].pos;
|
|
cur_portal_traversal = -1;
|
|
|
|
// ----------------------------------------------------------------
|
|
// Narrow the portal from each side by 20% or 20qu, whichever is smaller
|
|
// ----------------------------------------------------------------
|
|
float edge_len = vlen(cur_portal_right_pos - cur_portal_left_pos);
|
|
vector edge_dir = normalize(cur_portal_right_pos - cur_portal_left_pos);
|
|
float ofs = min(edge_len * 0.2, 20);
|
|
cur_portal_left_pos = cur_portal_left_pos + edge_dir * ofs;
|
|
cur_portal_right_pos = cur_portal_right_pos - edge_dir * ofs;
|
|
// ----------------------------------------------------------------
|
|
}
|
|
// If polygon and came from traversal, portal = traversal exit
|
|
else if(cur_poly != -1 && prev_traversal != -1) {
|
|
traversal_end_pos = sv_navmesh_get_traversal_end_pos(prev_traversal);
|
|
cur_portal_left_pos = traversal_end_pos;
|
|
cur_portal_right_pos = traversal_end_pos;
|
|
cur_portal_traversal = -1;
|
|
// TODO - Denote that it came from traversal? maybe we don't care?
|
|
}
|
|
// If traversal and came from polygon, portal = traversal start
|
|
else if(cur_traversal != -1 && prev_poly != -1) {
|
|
traversal_start_pos = sv_navmesh_traversals[cur_traversal].start_pos;
|
|
cur_portal_left_pos = traversal_start_pos;
|
|
cur_portal_right_pos = traversal_start_pos;
|
|
cur_portal_traversal = cur_traversal;
|
|
}
|
|
|
|
res->portals_left_pos[res->portals_length] = cur_portal_left_pos;
|
|
res->portals_right_pos[res->portals_length] = cur_portal_right_pos;
|
|
res->portals_traversal[res->portals_length] = cur_portal_traversal;
|
|
res->portals_length++;
|
|
}
|
|
|
|
|
|
// TODO - Should I add goal_pos as the final portal?
|
|
res->portals_left_pos[res->portals_length] = goal_point;
|
|
res->portals_right_pos[res->portals_length] = goal_point;
|
|
res->portals_traversal[res->portals_length] = -1;
|
|
res->portals_length++;
|
|
|
|
|
|
|
|
vector cur_funnel_corner = start_point;
|
|
vector cur_funnel_left = res->portals_left_pos[0];
|
|
vector cur_funnel_right = res->portals_right_pos[0];
|
|
float cur_funnel_left_portal_idx = 0;
|
|
float cur_funnel_right_portal_idx = 0;
|
|
|
|
// Loop through all portals, adding necessary funnel corners to final path
|
|
for(float i = 0; i < res->portals_length; i++) {
|
|
cur_portal_left_pos = res->portals_left_pos[i];
|
|
cur_portal_right_pos = res->portals_right_pos[i];
|
|
cur_portal_traversal = res->portals_traversal[i];
|
|
|
|
|
|
// TODO - Project portal points to axis made by cur funnel points
|
|
// Then we can check
|
|
// TODO - Compute value for a point:
|
|
// TODO 0: On funnel center
|
|
// TODO < -1 outside of funnel to the left
|
|
// TODO > 1 outside of funnel to the right
|
|
|
|
float cur_portal_left_in_funnel = pathfind_point_in_funnel( cur_portal_left_pos, cur_funnel_corner, cur_funnel_left, cur_funnel_right);
|
|
float cur_portal_right_in_funnel = pathfind_point_in_funnel(cur_portal_right_pos, cur_funnel_corner, cur_funnel_left, cur_funnel_right);
|
|
|
|
// print("{'portal_idx': ", ftos(i), ", 'portal': ");
|
|
// print(" [(", ftos(cur_portal_left_pos.x), ",", ftos(cur_portal_left_pos.y), "), (", ftos(cur_portal_right_pos.x), ",", ftos(cur_portal_right_pos.y));
|
|
// print(")], 'traversal': ", ftos(cur_portal_traversal), ", ");
|
|
// print("'funnel_result': (", ftos(cur_portal_left_in_funnel), ",", ftos(cur_portal_right_in_funnel), "), ");
|
|
// print("'funnel': [");
|
|
// print("(", ftos(cur_funnel_left.x), ",", ftos(cur_funnel_left.y), "), ");
|
|
// print("(", ftos(cur_funnel_corner.x), ",", ftos(cur_funnel_corner.y), "), ");
|
|
// print("(", ftos(cur_funnel_right.x), ",", ftos(cur_funnel_right.y), ")");
|
|
// print("]},\n");
|
|
|
|
|
|
// If cur portal left point is in cur funnel, narrow the cur funnel
|
|
if(cur_portal_left_in_funnel >= -1 && cur_portal_left_in_funnel <= 1) {
|
|
cur_funnel_left = cur_portal_left_pos;
|
|
cur_funnel_left_portal_idx = i;
|
|
}
|
|
// If cur portal right point is in cur funnel, narrow the cur funnel
|
|
if(cur_portal_right_in_funnel >= -1 && cur_portal_right_in_funnel <= 1) {
|
|
cur_funnel_right = cur_portal_right_pos;
|
|
cur_funnel_right_portal_idx = i;
|
|
}
|
|
|
|
// If cur portal is completely outside (to left of) current funnel
|
|
if(cur_portal_left_in_funnel < -1 && cur_portal_right_in_funnel < -1) {
|
|
// Add cur funnel left point to final path:
|
|
res->point_path_points[res->point_path_length] = cur_funnel_left;
|
|
res->point_path_traversals[res->point_path_length] = -1;
|
|
res->point_path_length++;
|
|
|
|
// Update funnel to start at the current funnel left endpoint, pointing to the next portal in the list
|
|
// Set next funnel to start at cur funnel left point
|
|
cur_funnel_corner = cur_funnel_left;
|
|
|
|
// Advance the funnel's left point to the next portal
|
|
cur_funnel_left_portal_idx += 1;
|
|
cur_funnel_right = res->portals_right_pos[cur_funnel_left_portal_idx];
|
|
cur_funnel_left = res->portals_left_pos[cur_funnel_left_portal_idx];
|
|
cur_funnel_right_portal_idx = cur_funnel_left_portal_idx;
|
|
// Restart funnel algorithm from the funnel's endpoint
|
|
i = cur_funnel_left_portal_idx - 1;
|
|
continue;
|
|
}
|
|
// If cur portal is completely outside (to right of) current funnel
|
|
else if(cur_portal_left_in_funnel > 1 && cur_portal_right_in_funnel > 1) {
|
|
// Add cur funnel right point to final path:
|
|
res->point_path_points[res->point_path_length] = cur_funnel_right;
|
|
res->point_path_traversals[res->point_path_length] = -1;
|
|
res->point_path_length++;
|
|
|
|
// Update funnel to start at the current funnel right endpoint, pointing to the next portal in the list
|
|
// Set next funnel to start at cur funnel right point
|
|
cur_funnel_corner = cur_funnel_right;
|
|
|
|
// Advance the funnel's right point to the next portal
|
|
cur_funnel_right_portal_idx += 1;
|
|
cur_funnel_right = res->portals_right_pos[cur_funnel_right_portal_idx];
|
|
cur_funnel_left = res->portals_left_pos[cur_funnel_right_portal_idx];
|
|
cur_funnel_left_portal_idx = cur_funnel_right_portal_idx;
|
|
// Restart funnel algorithm from the funnel's endpoint
|
|
i = cur_funnel_right_portal_idx - 1;
|
|
continue;
|
|
}
|
|
|
|
// If cur portal is a traversal
|
|
if(cur_portal_traversal != -1) {
|
|
vector start_pos = sv_navmesh_traversals[cur_portal_traversal].start_pos;
|
|
vector end_pos = sv_navmesh_get_traversal_end_pos(cur_portal_traversal);
|
|
// Add traversal start point to final path:
|
|
res->point_path_points[res->point_path_length] = start_pos;
|
|
res->point_path_traversals[res->point_path_length] = cur_portal_traversal;
|
|
res->point_path_length++;
|
|
// Add traversal end point to final path:
|
|
// FIXME - Should I remove this?
|
|
res->point_path_points[res->point_path_length] = end_pos;
|
|
res->point_path_traversals[res->point_path_length] = -1;
|
|
res->point_path_length++;
|
|
|
|
|
|
// Set funnel to start at traversal endpoint and use the next portal
|
|
cur_funnel_corner = end_pos;
|
|
cur_funnel_left = res->portals_left_pos[i+1];
|
|
cur_funnel_right = res->portals_right_pos[i+1];
|
|
cur_funnel_left_portal_idx = i+1;
|
|
cur_funnel_right_portal_idx = i+1;
|
|
}
|
|
}
|
|
|
|
// Always add goal point to the path
|
|
res->point_path_points[res->point_path_length] = goal_point;
|
|
res->point_path_traversals[res->point_path_length] = -1;
|
|
res->point_path_length++;
|
|
}
|
|
|
|
|
|
// void sv_pathfind_smooth_path(vector start_point, vector goal_point, navmesh_pathfind_result* res) {
|
|
// //Evaluate portals and identify left / right portal vertices
|
|
// for(float i = 1; i < res->result_node_length; i++)
|
|
// {
|
|
// float prev_poly = res->result_node_path[i-1];
|
|
// float poly = res->result_node_path[i];
|
|
|
|
// //TODO: find link index of portal
|
|
|
|
// //Finding what index prev_poly is linked to poly
|
|
|
|
// float link_index = -1;
|
|
|
|
// for(float j = 0; j < sv_navmesh_polies[prev_poly].connected_polies_count; j++)
|
|
// {
|
|
// if(sv_navmesh_polies[prev_poly].connected_polies[j] == poly)
|
|
// {
|
|
// link_index = j;
|
|
// break;
|
|
// }
|
|
// }
|
|
|
|
// //Use that index to get left and right vertices
|
|
// res->portals_left_vert[res->portals_length] = sv_navmesh_polies[prev_poly].connected_polies_left_vert[link_index];
|
|
// res->portals_right_vert[res->portals_length++] = sv_navmesh_polies[prev_poly].connected_polies_right_vert[link_index];
|
|
// }
|
|
// //the last left edge / right edge is the goal position
|
|
|
|
// /*print("Portals: ");
|
|
// for(float i = 0; i < res->portals_length; i++)
|
|
// {
|
|
// print("[");
|
|
// print(ftos(i));
|
|
// print("] = (");
|
|
// print(ftos(res->portals_left_vert[i]));
|
|
// print(" , ");
|
|
// print(ftos(res->portals_right_vert[i]));
|
|
// print(") , ");
|
|
// }*/
|
|
|
|
// //starting at start_pos (not at start center point)
|
|
// //get vectors that point to first left edge and first right edge
|
|
|
|
// vector funnel_apex = start_point;
|
|
// float funnel_left_index = 0;
|
|
// //Index of the funnel's left vertex in the portal list
|
|
// vector funnel_left = sv_navmesh_verts[res->portals_left_vert[0]].pos;
|
|
// //Index of the funnel's right vertex in the portal list
|
|
// float funnel_right_index = 0;
|
|
// vector funnel_right = sv_navmesh_verts[res->portals_right_vert[0]].pos;
|
|
|
|
// vector next_funnel_left = '0 0 0';
|
|
// vector next_funnel_right = '0 0 0';
|
|
// vector corner;
|
|
// int portal_idx;
|
|
|
|
|
|
// float inside_right_edge;
|
|
// float inside_left_edge;
|
|
|
|
// while(1)
|
|
// {
|
|
// //print("=============Funnel iteration.==========\n");
|
|
// //Check if we have reached the end of the portals, consider the goal position as the last portal
|
|
// //If we are not at the last index of the left portal
|
|
|
|
// //Keeping track of whether or not advancing the left edge or right edge narrows the funnel
|
|
// float advanced_left = FALSE;
|
|
// float advanced_right = FALSE;
|
|
|
|
|
|
|
|
// //Consider the end goal point as the last portal
|
|
// //================ Checking left funnel edge =================
|
|
// if(funnel_left_index < res->portals_length)
|
|
// {
|
|
// //print("Trying to advance left edge.\n");
|
|
|
|
// if(funnel_left_index < res->portals_length - 1)
|
|
// {
|
|
// next_funnel_left = sv_navmesh_verts[res->portals_left_vert[funnel_left_index+1]].pos;
|
|
// // print("Next left edge is vert: ",ftos(res->portals_left_vert[funnel_left_index+1],".\n");
|
|
// }
|
|
// else
|
|
// {
|
|
// // print("Trying next left edge as goal point.\n");
|
|
// //If funnel_left is pointing to the last portal in the array, consider the goal_point the last portal
|
|
// next_funnel_left = goal_point;
|
|
// }
|
|
|
|
// inside_right_edge = pathfind_point_is_to_left(funnel_apex,funnel_right,next_funnel_left);
|
|
// //If the next left edge crosses the current right edge
|
|
// if(inside_right_edge < 0)
|
|
// {
|
|
// //print("next left edge crosses current right edge.\n");
|
|
// //Add funnel right point to path
|
|
// // res->result_path[res->result_length++] = funnel_right;
|
|
// corner = funnel_right;
|
|
// // ------------------------------------------------------------
|
|
// // Instead of adding the funnel's corner to the path,
|
|
// // move away from the corner by some small amount
|
|
// // ------------------------------------------------------------
|
|
// // Find the first portal along the path with this corner
|
|
// portal_idx = funnel_right_index;
|
|
// for(int i = funnel_right_index; i >= 0; i--) {
|
|
// if(res->portals_right_vert[i] == res->portals_right_vert[funnel_right_index]) {
|
|
// portal_idx = i;
|
|
// continue;
|
|
// }
|
|
// break;
|
|
// }
|
|
// // For every portal with this corner, add a point to the path
|
|
// for(int i = portal_idx; i <= funnel_right_index; i++) {
|
|
// // Find the vertex opposite this portal edge
|
|
// vector opposite_corner = sv_navmesh_verts[res->portals_left_vert[i]].pos;
|
|
// // Move 20 qu or 20% of the edge length, whichever is smaller:
|
|
// float edge_len = vlen(opposite_corner - corner);
|
|
// vector edge_dir = normalize(opposite_corner - corner);
|
|
// vector delta_corner = edge_dir * min(edge_len * 0.2, 20);
|
|
// res->result_path[res->result_length++] = corner + delta_corner;
|
|
// }
|
|
// // ------------------------------------------------------------
|
|
// // res->result_path[res->result_length++] = corner;
|
|
|
|
|
|
// // TODO - Move 10% of the way the funnel's right portal vert to the left
|
|
|
|
// //Check if we are at the end of our portal list
|
|
// if(funnel_right_index >= res->portals_length - 1)
|
|
// {
|
|
// res->result_path[res->result_length++] = goal_point;
|
|
// return;
|
|
// }
|
|
|
|
|
|
// //Restart algorithm with the portal after the right point
|
|
// funnel_apex = funnel_right;
|
|
|
|
// funnel_left_index = funnel_right_index + 1;
|
|
// funnel_left = sv_navmesh_verts[res->portals_left_vert[funnel_left_index]].pos;
|
|
// funnel_right_index = funnel_right_index + 1;
|
|
// funnel_right = sv_navmesh_verts[res->portals_right_vert[funnel_right_index]].pos;
|
|
// continue;
|
|
// }
|
|
// //If the next left edge is in the funnel
|
|
// inside_left_edge = pathfind_point_is_to_left(funnel_left,funnel_apex,next_funnel_left);
|
|
// if(inside_left_edge >= 0 && inside_right_edge >= 0)
|
|
// {
|
|
// //print("next left edge is in the funnel.\n");
|
|
// //Advance the left edge
|
|
// funnel_left = next_funnel_left;
|
|
// funnel_left_index++;
|
|
|
|
// advanced_left = TRUE;
|
|
|
|
// //Check if the portal we just added was the goal point (will be after the list)
|
|
// if(funnel_left_index >= res->portals_length)
|
|
// {
|
|
// res->result_path[res->result_length++] = goal_point;
|
|
// return;
|
|
// }
|
|
// }
|
|
// }
|
|
// //================ Checking right funnel edge =================
|
|
// if(funnel_right_index < res->portals_length)
|
|
// {
|
|
// //print("Trying to advance right edge:.\n");
|
|
|
|
// if(funnel_right_index < res->portals_length - 1)
|
|
// {
|
|
// next_funnel_right = sv_navmesh_verts[res->portals_right_vert[funnel_right_index+1]].pos;
|
|
// //print("Next right edge is vert: ",ftos(res->portals_right_vert[funnel_right_index+1]),".\n");
|
|
// }
|
|
// else
|
|
// {
|
|
// //print("Trying next right edge as goal point.\n");
|
|
// //If funnel_right is pointing to the last portal in the array, consider the goal_point the last portal
|
|
// next_funnel_right = goal_point;
|
|
// }
|
|
|
|
// //If the next right edge crosses the current left edge
|
|
// inside_left_edge = pathfind_point_is_to_left(funnel_left,funnel_apex,next_funnel_right);
|
|
// if(inside_left_edge < 0)
|
|
// {
|
|
// //print("next right edge crosses current left edge.\n");
|
|
// //Add funnel left point to path
|
|
// // res->result_path[res->result_length++] = funnel_left;
|
|
// corner = funnel_left;
|
|
// // ------------------------------------------------------------
|
|
// // Instead of adding the funnel's corner to the path,
|
|
// // move away from the corner by some small amount
|
|
// // ------------------------------------------------------------
|
|
// // Find the first portal along the path with this corner
|
|
// portal_idx = funnel_left_index;
|
|
// for(int i = funnel_left_index; i >= 0; i--) {
|
|
// if(res->portals_left_vert[i] == res->portals_left_vert[funnel_left_index]) {
|
|
// portal_idx = i;
|
|
// continue;
|
|
// }
|
|
// break;
|
|
// }
|
|
// // For every portal with this corner, add a point to the path
|
|
// for(int i = portal_idx; i <= funnel_left_index; i++) {
|
|
// // Find the vertex opposite this portal edge
|
|
// vector opposite_corner = sv_navmesh_verts[res->portals_right_vert[i]].pos;
|
|
// // Move 20 qu or 20% of the edge length, whichever is smaller:
|
|
// float edge_len = vlen(opposite_corner - corner);
|
|
// vector edge_dir = normalize(opposite_corner - corner);
|
|
// vector delta_corner = edge_dir * min(edge_len * 0.2, 20);
|
|
// res->result_path[res->result_length++] = corner + delta_corner;
|
|
// }
|
|
// // ------------------------------------------------------------
|
|
// // res->result_path[res->result_length++] = corner;
|
|
|
|
// //Check if we are at the end of our portal list
|
|
// if(funnel_left_index >= res->portals_length - 1)
|
|
// {
|
|
// res->result_path[res->result_length++] = goal_point;
|
|
// return;
|
|
// }
|
|
|
|
|
|
// //Restart algorithm with the portal after the right point
|
|
// funnel_apex = funnel_left;
|
|
|
|
// funnel_right_index = funnel_left_index + 1;
|
|
// funnel_right = sv_navmesh_verts[res->portals_right_vert[funnel_right_index]].pos;
|
|
// funnel_left_index = funnel_left_index + 1;
|
|
// funnel_left = sv_navmesh_verts[res->portals_left_vert[funnel_left_index]].pos;
|
|
// continue;
|
|
// }
|
|
// //If the next right edge is in the funnel
|
|
// inside_right_edge = pathfind_point_is_to_left(funnel_apex,funnel_right,next_funnel_right);
|
|
// if(inside_left_edge >= 0 && inside_right_edge >= 0)
|
|
// {
|
|
// //print("next right edge is in the funnel.\n");
|
|
// //Advance the left edge
|
|
// funnel_right = next_funnel_right;
|
|
// funnel_right_index++;
|
|
|
|
// advanced_right = TRUE;
|
|
|
|
// //Check if the portal we just added was the goal point (will be after the list)
|
|
// if(funnel_right_index >= res->portals_length)
|
|
// {
|
|
// res->result_path[res->result_length++] = goal_point;
|
|
// return;
|
|
// }
|
|
// }
|
|
// }
|
|
// if(advanced_left == FALSE && advanced_right == FALSE)
|
|
// {
|
|
// //print("Hit funnel freeze condition.\n");
|
|
|
|
// float left_vert = -1;
|
|
// float left_type = -1;
|
|
// float left_portal_index = -1;//index of the vertex in the list of portals
|
|
|
|
// float right_vert = -1;
|
|
// float right_type = -1;
|
|
// float right_portal_index = -1;//index of the vertex in the list of portals
|
|
|
|
// float crossed = 1;//Indicates the vert crossed the funnel
|
|
// float contained = 2;//Indicates a vert is in the funnel
|
|
|
|
// float last_vert = -1;//used to skip calculating the same vertex more than once
|
|
|
|
|
|
// //Find the closest vertex in the portal left with the lowest index that crosses the funnel or is in the funnel
|
|
// for(float i = funnel_left_index + 1; i < res->portals_length + 1; i++)
|
|
// {
|
|
// if(i < res->portals_length - 1)
|
|
// {
|
|
// //Don't calculate the same vertex again
|
|
// if(last_vert == res->portals_left_vert[i])
|
|
// continue;
|
|
// last_vert = res->portals_left_vert[i];
|
|
// next_funnel_left = sv_navmesh_verts[last_vert].pos;
|
|
// }
|
|
// else//consider goal pos as last portal edge
|
|
// {
|
|
// next_funnel_left = goal_point;
|
|
// }
|
|
|
|
// inside_right_edge = pathfind_point_is_to_left(funnel_apex,funnel_right,next_funnel_left);
|
|
|
|
// //If the left vertex crosses the funnel (left vertex is outside the funnel's right edge)
|
|
// if(inside_right_edge < 0)
|
|
// {
|
|
// left_vert = res->portals_left_vert[i];
|
|
// left_type = crossed;
|
|
// break;
|
|
// }
|
|
|
|
// inside_left_edge = pathfind_point_is_to_left(funnel_left,funnel_apex,next_funnel_left);
|
|
|
|
// //If the left vertex is within the funnel
|
|
// if(inside_left_edge >= 0 && inside_right_edge >= 0)
|
|
// {
|
|
// left_vert = res->portals_left_vert[i];
|
|
// left_type = contained;
|
|
// left_portal_index = i;
|
|
// break;
|
|
// }
|
|
// }
|
|
|
|
// last_vert = -1;
|
|
|
|
// //Find the closest vertex in the portal right with the lowest index that crosses the funnel or is in the funnel
|
|
// for(float i = funnel_right_index + 1; i < res->portals_length; i++)
|
|
// {
|
|
// if(i < res->portals_length - 1)
|
|
// {
|
|
// //Don't calculate the same vertex again
|
|
// if(last_vert == res->portals_right_vert[i])
|
|
// continue;
|
|
// last_vert = res->portals_right_vert[i];
|
|
// next_funnel_right = sv_navmesh_verts[last_vert].pos;
|
|
// }
|
|
// else//consider goal pos as last portal edge
|
|
// {
|
|
// next_funnel_right = goal_point;
|
|
// }
|
|
|
|
// inside_left_edge = pathfind_point_is_to_left(funnel_left,funnel_apex,next_funnel_right);
|
|
|
|
// //If the right vertex crosses the funnel (right vertex is outside the funnel's left edge)
|
|
// if(inside_left_edge < 0)
|
|
// {
|
|
// right_vert = res->portals_right_vert[i];
|
|
// right_type = crossed;
|
|
// break;
|
|
// }
|
|
|
|
// inside_right_edge = pathfind_point_is_to_left(funnel_apex,funnel_right,next_funnel_right);
|
|
|
|
// //If the right vertex is within the funnel
|
|
// if(inside_left_edge >= 0 && inside_right_edge >= 0)
|
|
// {
|
|
// right_vert = res->portals_right_vert[i];
|
|
// right_type = contained;
|
|
// right_portal_index = i;
|
|
// break;
|
|
// }
|
|
// }
|
|
|
|
// //If no vertices were found, goal is reachable from here
|
|
// if(left_vert == -1 && right_vert == -1)
|
|
// {
|
|
// res->result_path[res->result_length++] = goal_point;
|
|
// return;
|
|
// }
|
|
|
|
|
|
// float use_left;
|
|
|
|
// //If both verts were found
|
|
// //Find which vertex has a lower index
|
|
// if(left_vert != -1 && right_vert != -1)
|
|
// {
|
|
// if(left_portal_index < right_portal_index)
|
|
// {
|
|
// use_left = TRUE;
|
|
// }
|
|
// else
|
|
// {
|
|
// use_left = FALSE;
|
|
// }
|
|
// }
|
|
|
|
// //if left vert was found
|
|
// else if(left_vert != -1)
|
|
// {
|
|
// use_left = TRUE;
|
|
// }
|
|
// //else right vert was found
|
|
// else
|
|
// {
|
|
// use_left = FALSE;
|
|
// }
|
|
|
|
// if(use_left)
|
|
// {
|
|
// if(left_type == crossed)
|
|
// {
|
|
// //Place corner at current right funnel edge
|
|
// res->result_path[res->result_length++] = funnel_right;
|
|
|
|
// //Restart algorithm with the portal after the right point
|
|
// funnel_apex = funnel_right;
|
|
|
|
// //Check if portal after this portal is goal:
|
|
// funnel_right_index++;
|
|
// if(funnel_right_index >= res->portals_length)
|
|
// {
|
|
// res->result_path[res->result_length++] = goal_point;
|
|
// return;
|
|
// }
|
|
|
|
// funnel_right = sv_navmesh_verts[res->portals_right_vert[funnel_right_index]].pos;
|
|
// funnel_left_index = funnel_right_index;
|
|
// funnel_left = sv_navmesh_verts[res->portals_left_vert[funnel_left_index]].pos;
|
|
// continue;
|
|
// }
|
|
// else//in funnel
|
|
// {
|
|
// //If the next funnel right is the goal, we are done
|
|
// //Check if portal after this portal is goal:
|
|
// if(right_portal_index >= res->portals_length)
|
|
// {
|
|
// res->result_path[res->result_length++] = goal_point;
|
|
// return;
|
|
// }
|
|
// //Update current left funnel edge to use this vertex
|
|
// funnel_left = next_funnel_left;
|
|
// funnel_left_index = left_portal_index;
|
|
|
|
// continue;
|
|
// }
|
|
// }
|
|
// else//use right
|
|
// {
|
|
// if(right_type == crossed)
|
|
// {
|
|
// //Place corner at current left funnel edge
|
|
// res->result_path[res->result_length++] = funnel_left;
|
|
|
|
// //Restart algorithm with the portal after the left point
|
|
// funnel_apex = funnel_left;
|
|
|
|
// //Check if portal after this portal is goal:
|
|
// funnel_left_index++;
|
|
// if(funnel_left_index >= res->portals_length)
|
|
// {
|
|
// res->result_path[res->result_length++] = goal_point;
|
|
// return;
|
|
// }
|
|
|
|
// funnel_left = sv_navmesh_verts[res->portals_left_vert[funnel_left_index]].pos;
|
|
// funnel_right_index = funnel_left_index;
|
|
// funnel_right = sv_navmesh_verts[res->portals_right_vert[funnel_right_index]].pos;
|
|
// continue;
|
|
// }
|
|
// else//in funnel
|
|
// {
|
|
// //If the next funnel right is the goal, we are done
|
|
// //Check if portal after this portal is goal:
|
|
// if(right_portal_index >= res->portals_length)
|
|
// {
|
|
// res->result_path[res->result_length++] = goal_point;
|
|
// return;
|
|
// }
|
|
// //Update current right funnel edge to use this vertex
|
|
// funnel_right = next_funnel_right;
|
|
// funnel_right_index = right_portal_index;
|
|
|
|
// continue;
|
|
// }
|
|
// }
|
|
// }
|
|
// //print("==========================\n");
|
|
// }
|
|
// }
|
|
|
|
//Evaluates the path that is found in the pathfinding algorithm and populates an array with the nodes in the path from start to goal
|
|
void sv_pathfind_trace_path(float start_poly, float goal_poly, navmesh_pathfind_result* res) {
|
|
// print("Polygon from poly: ");
|
|
// for(float i = 0; i < sv_navmesh_poly_count; i++) {
|
|
// print(ftos(i));
|
|
// print(": ");
|
|
// print(ftos(res->poly_prev_poly[i]));
|
|
// print(", ");
|
|
// }
|
|
// print("\n");
|
|
// print("Polygon from traversal: ");
|
|
// for(float i = 0; i < sv_navmesh_poly_count; i++) {
|
|
// print(ftos(i));
|
|
// print(": ");
|
|
// print(ftos(res->poly_prev_traversal[i]));
|
|
// print(", ");
|
|
// }
|
|
// print("\n");
|
|
// print("Traversal from polygon:");
|
|
// for(float i = 0; i < sv_navmesh_traversal_count; i++) {
|
|
// print(ftos(i));
|
|
// print(": ");
|
|
// print(ftos(res->traversal_prev_poly[i]));
|
|
// print(", ");
|
|
// }
|
|
// print("\n");
|
|
|
|
|
|
//Count the length of the path (how many polygons the path traverses)
|
|
float current_poly = goal_poly;
|
|
float current_traversal = -1;
|
|
res->path_length = 1;
|
|
do {
|
|
// Current node is a polygon
|
|
if(current_poly != -1) {
|
|
current_traversal = res->poly_prev_traversal[current_poly];
|
|
current_poly = res->poly_prev_poly[current_poly];
|
|
}
|
|
// Current node is a traversal
|
|
else if(current_traversal != -1) {
|
|
current_poly = res->traversal_prev_poly[current_traversal];
|
|
current_traversal = -1;
|
|
}
|
|
res->path_length++;
|
|
} while(current_poly != start_poly);
|
|
|
|
|
|
// Starting at goal_poly, add the traversed polygons / traversals to the result path in reverse order
|
|
current_poly = goal_poly;
|
|
current_traversal = -1;
|
|
for(float i = 0; i < res->path_length; i++) {
|
|
// print("Current node (P=");
|
|
// print(ftos(current_poly));
|
|
// print(",T=");
|
|
// print(ftos(current_traversal));
|
|
// print(") came from ");
|
|
|
|
res->path_polygons[res->path_length - 1 - i] = current_poly;
|
|
res->path_traversals[res->path_length - 1 - i] = current_traversal;
|
|
// Current node is a polygon
|
|
if(current_poly != -1) {
|
|
current_traversal = res->poly_prev_traversal[current_poly];
|
|
current_poly = res->poly_prev_poly[current_poly];
|
|
}
|
|
// Current node is a traversal
|
|
else if(current_traversal != -1) {
|
|
current_poly = res->traversal_prev_poly[current_traversal];
|
|
current_traversal = -1;
|
|
}
|
|
|
|
// print(" (P=");
|
|
// print(ftos(current_poly));
|
|
// print(",T=");
|
|
// print(ftos(current_traversal));
|
|
// print(")\n");
|
|
}
|
|
|
|
// print("Pathfind success, path length: ");
|
|
// print(ftos(res->path_length));
|
|
// print(".\n");
|
|
// print("Path: ");
|
|
// for(float i = 0; i < res->path_length; i++) {
|
|
// if(i > 0) {
|
|
// print(" , ");
|
|
// }
|
|
// if(res->path_polygons[i] != -1) {
|
|
// print("P");
|
|
// print(ftos(res->path_polygons[i]));
|
|
// }
|
|
// else if(res->path_traversals[i] != -1) {
|
|
// print("T");
|
|
// print(ftos(res->path_traversals[i]));
|
|
// }
|
|
// }
|
|
// print(" .\n");
|
|
}
|
|
|
|
|
|
|
|
//Accepts start polygon and goal polygon
|
|
//Returns 1 on success.
|
|
//Returns 0 on fail.
|
|
float sv_navmesh_pathfind_start(float start_poly, float goal_poly, vector start_pos, vector end_pos, navmesh_pathfind_result* res) {
|
|
if(start_poly == -1) {
|
|
print("Error: pathfind start polygon invalid.\n");
|
|
return 0;
|
|
}
|
|
if(goal_poly == -1) {
|
|
print("Error: pathfind goal polygon invalid.\n");
|
|
return 0;
|
|
}
|
|
if(start_poly == goal_poly) {
|
|
//Calculating node path
|
|
res->path_polygons[0] = start_poly;
|
|
res->path_traversals[0] = -1;
|
|
res->path_length = 1;
|
|
// print("Pathind success: trivial case (start = goal).\n");
|
|
|
|
//Calculating vector based path (go directly to goal)
|
|
res->point_path_points[0].x = end_pos.x;
|
|
res->point_path_points[0].y = end_pos.y;
|
|
res->point_path_points[0].z = end_pos.z;
|
|
// Indicate non-traversal
|
|
res->point_path_traversals[0] = -1;
|
|
res->point_path_length = 1;
|
|
return 1;
|
|
}
|
|
|
|
//Clearing previous data
|
|
sv_pathfind_clear_result_data(res);
|
|
|
|
//Adding start polygon to the open set
|
|
res->poly_set[start_poly] = PATHFIND_POLY_SET_OPEN;
|
|
res->poly_g_score[start_poly] = 0;
|
|
res->poly_f_score[start_poly] = 0 + sv_pathfind_calc_poly_h_score(start_poly , goal_poly);
|
|
|
|
// print("Pathfind init. Start: ");
|
|
// print(ftos(start_poly));
|
|
// print(" , Goal: ");
|
|
// print(ftos(goal_poly));
|
|
// print(".\n");
|
|
|
|
float current_poly = start_poly;
|
|
float current_traversal = -1;
|
|
float pathfind_success = FALSE;
|
|
|
|
// While we have a traversal OR a polygon
|
|
while(current_poly != -1 || current_traversal != -1) {
|
|
|
|
// If we are currently evaluating a polygon:
|
|
if(current_poly != -1) {
|
|
if(current_poly == goal_poly) {
|
|
//print("Current is now goal. Breaking.\n");
|
|
pathfind_success = TRUE;
|
|
break;
|
|
}
|
|
|
|
//Add current node to the closed set
|
|
res->poly_set[current_poly] = PATHFIND_POLY_SET_CLOSED;
|
|
//Add connected neighbor polygons to the open set
|
|
for(float i = 0; i < sv_navmesh_polies[current_poly].connected_polies_count; i++) {
|
|
float neighbor_poly = sv_navmesh_polies[current_poly].connected_polies[i];
|
|
|
|
// print("Checking poly ");
|
|
// print(ftos(current_poly));
|
|
// print("'s neighbor poly ");
|
|
// print(ftos(neighbor_poly));
|
|
// print(".\n");
|
|
|
|
|
|
// ----------------------------------------------------------------
|
|
// Door check
|
|
// ----------------------------------------------------------------
|
|
// NOTE - If polygon's door hasn't been opened, don't consider it.
|
|
// If this polygon references a door:
|
|
if(sv_navmesh_polies[neighbor_poly].doortarget != "") {
|
|
entity door;
|
|
door = find(world, wayTarget, sv_navmesh_polies[neighbor_poly].doortarget);
|
|
if(door != world) {
|
|
// If the referenced door is closed, don't consider this polygon.
|
|
if(door.state == STATE_BOTTOM) {
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
// ----------------------------------------------------------------
|
|
|
|
|
|
if(res->poly_set[neighbor_poly] != PATHFIND_POLY_SET_CLOSED) {
|
|
//print("Neighbor is not in closed list.\n");
|
|
//Calculate tentative f score
|
|
//Calculate tentative g score (distance from start to current + distance from current to neighbor)
|
|
float tentative_g_score = res->poly_g_score[current_poly] + vlen(sv_navmesh_polies[neighbor_poly].center - sv_navmesh_polies[current_poly].center);
|
|
|
|
// If not in open-set, or this is a better score, set score for this polygon
|
|
if(res->poly_set[neighbor_poly] != PATHFIND_POLY_SET_OPEN || tentative_g_score < res->poly_g_score[neighbor_poly]) {
|
|
//print("Neighbor is not in open list, or a better g score has been found.\n");
|
|
//Adding neighbor to open set
|
|
res->poly_set[neighbor_poly] = PATHFIND_POLY_SET_OPEN;
|
|
|
|
//Updating scores for neighbor node
|
|
float tentative_f_score = tentative_g_score + sv_pathfind_calc_poly_h_score(neighbor_poly , goal_poly);
|
|
res->poly_g_score[neighbor_poly] = tentative_g_score;
|
|
res->poly_f_score[neighbor_poly] = tentative_f_score;
|
|
|
|
//Linking neighbor node to current node (for tracing path)
|
|
res->poly_prev_poly[neighbor_poly] = current_poly;
|
|
res->poly_prev_traversal[neighbor_poly] = -1;
|
|
|
|
// print("Assigning Poly ");
|
|
// print(ftos(neighbor_poly));
|
|
// print(" came from poly ");
|
|
// print(ftos(current_poly));
|
|
// print(".\n");
|
|
}
|
|
}
|
|
}
|
|
|
|
//Add connected neighbor traversals to the open set
|
|
for(float i = 0; i < sv_navmesh_polies[current_poly].connected_traversals_count; i++) {
|
|
float neighbor_traversal = sv_navmesh_polies[current_poly].connected_traversals[i];
|
|
|
|
// print("Checking poly ");
|
|
// print(ftos(current_poly));
|
|
// print("'s neighbor traversal ");
|
|
// print(ftos(neighbor_traversal));
|
|
// print(".\n");
|
|
|
|
if(res->traversal_set[neighbor_traversal] != PATHFIND_POLY_SET_CLOSED) {
|
|
//Calculate tentative f score
|
|
//Calculate tentative g score (distance from start to current + distance from current to neighbor)
|
|
// vector traversal_start_pos = sv_navmesh_traversals[neighbor].start_pos;
|
|
vector traversal_end_pos = sv_navmesh_get_traversal_end_pos(neighbor_traversal);
|
|
// TODO - Should I cache traversal length?
|
|
float tentative_g_score = res->poly_g_score[current_poly] + vlen(traversal_end_pos - sv_navmesh_polies[current_poly].center);
|
|
|
|
// If not in open-set, or this is a better score, set score for this traversal
|
|
if(res->traversal_set[neighbor_traversal] != PATHFIND_POLY_SET_OPEN || tentative_g_score < res->traversal_g_score[neighbor_traversal]) {
|
|
//print("Neighbor is not in open list, or a better g score has been found.\n");
|
|
//Adding neighbor to open set
|
|
res->traversal_set[neighbor_traversal] = PATHFIND_POLY_SET_OPEN;
|
|
|
|
//Updating scores for neighbor node
|
|
float tentative_f_score = tentative_g_score + sv_pathfind_calc_traversal_h_score(neighbor_traversal, goal_poly);
|
|
res->traversal_g_score[neighbor_traversal] = tentative_g_score;
|
|
res->traversal_f_score[neighbor_traversal] = tentative_f_score;
|
|
|
|
//Linking neighbor node to current node (for tracing path)
|
|
res->traversal_prev_poly[neighbor_traversal] = current_poly;
|
|
// print("Assigning traversal ");
|
|
// print(ftos(neighbor_traversal));
|
|
// print(" came from poly ");
|
|
// print(ftos(current_poly));
|
|
// print(".\n");
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
else if(current_traversal != -1) {
|
|
// print("Checking traversal: ");
|
|
// print(ftos(current_traversal));
|
|
// print(" with end polygon: ");
|
|
// print(ftos(sv_navmesh_traversals[current_traversal].end_poly));
|
|
// print("\n");
|
|
//Add current traversal to the closed set
|
|
res->traversal_set[current_traversal] = PATHFIND_POLY_SET_CLOSED;
|
|
|
|
// Traversals have single neighbor polygons (at the traversal endpoint)
|
|
float neighbor_poly = sv_navmesh_traversals[current_traversal].end_poly;
|
|
if(neighbor_poly != -1) {
|
|
|
|
// print("Checking traversal ");
|
|
// print(ftos(current_traversal));
|
|
// print("'s neighbor poly ");
|
|
// print(ftos(neighbor_poly));
|
|
// print(".\n");
|
|
|
|
if(res->poly_set[neighbor_poly] != PATHFIND_POLY_SET_CLOSED) {
|
|
//print("Neighbor is not in closed list.\n");
|
|
//Calculate tentative f score
|
|
//Calculate tentative g score (distance from start to current + distance from current to neighbor)
|
|
vector traversal_end_pos = sv_navmesh_get_traversal_end_pos(current_traversal);
|
|
float tentative_g_score = res->traversal_g_score[current_traversal] + vlen(sv_navmesh_polies[neighbor_poly].center - traversal_end_pos);
|
|
|
|
// If not in open-set, or this is a better score, set score for this polygon
|
|
if(res->poly_set[neighbor_poly] != PATHFIND_POLY_SET_OPEN || tentative_g_score < res->poly_g_score[neighbor_poly]) {
|
|
//print("Neighbor is not in open list, or a better g score has been found.\n");
|
|
//Adding neighbor to open set
|
|
res->poly_set[neighbor_poly] = PATHFIND_POLY_SET_OPEN;
|
|
|
|
//Updating scores for neighbor node
|
|
float tentative_f_score = tentative_g_score + sv_pathfind_calc_poly_h_score(neighbor_poly , goal_poly);
|
|
res->poly_g_score[neighbor_poly] = tentative_g_score;
|
|
res->poly_f_score[neighbor_poly] = tentative_f_score;
|
|
|
|
//Linking neighbor node to current node (for tracing path)
|
|
res->poly_prev_poly[neighbor_poly] = -1;
|
|
res->poly_prev_traversal[neighbor_poly] = current_traversal;
|
|
|
|
// print("Assigning Poly ");
|
|
// print(ftos(neighbor_poly));
|
|
// print(" came from traversal ");
|
|
// print(ftos(current_traversal));
|
|
// print(".\n");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
float best_openset_poly = sv_pathfind_get_lowest_f_score_poly(res);
|
|
float best_openset_traversal = sv_pathfind_get_lowest_f_score_traversal(res);
|
|
|
|
// print("Best openset poly: ");
|
|
// print(ftos(best_openset_poly));
|
|
// print("\n");
|
|
// print("Best openset traversal: ");
|
|
// print(ftos(best_openset_traversal));
|
|
// print("\n");
|
|
|
|
// If we have both, compare their f-scores
|
|
if(best_openset_poly != -1 && best_openset_traversal != -1) {
|
|
// If traversal score is better, don't consider polygon
|
|
if(res->traversal_f_score[best_openset_poly] < res->poly_f_score[best_openset_poly]) {
|
|
best_openset_poly = -1;
|
|
}
|
|
// If polygon score is better, don't consider traversal
|
|
else {
|
|
best_openset_traversal = -1;
|
|
}
|
|
}
|
|
// Assign what we found, loop will terminate if we have neither
|
|
current_traversal = best_openset_traversal;
|
|
current_poly = best_openset_poly;
|
|
// --------------------------------------------------------------------
|
|
}
|
|
|
|
|
|
|
|
|
|
//Tracing the pathfind results
|
|
if(pathfind_success == TRUE) {
|
|
sv_pathfind_trace_path(start_poly, goal_poly, res);
|
|
sv_pathfind_smooth_path(start_pos, end_pos, res);
|
|
return 1;
|
|
}
|
|
print("Pathfind fail");
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// float sv_navmesh_pathfind_start(float start, float goal, vector start_pos, vector end_pos, navmesh_pathfind_result* res) {
|
|
// if(start == -1) {
|
|
// //print("Error: pathfind start node invalid.\n");
|
|
// return 0;
|
|
// }
|
|
// if(goal == -1) {
|
|
// //print("Error: pathfind goal node invalid.\n");
|
|
// return 0;
|
|
// }
|
|
// if(start == goal) {
|
|
// //Calculating node path
|
|
// res->result_node_path[0] = start;
|
|
// res->result_node_length = 1;
|
|
// // print("Pathind success: trivial case (start = goal).\n");
|
|
|
|
// //Calculating vector based path (go directly to goal)
|
|
// res->result_path[0].x = end_pos.x;
|
|
// res->result_path[0].y = end_pos.y;
|
|
// res->result_path[0].z = end_pos.z;
|
|
// res->result_length = 1;
|
|
// return 1;
|
|
// }
|
|
|
|
// //Clearing previous data
|
|
// sv_pathfind_clear_result_data(res);
|
|
|
|
// //Adding start polygon to the open set
|
|
// res->poly_set[start] = PATHFIND_POLY_SET_OPEN;
|
|
// res->poly_g_score[start] = 0;
|
|
// res->poly_f_score[start] = 0 + sv_pathfind_calc_h_score(start , goal);
|
|
|
|
// //Fields that need to be set:
|
|
// //res->poly_set[NAV_MAX_POLIES];
|
|
// //res->prev_poly[NAV_MAX_POLIES];
|
|
// //res->poly_g_score[NAV_MAX_POLIES];
|
|
// //res->poly_f_score[NAV_MAX_POLIES];
|
|
|
|
// // print("Pathfind init. Start: ");
|
|
// // print(ftos(start));
|
|
// // print(" , Goal: ");
|
|
// // print(ftos(goal));
|
|
// // print(".\n");
|
|
|
|
// float current = start;
|
|
|
|
// float pathfind_success = FALSE;
|
|
|
|
// while(current != -1) {
|
|
// if(current == goal) {
|
|
// //print("Current is now goal. Breaking.\n");
|
|
// pathfind_success = TRUE;
|
|
// break;
|
|
// }
|
|
// //Add current node to the closed set
|
|
// res->poly_set[current] = PATHFIND_POLY_SET_CLOSED;
|
|
// //Add connected nodes to the open set
|
|
// for(float i = 0; i < sv_navmesh_polies[current].connected_polies_count; i++) {
|
|
// float neighbor = sv_navmesh_polies[current].connected_polies[i];
|
|
|
|
// //print("Checking poly ");
|
|
// //print(ftos(current));
|
|
// //print("'s neighbor ");
|
|
// //print(ftos(neighbor));
|
|
// //print(".\n");
|
|
|
|
// // ----------------------------------------------------------------
|
|
// // Door check
|
|
// // ----------------------------------------------------------------
|
|
// // If this polygon references a door:
|
|
// if(sv_navmesh_polies[neighbor].doortarget != "") {
|
|
// entity door;
|
|
// door = find(world, wayTarget, sv_navmesh_polies[neighbor].doortarget);
|
|
// if(door != world) {
|
|
// // If the referenced door is closed, don't consider this polygon.
|
|
// if(door.state == STATE_BOTTOM) {
|
|
// continue;
|
|
// }
|
|
// }
|
|
// }
|
|
// // ----------------------------------------------------------------
|
|
|
|
|
|
// if(res->poly_set[neighbor] != PATHFIND_POLY_SET_CLOSED) {
|
|
// //print("Neighbor is not in closed list.\n");
|
|
// //Calculate tentative f score
|
|
// //Calculate tentative g score (distance from start to current + distance from current to neighbor)
|
|
// float tentative_g_score = res->poly_g_score[current] + vlen(sv_navmesh_polies[neighbor].center - sv_navmesh_polies[current].center);
|
|
|
|
// if(res->poly_set[neighbor] != PATHFIND_POLY_SET_OPEN || tentative_g_score < res->poly_g_score[neighbor]) {
|
|
// //print("Neighbor is not in open list, or a better g score has been found.\n");
|
|
// //Adding neighbor to open set
|
|
// res->poly_set[neighbor] = PATHFIND_POLY_SET_OPEN;
|
|
|
|
// //Updating scores for neighbor node
|
|
// float tentative_f_score = tentative_g_score + sv_pathfind_calc_h_score(neighbor , goal);
|
|
// res->poly_g_score[neighbor] = tentative_g_score;
|
|
// res->poly_f_score[neighbor] = tentative_f_score;
|
|
|
|
// //Linking neighbor node to current node (for tracing path)
|
|
// res->poly_prev[neighbor] = current;
|
|
|
|
// //print("Assigning Poly ");
|
|
// //print(ftos(neighbor));
|
|
// //print(" came from ");
|
|
// //print(ftos(current));
|
|
// //print(".\n");
|
|
// }
|
|
|
|
// }
|
|
// }
|
|
|
|
// current = sv_pathfind_get_lowest_f_score(res);
|
|
// }
|
|
|
|
|
|
// //Tracing the pathfind results
|
|
// if(pathfind_success == TRUE) {
|
|
// sv_pathfind_trace_path(start,goal,res);
|
|
// sv_pathfind_smooth_path(start_pos,end_pos,res);
|
|
|
|
// return 1;
|
|
// }
|
|
// print("Pathfind fail");
|
|
// return 0;
|
|
// }
|
|
|
|
float sv_navmesh_pathfind(entity from, entity to) {
|
|
// TODO ... How do I get...?
|
|
// Every AI_Chase entity gets an integer
|
|
// TODO - In pathfinding, each entity needs a place to store its results...
|
|
// TODO - I should really make a MAX_AI number
|
|
// TODO - I can have an array of pre-allocated pathfind results, and dynamically use them?
|
|
// TODO - Or maybe I have one reserved per AI, and one supplemental one reesrved per AI...
|
|
// FIXME - Will need to pre-allocate a certain number of AI_Chase ents and reuse them... or maybe
|
|
// FIXME I need to pre-allocate a certain number of zombies, and dogs?
|
|
// FIXME I wanted to subclass zombies to crawlers to override their behaviors...
|
|
// We can perform pathfinding logic without losing our current path.
|
|
|
|
// TODO - In pathfinding, I need a reference to the calling entity to know which traversal types this entity can use
|
|
AI_Chase chase_ent = (AI_Chase) from;
|
|
navmesh_pathfind_result* res = &(sv_zombie_pathfind_result[chase_ent.pathfind_result_idx]);
|
|
|
|
float start = sv_navmesh_get_containing_poly(from.origin);
|
|
float goal = sv_navmesh_get_containing_poly(to.origin);
|
|
|
|
return sv_navmesh_pathfind_start(start,goal,from.origin,to.origin,res);
|
|
}
|
|
|
|
//===========================================================================================================
|