diff --git a/src/unit.cpp b/src/unit.cpp
index 131c9bd..bdb23dd 100644
--- a/src/unit.cpp
+++ b/src/unit.cpp
@@ -5,6 +5,8 @@
 #include "rts_states.hpp"
 #include "utils/godot_macros.hpp"
 #include <godot_cpp/classes/navigation_agent3d.hpp>
+#include <godot_cpp/classes/navigation_server3d.hpp>
+#include <godot_cpp/classes/world3d.hpp>
 #include <godot_cpp/variant/callable_method_pointer.hpp>
 #include <godot_cpp/variant/utility_functions.hpp>
 
@@ -29,8 +31,15 @@ void Unit::_enter_tree() { GDGAMEONLY();
     this->health->connect("death", callable_mp(this, &Unit::on_death));
 }
 
-void Unit::_physics_process(double) {
+void Unit::_physics_process(double) { GDGAMEONLY();
     this->move_and_slide();
+    if(!this->get_velocity().is_zero_approx()) {
+        gd::RID const map_id{this->get_world_3d()->get_navigation_map()};
+        gd::Vector3 const offset{0.f, -float(this->agent->get_path_height_offset()), 0.f};
+        gd::Vector3 const adjusted_pos{this->get_global_position() + offset};
+        this->set_global_position(gd::NavigationServer3D::get_singleton()->map_get_closest_point(map_id, adjusted_pos) + offset);
+    }
+    this->set_velocity({0.f, 0.f, 0.f});
 }
 
 void Unit::stop_plan() {
@@ -104,8 +113,6 @@ void Unit::on_velocity_computed(gd::Vector3 vel) {
     gd::Vector3 const pos{this->get_global_position()};
     if(vel.x == 0 && vel.z == 0)
         return;
-    if(gd::Object::cast_to<MoveTo>(this->state) == nullptr)
-        return;
     this->set_velocity(vel.normalized() * this->movement_speed);
     this->look_at(pos - gd::Vector3{vel.x, 0.f, vel.z});
 }