From 1b61bc4bb055adb025429abccf958e9529c1f9dd Mon Sep 17 00:00:00 2001 From: David Briscoe <43559+idbrii@users.noreply.github.com> Date: Tue, 17 Oct 2023 18:23:57 -0700 Subject: [PATCH] Use NavigationAgent3D in 3D Navigation demo (#975) Co-authored-by: Hugo Locurcio --- 3d/navigation/character.gd | 45 +++++++++++++++ 3d/navigation/line3d.gd | 22 ++++++++ 3d/navigation/navmesh.gd | 88 ++++++------------------------ 3d/navigation/navmesh.res | Bin 2303 -> 2287 bytes 3d/navigation/navmesh.tscn | 27 ++++----- 3d/navigation/particle.png | Bin 129 -> 0 bytes 3d/navigation/particle.png.import | 36 ------------ 7 files changed, 95 insertions(+), 123 deletions(-) create mode 100644 3d/navigation/character.gd create mode 100644 3d/navigation/line3d.gd delete mode 100644 3d/navigation/particle.png delete mode 100644 3d/navigation/particle.png.import diff --git a/3d/navigation/character.gd b/3d/navigation/character.gd new file mode 100644 index 00000000..bc00fdd1 --- /dev/null +++ b/3d/navigation/character.gd @@ -0,0 +1,45 @@ +extends Marker3D + + +const Line3D = preload("res://line3d.gd") + +@export var character_speed := 10.0 +@export var show_path := true + +@onready var _nav_agent := $NavigationAgent3D as NavigationAgent3D + +var _nav_path_line : Line3D + + +func _ready(): + _nav_path_line = Line3D.new() + add_child(_nav_path_line) + _nav_path_line.set_as_top_level(true) + + +func _physics_process(delta): + if _nav_agent.is_navigation_finished(): + return + var next_position := _nav_agent.get_next_path_position() + var offset := next_position - global_position + global_position = global_position.move_toward(next_position, delta * character_speed) + + # Make the robot look at the direction we're traveling. + # Clamp y to 0 so the robot only looks left and right, not up/down. + offset.y = 0 + look_at(global_position + offset, Vector3.UP) + + +func set_target_position(target_position: Vector3): + _nav_agent.set_target_position(target_position) + # Get a full navigation path with the NavigationServer API. + if show_path: + var start_position := global_transform.origin + var optimize := true + var navigation_map := get_world_3d().get_navigation_map() + var path := NavigationServer3D.map_get_path( + navigation_map, + start_position, + target_position, + optimize) + _nav_path_line.draw_path(path) diff --git a/3d/navigation/line3d.gd b/3d/navigation/line3d.gd new file mode 100644 index 00000000..eb6193d4 --- /dev/null +++ b/3d/navigation/line3d.gd @@ -0,0 +1,22 @@ +extends MeshInstance3D + + +func _ready(): + set_mesh(ImmediateMesh.new()) + var material := StandardMaterial3D.new() + material.flags_unshaded = true + material.albedo_color = Color.WHITE + set_material_override(material) + + +func draw_path(path): + var im: ImmediateMesh = mesh + im.clear_surfaces() + im.surface_begin(Mesh.PRIMITIVE_POINTS, null) + im.surface_add_vertex(path[0]) + im.surface_add_vertex(path[path.size() - 1]) + im.surface_end() + im.surface_begin(Mesh.PRIMITIVE_LINE_STRIP, null) + for current_vector in path: + im.surface_add_vertex(current_vector) + im.surface_end() diff --git a/3d/navigation/navmesh.gd b/3d/navigation/navmesh.gd index e722b094..437afe5b 100644 --- a/3d/navigation/navmesh.gd +++ b/3d/navigation/navmesh.gd @@ -1,84 +1,30 @@ extends Node3D -const SPEED := 10.0 -@export var show_path := true +const Character = preload("res://character.gd") -var cam_rotation := 0.0 -var path: PackedVector3Array +@onready var _camera := $CameraBase/Camera3D as Camera3D +@onready var _robot := $RobotBase as Character -@onready var robot: Marker3D = $RobotBase -@onready var camera: Camera3D = $CameraBase/Camera3D - -func _ready(): - set_process_input(true) - - -func _physics_process(delta: float): - var direction := Vector3() - - # We need to scale the movement speed by how much delta has passed, - # otherwise the motion won't be smooth. - var step_size := delta * SPEED - - if not path.is_empty(): - # Direction is the difference between where we are now - # and where we want to go. - var destination := path[0] - direction = destination - robot.position - - # If the next node is closer than we intend to 'step', then - # take a smaller step. Otherwise we would go past it and - # potentially go through a wall or over a cliff edge! - if step_size > direction.length(): - step_size = direction.length() - # We should also remove this node since we're about to reach it. - path.remove_at(0) - - # Move the robot towards the path node, by how far we want to travel. - # TODO: This information should be set to the CharacterBody properties instead of arguments. - # Note: For a CharacterBody3D, we would instead use move_and_slide - # so collisions work properly. - robot.position += direction.normalized() * step_size - - # Lastly let's make sure we're looking in the direction we're traveling. - # Clamp y to 0 so the robot only looks left and right, not up/down. - direction.y = 0 - if direction: - # Direction is relative, so apply it to the robot's location to - # get a point we can actually look at. - var look_at_point := robot.position + direction.normalized() - # Make the robot look at the point. - robot.look_at(look_at_point, Vector3.UP) +var _cam_rotation := 0.0 func _unhandled_input(event: InputEvent): if event is InputEventMouseButton and event.button_index == MOUSE_BUTTON_LEFT and event.pressed: - var map := get_world_3d().navigation_map - var from := camera.project_ray_origin(event.position) - var to := from + camera.project_ray_normal(event.position) * 1000 - var target_point := NavigationServer3D.map_get_closest_point_to_segment(map, from, to) + # Get closest point on navmesh for the current mouse cursor position. + var mouse_cursor_position = event.position + var camera_ray_length := 1000.0 + var camera_ray_start := _camera.project_ray_origin(mouse_cursor_position) + var camera_ray_end := camera_ray_start + _camera.project_ray_normal(mouse_cursor_position) * camera_ray_length - # Set the path between the robot's current location and our target. - path = NavigationServer3D.map_get_path(map, robot.position, target_point, true) - - if show_path: - draw_path(path) + var closest_point_on_navmesh := NavigationServer3D.map_get_closest_point_to_segment( + get_world_3d().navigation_map, + camera_ray_start, + camera_ray_end + ) + _robot.set_target_position(closest_point_on_navmesh) elif event is InputEventMouseMotion: if event.button_mask & (MOUSE_BUTTON_MASK_MIDDLE + MOUSE_BUTTON_MASK_RIGHT): - cam_rotation += event.relative.x * 0.005 - $CameraBase.set_rotation(Vector3(0, cam_rotation, 0)) - - -func draw_path(path_array: PackedVector3Array) -> void: - var im: ImmediateMesh = $DrawPath.mesh - im.clear_surfaces() - im.surface_begin(Mesh.PRIMITIVE_POINTS, null) - im.surface_add_vertex(path_array[0]) - im.surface_add_vertex(path_array[path_array.size() - 1]) - im.surface_end() - im.surface_begin(Mesh.PRIMITIVE_LINE_STRIP, null) - for current_vector in path: - im.surface_add_vertex(current_vector) - im.surface_end() + _cam_rotation += event.relative.x * 0.005 + $CameraBase.set_rotation(Vector3.UP * _cam_rotation) diff --git a/3d/navigation/navmesh.res b/3d/navigation/navmesh.res index a768b0664f617a2a55780594ce5c55ab490ba8b6..cafb917904cb7964e9bc351acc3e4f6274641b4f 100644 GIT binary patch literal 2287 zcmVkN>iPe*ZLA5u>aAQ=6W+J~4_>-ES30=cy6#S4l-=t_ zZ+oNeX27brl5JIVyF1seZw;rewvx%j{rpu@Z>9^g8#VtUIGm#DPFvqJ)VEyMZTo-V z`_|&h+kTr|c;niQ{*UmTYl2G^cFkP5*Bs7#{|EhLTeN9ybMLmPu)AmcH}H)!*#zJ0 zw!LKwm#b-O*K*rjrS{$Kf1&@b7iiH4AvL~b{_{M2_X@UEVG~tnm@NJ)_%Gl;fd4&z zagmTOiw7ms(0vqT#C$P$P_j#RswrViCP4ei)PzCuxtV8qMK2s-88P`qn`63e< z6hk~5z9Q#M+i>k<;De{O;d|mi=^Rco`H9JS%%hW=UijLp<3}B{f%#V8Rc&D433C z{;|tz&}Mx>_Y6Z(dK`*+f}Pq0O(z9YEutr0(IVkK5i|7*gmR`Mo4INv;z5l^e%X&RH~t7k zpPJAEmUY(Y61s%$BadIUfG0J^Ia4DIziXf18Ml68{~`Q$x&A|V5ESHKr~%Nw;77rq zpylH;phv)YcqaVUL0)JxLBCMsNDt%*FC`|B9_qv)u1;DY0up0#WD)?umvS~W=Y5nNY`_mW>c_4%|?Egqj-Z+Q%p+Og2twR`pypcGxbvd6lyB4Q@^xe ziFyZ?sMo;GRujq4oH?=wNZ3O6A03ikDU4}3@<|4DV&4v^&+LHO2okH@v5ll&+t7{lN(bfw!4|XnT{ch}LofL#}rd2T|Ihb7N zDDt(GfsLr)6=9CZjEJNlNg=HP@dU&%2^~8g5+F4d22==|HYEkA4Er4`Z?13dfBi#a zU3uGA>L{VhjO@`HQ*+m!ymS@X~&`*KO*Hr{x*;dcsld4_Z=jI_^*Bw`oTRte=vB! zKaq#}C3R!Q2PW}+8HIW>1B#rO&J_9`N&ug^oLKDPEclmC?Vo}?Nk6}OkYdUX6WQfj zDEGF&Bm>dg)M0g%=q_$k{jt8RU6gP3;b1O2M)D;bA7>pqUzhON_#Uub#3A;MvkcrD zxHVnB=5P7ZDT|SSJ3_JpWz@JzzZq`xWS#QhuV%;iFUA1R`8Q2I|78*XBOYQ`Py6H! ziB*Z;_-ucI?R~6xO8@K8Tx*NGiZ-}6b3$90kKow+DXHJqc~?=`g5oNo9-3cXvD|z9 z=miw`YvU2tw+vj}#see$yC3m&!t?ZuZoP=N)Yce;p40ztVOqXXhg`>T!(XQ&-eIZGxhaGK06 zdM|#$Ave@bMH8diNuqr-|w(lk)xT8 zFMcRJ71~q&h05(W|3_g7D77#BV4Mgg5dg-cStLOA>;}Lrmp@_9B-IfLX=L|9O7zEU z3@mwl2H1Ci{ZQZO+7wLyPXJEWZ&Q1SgZPk^ z_73-;fx`wgw6n8gc!mo;-z(e#g5@L_(IGt&qeX9YM~V{t z&=DCb^g$;SND!d=`JX*}=;->~(Q`b{L+5pFXO5gWe{(o)+I-E~pgHq2S7Rn^Ke*ok zya3zW1k~Q@>@MH~j%-;oWoPze$BdoWiwzSN?7t4I*L|JWE?vjZ(wkda0WtHR^S@6BX)!@u=a|P(p79aFa^X0T6i6gf9nAKP&P0o=5ld z$3xsG{tI%Q7ss|*{W5iR$fJ_moQgp!hu8C8#8qWOkHO$Qvc%ObX=uG?xhnjA2Ko?3 zWvhxj9yHO_{1DG*kTt^MVjPFcA_}BglN2SKhQ=WUtne(?U5?oqoR@nqa+Hk_bd`yR z{oa>@Ryp)_j>gmuONK)qW-mPoo&iN5HRJ~-vYMkBCPI471gcIv8$_eQ`cJQWZr=8GO0GReQ_E!|0npHqN+}Ru5YMrxvskQA3=_ZZ6jRU z*xQn8-bx&+y6U>JwV~3c-KX}|H&&znCtT;A-co&AGgs<0cQf6)I2L_c*_^9u>f34? z{|{W>Oq_yioUXYg`3*JH8Fdw;gJBu3@`HA-vEO^{98rmo=&Iw$*z2hhT zzxDsZe*yv4+|{jz)YUf|Ca$PRg!=<{b81_2{}*UlmCU8pY_f0{XXf0XyLzpeudBqC zC{9IjW?D9}YuV~|4PwTF1nc;787=XM_RC!M$pSMby9NeK~DHM^gULOdJaZ&rE>`F3}6~0NtDLrhd3#iYj1kKm%rYg7XzD%`exE zgb*Zl;ZbF!o|up9%k0EgnJJiN3AA`T6300}PaLP5>F6fD8jE;Rqmo}H=x8!FEW0}rA+29WB+W|A09WW*LTjfhB+q>$F(1jI23Av+!t07VrARLG%4Qc#e}u-^eIhgnS`kKn>s=iED+lJvc!6yr zglv^K6w86LWp1Jz{=;^J<*^j^N51oratCs$&V0O_xPyEUH!J&OI%G~KK8SM^pVguN zu5cSitCRff8-;rs(!?hwX1c~tuvBO6CmHtbtK?Hp6`F!DAwHSyAg~%tbG(y-{*1q**bAT?^o?M+))i{B^s<0YnaA^D%jh`Ve6~00?%qR|gL@9Q zCg^MK4au7dFalRcuyDvmjVp>_O5HqQ9r7Em4jdC+)nw20hnl?oA%A?Aa89r8Y9YSD zReirr)A1ko@Ui2A{*$jp=$2;{M7y(pvaeF7{0jApWqFI>SI5FWd-YfkMLg3w+;;xh z1u*}|;Ss`HK3pA~gNy`Be?HfQ=g>Fb-hF9#rE_v$_-H8q<3)m`cU+io4l{Z9!_AH_mGLzSA7H@%+K)d zW%q_7F13##{;#b}ua>;|We$6Whve?zNV}S2(ObM)#)sl?>ju}iIH@20_ck9r zfG_RUx_$8+nbz+1@UAnQl3J4?)m@;(5R!;|t_`{imbLN`L`|fS)uNlhA?#_LzAJR3 z(Ji{G?#1GHmk9TWavQ-UAG`-TX2iR&)%k&prFV2s(T+A`sj{+JRi8qT`l2}8oamd6 z!cTrNgD~aMtFV>wxo|;Hj)W2@wJ-f(un3J20Cr?sBQSLB2Ef$7DV=8!o*J;1&O%1IXJkNxZK-E+)YGV z+d(`;hYudu*w)s*;T$xmsU5>JTu4iMgC!@w^oQCf&28X(+PoO-V(>ra_bWG25$Sm2lPOF{N&~R$;0P*zQ@k*98VoOa~|h%;JkU8yHS(oYt9DE znE`UXI^O}@Kpfr$-slLp{eF5E@BvrW>>0B=`?6%nZtTa35gYbk7xwGG?yDEC>-sKS zx%T#M>$hgXqE+j$F3Z)cR$p~is#5*bQJE_B-}Ok18i4Ys;nhF_3B4U))0CtG5OC21 z61*HB_U|oM!>$rJ%D~|s;#2(x%q6-rAFKV#dCFlw)wgAWPn$;F$l2M>EE{Uvj6+H- z%Ec~qov@MjfCC5f=9Bs- zC!M!jr@!4*jTwKvi-r0Q