67 lines
2.2 KiB
GDScript
67 lines
2.2 KiB
GDScript
extends MeshInstance3D
|
|
|
|
var trajectory = Trajectory.new()
|
|
@export var wheelbase_to_front: float = 3.78
|
|
|
|
func velocity_to_normalized_value(velocity):
|
|
return clamp(velocity / 5.0, 0.0, 1.0)
|
|
func velocity_to_color(velocity):
|
|
var alpha = clamp(velocity_to_normalized_value(velocity), 0.2, 0.9)
|
|
return Color(0, 0.2, 1.0, alpha)
|
|
|
|
func _ready():
|
|
trajectory.subscribe("/planning/scenario_planning/trajectory", false)
|
|
|
|
func _process(_delta):
|
|
if !trajectory.has_new():
|
|
return
|
|
|
|
# Trajectory
|
|
var traj_arr = []
|
|
traj_arr.resize(Mesh.ARRAY_MAX)
|
|
var traj_verts = PackedVector3Array()
|
|
#var traj_uvs = PackedVector2Array()
|
|
var traj_normals = PackedVector3Array()
|
|
#var traj_indices = PackedInt32Array()
|
|
var traj_colors = PackedColorArray()
|
|
# Create triangle
|
|
var trajectory_triangle_strip = trajectory.get_trajectory_triangle_strip(2.0)
|
|
for point in trajectory_triangle_strip:
|
|
traj_verts.append(point["position"])
|
|
traj_normals.append(point["normal"])
|
|
#traj_uvs.append(Vector2(velocity_to_normalized_value(point["velocity"]), 0))
|
|
traj_colors.append(Color(0.0, 0.02, 1.0, 0.8))
|
|
traj_arr[Mesh.ARRAY_VERTEX] = traj_verts
|
|
traj_arr[Mesh.ARRAY_NORMAL] = traj_normals
|
|
traj_arr[Mesh.ARRAY_COLOR] = traj_colors
|
|
# traj_arr[Mesh.ARRAY_INDEX] = traj_indices
|
|
# traj_arr[Mesh.ARRAY_TEX_UV] = traj_uvs
|
|
|
|
|
|
# Wall
|
|
var wall_arr = []
|
|
wall_arr.resize(Mesh.ARRAY_MAX)
|
|
var wall_verts = PackedVector3Array()
|
|
#var wall_uvs = PackedVector2Array()
|
|
var wall_normals = PackedVector3Array()
|
|
#var wall_indices = PackedInt32Array()
|
|
var wall_colors = PackedColorArray()
|
|
# Create triangle
|
|
var wall_triangle_strip = trajectory.get_wall_triangle_strip(4.0, 2.0, wheelbase_to_front, true, true)
|
|
for point in wall_triangle_strip:
|
|
wall_verts.append(point["position"])
|
|
wall_normals.append(point["normal"])
|
|
wall_colors.append(point["color"])
|
|
wall_arr[Mesh.ARRAY_VERTEX] = wall_verts
|
|
wall_arr[Mesh.ARRAY_NORMAL] = wall_normals
|
|
wall_arr[Mesh.ARRAY_COLOR] = wall_colors
|
|
|
|
if !traj_verts.is_empty():
|
|
mesh.clear_surfaces()
|
|
# Trajectory
|
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLE_STRIP, traj_arr)
|
|
# Wall
|
|
if !wall_triangle_strip.is_empty():
|
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLE_STRIP, wall_arr)
|
|
trajectory.set_old()
|