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
|
extends Navigation
# Member variables
const SPEED = 4.0
var camrot = 0.0
var begin = Vector3()
var end = Vector3()
var m = FixedMaterial.new()
var path = []
var draw_path = false
func _process(delta):
if (path.size() > 1):
var to_walk = delta*SPEED
var to_watch = Vector3(0, 1, 0)
while(to_walk > 0 and path.size() >= 2):
var pfrom = path[path.size() - 1]
var pto = path[path.size() - 2]
to_watch = (pto - pfrom).normalized()
var d = pfrom.distance_to(pto)
if (d <= to_walk):
path.remove(path.size() - 1)
to_walk -= d
else:
path[path.size() - 1] = pfrom.linear_interpolate(pto, to_walk/d)
to_walk = 0
var atpos = path[path.size() - 1]
var atdir = to_watch
atdir.y = 0
var t = Transform()
t.origin = atpos
t=t.looking_at(atpos + atdir, Vector3(0, 1, 0))
get_node("robot_base").set_transform(t)
if (path.size() < 2):
path = []
set_process(false)
else:
set_process(false)
func _update_path():
var p = get_simple_path(begin, end, true)
path = Array(p) # Vector3array too complex to use, convert to regular array
path.invert()
set_process(true)
if (draw_path):
var im = get_node("draw")
im.set_material_override(m)
im.clear()
im.begin(Mesh.PRIMITIVE_POINTS, null)
im.add_vertex(begin)
im.add_vertex(end)
im.end()
im.begin(Mesh.PRIMITIVE_LINE_STRIP, null)
for x in p:
im.add_vertex(x)
im.end()
func _input(event):
if (event.type == InputEvent.MOUSE_BUTTON and event.button_index == BUTTON_LEFT and event.pressed):
var from = get_node("cambase/Camera").project_ray_origin(event.pos)
var to = from + get_node("cambase/Camera").project_ray_normal(event.pos)*100
var p = get_closest_point_to_segment(from, to)
begin = get_closest_point(get_node("robot_base").get_translation())
end = p
_update_path()
if (event.type == InputEvent.MOUSE_MOTION):
if (event.button_mask&BUTTON_MASK_MIDDLE):
camrot += event.relative_x*0.005
get_node("cambase").set_rotation(Vector3(0, camrot, 0))
print("camrot ", camrot)
func _ready():
set_process_input(true)
m.set_line_width(3)
m.set_point_size(3)
m.set_fixed_flag(FixedMaterial.FLAG_USE_POINT_SIZE, true)
m.set_flag(Material.FLAG_UNSHADED, true)
#begin = get_closest_point(get_node("start").get_translation())
#end = get_closest_point(get_node("end").get_translation())
#call_deferred("_update_path")
|