Skip to content

Instantly share code, notes, and snippets.

@Einlander

Einlander/path_manager2.gd

Created April 30, 2022 22:27
Show Gist options
  • Save Einlander/e3fbb214f2624bcf5f6b136696adb2e3 to your computer and use it in GitHub Desktop.
Save Einlander/e3fbb214f2624bcf5f6b136696adb2e3 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
path_manager2.gd
# TODO: Make sure it can handle multiple spawn locations
# TODO: Add the ability to trace back along player nav_path to find furthest point to use as distance traveled
# TODO: Add ability to get a point from a percentage along the nav_path
# TODO: Add ability to look forward n distance from where the intersect or player is
# TODO: Add ability to check if player can travel to path without obstacles. if not find another near segment to check from.
# TODO: Add ability to get path ahead from vector3 to distance
extends Node
var director:Node
export(Color) var path_color = Color.red
export(Color) var path_color2 = Color.blue
export(Color) var path_color3 = Color.green
var path_material = SpatialMaterial.new()
var path_material2 = SpatialMaterial.new()
var path_material3 = SpatialMaterial.new()
var path_geometry = ImmediateGeometry.new()
var path_geometry2 = ImmediateGeometry.new()
var path_geometry3 = ImmediateGeometry.new()
var path_geometry4 = ImmediateGeometry.new()
func _ready() -> void:
#create an immediate geometry node
add_child(path_geometry)
add_child(path_geometry2)
add_child(path_geometry3)
add_child(path_geometry4)
path_material.albedo_color = path_color
# path_material.vertex_color_use_as_albedo = false
path_material.params_line_width = 1
path_material.params_point_size = 2
path_material.flags_unshaded = true
path_material.flags_no_depth_test = true
path_material.params_grow = true
path_material.params_grow_amount = 2
path_material2.albedo_color = path_color2
# path_material2.vertex_color_use_as_albedo = false
path_material2.params_line_width = 1
path_material2.params_point_size = 2
path_material2.flags_unshaded = true
path_material2.flags_no_depth_test = true
path_material2.params_grow = true
path_material2.params_grow_amount = 2
path_material3.albedo_color = path_color3
# path_material3.vertex_color_use_as_albedo = false
path_material3.params_line_width = 1
path_material3.params_point_size = 2
path_material3.flags_unshaded = true
path_material3.flags_no_depth_test = true
path_material3.params_grow = true
path_material3.params_grow_amount = 2
#needs more robust checking
director = get_node("/root").get_child(0).get_node("_director")
if director == null:
dprint("Path Manager:: Director not found, disabled.")
pass
# Starting point. Called by director
func initialize(nav_node:Navigation, path_hint_node:Spatial,path_zone_node:Spatial) -> Array:
#get waypoints
var waypoints = validate_waypoints(get_waypoints_from_node(path_hint_node))
var output_path = generate_path(nav_node, waypoints)
dprint(["Is path valid?",validate_path(output_path,waypoints,path_zone_node)])
_draw_path(output_path,path_geometry,path_geometry2,path_material,path_material2)
# _draw_path2(build_path(nav_node, waypoints),path_geometry,path_material)
return output_path
pass
# takes a parent node with child waypoint nodes. Finds all waypoints
# Gives and array of waypoint nodes
func get_waypoints_from_node(path_hint_node:Spatial) -> Array:
var child_nodes = path_hint_node.get_children()
var path_nodes = []
for node in child_nodes:
if node is Waypoint:
path_nodes.append(node)
return path_nodes
pass
func validate_path(path:Array, waypoints:Array,path_zone_node:Spatial) -> bool:
var aa:Array = validate_waypoints(waypoints)
if aa.size() <= 0:
#It is missing at least 1 of the required waypoints
return false
var start_node:Waypoint
var end_node:Waypoint
for waypoint in aa:
if waypoint.name == "_player_spawn":
start_node = waypoint
if waypoint.name == "_safe_room":
end_node = waypoint
var bb = get_path_zones(path_zone_node)
if is_valid_path_zones(bb) == false:
#It is missing at least 1 of the required zones
return false
var start_zone:Area
var end_zone:Area
for zone in bb:
if zone.name == "_player_spawn":
start_zone = zone
if zone.name == "_safe_room":
end_zone = zone
if point_in_area(start_node.global_transform.origin,start_zone) == false:
return false
if point_in_area(end_node.global_transform.origin,end_zone) == false:
return false
#check if first and last points are in safe zones
if path.size()>1:
if point_in_area(path[0],start_zone) == false:
return false
if point_in_area(path[path.size()-1],end_zone) == false:
return false
else:
return false
return true
# Takes a list of waypoints, finds required waypoints, then traverses them to see if they go from begginging
# to the end.
# Gives an array of Waypoint Nodes
func validate_waypoints(waypoints:Array) -> Array :
var start_node:Spatial = null
for waypoint in waypoints:
if waypoint.name == "_player_spawn":
start_node = waypoint
break
if start_node == null:
dprint("_player_spawn NOT found!")
return []
var visited_nodes: Array = []
var next_node:Waypoint = start_node
while true:
if (visited_nodes.find(next_node)) < 0:
visited_nodes.append(next_node)
if next_node.next_node() != null:
next_node = next_node.next_node()
else:
dprint("Node ordering Complete")
break
else:
dprint("Looping, Halting")
break
if next_node.name != "_safe_room":
dprint("_safe_room NOT found!")
return []
return visited_nodes
# Takes validated waypoints and connects them together using the navigation mesh.
# Gives an array of Vector3
func generate_path(nav_node:Navigation, waypoints:Array) -> Array:
var points = validate_waypoints(waypoints)
var path = []
for i in range(points.size()-1):
var start = points[i].global_transform.origin
var end = points[i+1].global_transform.origin
var local_path = nav_node.get_simple_path(start - nav_node.global_transform.origin ,end - nav_node.global_transform.origin)
if local_path.size() > 0:
# path.append_array(local_path)
for point in local_path:
path.append(point)
else:
path.append(end)
# print(local_path.size())
path = optimize_path(path)
return path
# Checks an Area node to see if a point is inside it
func point_in_area(point:Vector3,area:Area, collide_with_bodies:bool = false, collide_with_areas: bool = true) -> bool:
var _shape = SphereShape.new()
var _temp_spatial = Spatial.new()
add_child(_temp_spatial)
var _space_state = _temp_spatial.get_world().direct_space_state
var _query = PhysicsShapeQueryParameters.new()
_shape.radius = .00001
_query.shape_rid = _shape.get_rid()
_query.transform.origin = point
_query.collide_with_bodies = collide_with_bodies
_query.collide_with_areas = collide_with_areas
_query.exclude = [_shape.get_rid()]
var _result = _space_state.intersect_shape(_query)
remove_child(_temp_spatial)
_temp_spatial.queue_free()
for item in _result:
if item.collider == area:
return true
return false
# Optimize Path
# Removes consecutive linear points (repeated vector3's in a row), and merges line segments with the similar normals
# path:Array-> An array of Vector3
# remove_duplicates:bool = true -> Should the repeated Vector3's be removed?
# margin:float-> The level of floating point precision when joining the line segments.
func optimize_path(path:Array,remove_duplicates:bool = true, margin = .01):
# return optimize_path2(path)
#fail early
if path.size() < 1:
return path
var uniq = []
#remove linear duplicates
if remove_duplicates == true:
uniq.append(path[0])
for _point in path:
if _point != uniq[uniq.size()-1]:
uniq.append(_point)
else:
uniq = path
#join straight lines
var last_normal:Vector3
var optimized_list = []
var current_normal:Vector3
for i in range(1,uniq.size()):
var start:Vector3 = uniq[i-1]
var end:Vector3 = uniq[i]
current_normal = stepify_vector(start.direction_to(end),margin)
if (last_normal != current_normal):
optimized_list.append(start)
last_normal = current_normal
optimized_list.append(uniq[uniq.size()-1])
return optimized_list
# Takes a Vector3 and a float and changes the decimal precision
# Gives a Vector3
func stepify_vector(Vector:Vector3, step) -> Vector3:
return Vector.snapped(Vector3.ONE *step)
# Print function that links to director, tells where text is comming from
func dprint(args=[]) -> void:
if director != null:
director.dprint(str("Path Generator::",args))
else:
print(str("Path Generator::",args))
func _draw_path2(path: Array,geometry:ImmediateGeometry,material:SpatialMaterial) -> void:
geometry.clear()
geometry.set_material_override(material)
geometry.begin(Mesh.PRIMITIVE_LINES, null)
for i in range(0, path.size()-1):
geometry.add_vertex(path[i])
geometry.add_vertex(path[i+1])
geometry.end()
func _draw_path(path: Array,geometry0:ImmediateGeometry,geometry1:ImmediateGeometry,material0:SpatialMaterial,material1:SpatialMaterial) -> void:
geometry0.clear()
geometry1.clear()
var aa = true
geometry0.set_material_override(material0)
geometry0.begin(Mesh.PRIMITIVE_LINES, null)
geometry1.set_material_override(material1)
geometry1.begin(Mesh.PRIMITIVE_LINES, null)
for i in range(0, path.size()-1 ):
if aa== true:
geometry0.add_vertex(path[i])
geometry0.add_vertex(path[i+1])
else:
geometry1.add_vertex(path[i])
geometry1.add_vertex(path[i+1])
aa = !aa
geometry0.end()
geometry1.end()
func optimize_path2(path:Array,remove_duplicates = true) -> Array:
#fail early
if path.size() <= 1:
return path
var uniq = []
#remove linear duplicates
if remove_duplicates == true:
uniq.append(path[0])
for _point in path:
if _point != uniq[uniq.size()-1]:
uniq.append(_point)
else:
uniq = path
#join straight lines
var current_normal:Vector3
var last_normal:Vector3
var optimized_list = []
# print(uniq.size())
for i in range(1,uniq.size()):
var start:Vector3 = uniq[i-1]
var end:Vector3 = uniq[i]
current_normal = stepify_vector(start.direction_to(end),.01)
# print(current_normal , last_normal == current_normal, [start, end] )
if (last_normal != current_normal):
optimized_list.append(start)
last_normal = current_normal
optimized_list.append(uniq[uniq.size()-1])
return optimized_list
func get_path_zones(_path_zone_node:Spatial) -> Array:
var child_nodes = _path_zone_node.get_children()
var zone_nodes = []
for node in child_nodes:
if node is Area:
zone_nodes.append(node)
return zone_nodes
func is_valid_path_zones(_path_zones:Array) -> bool:
var found_nodes = []
for node in _path_zones:
match node.name:
"_player_spawn":
# dprint("spawn zone found")
found_nodes.append(node)
"_safe_room":
found_nodes.append(node)
# dprint("safe zone found")
if found_nodes.size() != 2:
return false
# dprint("Start and end zones found")
return true
func path_length(path:Array) -> float:
var distance = 0.0
for i in range(0, path.size()-1):
distance += path[i].distance_to(path[i+1])
return distance
func get_point_from_distance(path_array:Array,distance:float) -> Vector3:
var distance_clamped = clamp(distance,0,path_length(path_array))
var working_distance:float = 0
var normal:Vector3
var start_point:Vector3
for i in range(0, path_array.size()-1):
var temp_distance = path_array[i].distance_to(path_array[i+1])
if working_distance + temp_distance >= distance_clamped:
normal = path_array[i].direction_to(path_array[i+1])
start_point = path_array[i]
break
else:
working_distance += temp_distance
return start_point + ((distance_clamped - working_distance) * normal)
func get_point_ahead(path_array:Array, start_distance:float, distance_ahead:float) -> Vector3:
return get_point_from_distance(path_array,start_distance+distance_ahead)
pass
# Gets distance strictly along the path
func get_distance_from_point(path_array:Array, global_point:Vector3) -> float:
#get the distance from each segment
var seg_list = direct_distance_from_each_segment( path_array, global_point)
#find the segment with the shortest distance
var current_segment = find_nearest_euclidean_segment(seg_list)
var intersect = get_nearest_point_on_line_segment(path_array[current_segment],path_array[current_segment+1],global_point)
var completed_distance = 0.0
if current_segment>0:
for i in range(0, current_segment):
completed_distance += path_array[i].distance_to(path_array[i+1])
completed_distance += path_array[current_segment].distance_to(intersect)
else:
completed_distance = path_array[current_segment].distance_to(intersect)
return (completed_distance)
# Measures distance of global_point to closest point in each segment
func direct_distance_from_each_segment(path_flow:Array, global_point:Vector3):
var seg_list = []
for i in range(0, path_flow.size()-1):
var intersect:Vector3 = get_nearest_point_on_line_segment(path_flow[i],path_flow[i+1],global_point)
seg_list.append(intersect.distance_to(global_point))
return seg_list
func find_nearest_euclidean_segment(segment_distances:Array):
var min_dist
var current_seg = 0
min_dist = segment_distances[0]
for i in range(0, segment_distances.size()):
if (segment_distances[i] <min_dist):
current_seg = i
min_dist = segment_distances[i]
return current_seg
pass
# Measures distance of global_point to closest point in each segment
func get_nearest_point_on_line_segment(start:Vector3, end:Vector3, point:Vector3):
#https://forum.unity.com/threads/how-do-i-find-the-closest-point-on-a-line.340058/#post-5839648
var linedir = start.direction_to(end)
var v = point - start
var d = v.dot(linedir)
d = clamp(d, 0.0, start.distance_to(end))
return start + linedir * d
pass
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment