51 lines
1.8 KiB
GDScript3
51 lines
1.8 KiB
GDScript3
|
extends MeshInstance3D
|
||
|
|
|
||
|
|
var path = BehaviorPath.new()
|
||
|
|
|
||
|
|
func _ready():
|
||
|
|
path.subscribe("/planning/scenario_planning/lane_driving/behavior_planning/path", false)
|
||
|
|
|
||
|
|
func _process(_delta):
|
||
|
|
if !path.has_new():
|
||
|
|
return
|
||
|
|
|
||
|
|
# Drivable area
|
||
|
|
var drivable_area_triangle_strip = path.get_drivable_area_triangle_strip(0.1)
|
||
|
|
|
||
|
|
# Left line
|
||
|
|
var left_line_triangle_strip = drivable_area_triangle_strip["left_line"]
|
||
|
|
var left_line_arr = []
|
||
|
|
left_line_arr.resize(Mesh.ARRAY_MAX)
|
||
|
|
var left_line_verts = PackedVector3Array()
|
||
|
|
var left_line_normals = PackedVector3Array()
|
||
|
|
var left_line_colors = PackedColorArray()
|
||
|
|
for point in left_line_triangle_strip:
|
||
|
|
left_line_verts.append(point["position"])
|
||
|
|
left_line_normals.append(point["normal"])
|
||
|
|
left_line_colors.append(Color(0.0, 0.25, 1.0, 0.9))
|
||
|
|
left_line_arr[Mesh.ARRAY_VERTEX] = left_line_verts
|
||
|
|
left_line_arr[Mesh.ARRAY_NORMAL] = left_line_normals
|
||
|
|
left_line_arr[Mesh.ARRAY_COLOR] = left_line_colors
|
||
|
|
|
||
|
|
# Right line
|
||
|
|
var right_line_triangle_strip = drivable_area_triangle_strip["right_line"]
|
||
|
|
var right_line_arr = []
|
||
|
|
right_line_arr.resize(Mesh.ARRAY_MAX)
|
||
|
|
var right_line_verts = PackedVector3Array()
|
||
|
|
var right_line_normals = PackedVector3Array()
|
||
|
|
var right_line_colors = PackedColorArray()
|
||
|
|
for point in right_line_triangle_strip:
|
||
|
|
right_line_verts.append(point["position"])
|
||
|
|
right_line_normals.append(point["normal"])
|
||
|
|
right_line_colors.append(Color(0.0, 0.25, 1.0, 0.9))
|
||
|
|
right_line_arr[Mesh.ARRAY_VERTEX] = right_line_verts
|
||
|
|
right_line_arr[Mesh.ARRAY_NORMAL] = right_line_normals
|
||
|
|
right_line_arr[Mesh.ARRAY_COLOR] = right_line_colors
|
||
|
|
|
||
|
|
if !left_line_verts.is_empty():
|
||
|
|
mesh.clear_surfaces()
|
||
|
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLE_STRIP, left_line_arr)
|
||
|
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLE_STRIP, right_line_arr)
|
||
|
|
path.set_old()
|
||
|
|
|