1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
|
extends CharacterBody2D
# editor variables
@export var max_speed = 800
@export var steer_force = 20
@export var acceleration = 8
@export var mass = 1.0
@export var look_ahead = 75
@export var num_rays = 32
@export var driving = false
# context array
var ray_directions = []
var interest = []
var danger = []
var chosen_dir = Vector2.ZERO
var current_speed = 0
# Called when the node enters the scene tree for the first time.
func _ready():
interest.resize(num_rays)
danger.resize(num_rays)
ray_directions.resize(num_rays)
for i in num_rays:
var angle = i * 2 * PI / num_rays
ray_directions[i] = Vector2.RIGHT.rotated(angle)
func _physics_process(delta):
if not driving:
return
# Code for getting the next desired direction based on the environment
set_interest()
set_danger()
choose_direction()
# example_movement(delta)
# own_movement(delta)
own_movement_improved(delta)
func set_interest():
# Set interest in each slot based on world direction
# if owner and owner.has_method("get_path_direction"):
if owner and owner.has_method("get_path_next_position"):
# var path_direction = owner.get_path_direction(position)
var next_pos = owner.get_path_next_position(position)
var path_direction = (next_pos - position).normalized()
for i in num_rays:
var d = ray_directions[i].rotated(rotation).dot(path_direction)
interest[i] = max(0, d)
# If no world path, use default interest
else:
set_default_interest()
func set_default_interest():
# Default to moving forward
for i in num_rays:
var d = ray_directions[i].rotated(rotation).dot(transform.x)
interest[i] = max(0, d)
func set_danger():
# Cast rays to find danger directions
var space_state = get_world_2d().direct_space_state
var params = PhysicsRayQueryParameters2D.new()
params.from = position
params.exclude = [self]
for i in num_rays:
params.to = position + ray_directions[i].rotated(rotation) * look_ahead
var result = space_state.intersect_ray(params)
danger[i] = 1.0 if result else 0.0
func choose_direction():
# Eliminate interest in slots with danger
for i in num_rays:
if danger[i] > 0.0:
interest[i] = 0.0
# Choose direction based on remaining interest
chosen_dir = Vector2.ZERO
for i in num_rays:
chosen_dir += ray_directions[i] * interest[i]
chosen_dir = chosen_dir.normalized()
func example_movement(delta):
var desired_velocity = chosen_dir.rotated(rotation) * max_speed
velocity = velocity.lerp(desired_velocity, steer_force)
rotation = velocity.angle()
move_and_collide(velocity * delta)
func own_movement(delta):
var desired_velocity = chosen_dir.rotated(rotation) * max_speed
var new_speed = lerpf(velocity.length(), max_speed, acceleration)
var new_angle = lerp_angle(velocity.angle(), desired_velocity.angle(), steer_force)
velocity = Vector2.from_angle(new_angle) * new_speed
rotation = new_angle
move_and_collide(velocity * delta)
func own_movement_improved(delta):
var desired_velocity = chosen_dir.rotated(rotation) * max_speed
print('==========')
print('current speed: ', velocity.length(), ' | current angle: ', velocity.angle())
print('desired speed: ', desired_velocity.length(), ' | desired angle: ', desired_velocity.angle())
# var possible_radius = mass * velocity.length_squared() / steer_force
# var possible_steer_angle = velocity.length() / possible_radius
var possible_steer_angle = steer_force / (mass * velocity.length())
var angle_change = clampf(wrapf(desired_velocity.angle() - velocity.angle(), -PI, PI), -possible_steer_angle, possible_steer_angle)
var new_angle = wrapf(velocity.angle() + angle_change, -PI, PI)
print('possible_steer_angle: ', possible_steer_angle, ' | angle_change: ', angle_change, ' | new_angle: ', new_angle )
var wanted_speed = steer_force / (mass * absf(desired_velocity.angle() - velocity.angle()))
var new_max_speed = min(velocity.length() + (acceleration), max_speed)
var new_min_speed = max(velocity.length() - (acceleration), 0)
var new_speed = clampf(wanted_speed, new_min_speed, new_max_speed)
print('wanted: ', wanted_speed, ' | min: ', new_min_speed, ' | max: ', new_max_speed, ' | new: ', new_speed)
velocity = Vector2.from_angle(new_angle) * new_speed
rotation = new_angle
move_and_collide(velocity * delta)
|
