1/**************************************************************************/
2/* navigation_obstacle_2d.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#include "navigation_obstacle_2d.h"
32
33#include "core/math/geometry_2d.h"
34#include "scene/resources/world_2d.h"
35#include "servers/navigation_server_2d.h"
36#include "servers/navigation_server_3d.h"
37
38void NavigationObstacle2D::_bind_methods() {
39 ClassDB::bind_method(D_METHOD("get_rid"), &NavigationObstacle2D::get_rid);
40
41 ClassDB::bind_method(D_METHOD("set_avoidance_enabled", "enabled"), &NavigationObstacle2D::set_avoidance_enabled);
42 ClassDB::bind_method(D_METHOD("get_avoidance_enabled"), &NavigationObstacle2D::get_avoidance_enabled);
43
44 ClassDB::bind_method(D_METHOD("set_navigation_map", "navigation_map"), &NavigationObstacle2D::set_navigation_map);
45 ClassDB::bind_method(D_METHOD("get_navigation_map"), &NavigationObstacle2D::get_navigation_map);
46
47 ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationObstacle2D::set_radius);
48 ClassDB::bind_method(D_METHOD("get_radius"), &NavigationObstacle2D::get_radius);
49
50 ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationObstacle2D::set_velocity);
51 ClassDB::bind_method(D_METHOD("get_velocity"), &NavigationObstacle2D::get_velocity);
52
53 ClassDB::bind_method(D_METHOD("set_vertices", "vertices"), &NavigationObstacle2D::set_vertices);
54 ClassDB::bind_method(D_METHOD("get_vertices"), &NavigationObstacle2D::get_vertices);
55
56 ClassDB::bind_method(D_METHOD("set_avoidance_layers", "layers"), &NavigationObstacle2D::set_avoidance_layers);
57 ClassDB::bind_method(D_METHOD("get_avoidance_layers"), &NavigationObstacle2D::get_avoidance_layers);
58 ClassDB::bind_method(D_METHOD("set_avoidance_layer_value", "layer_number", "value"), &NavigationObstacle2D::set_avoidance_layer_value);
59 ClassDB::bind_method(D_METHOD("get_avoidance_layer_value", "layer_number"), &NavigationObstacle2D::get_avoidance_layer_value);
60
61 ADD_GROUP("Avoidance", "");
62 ADD_PROPERTY(PropertyInfo(Variant::BOOL, "avoidance_enabled"), "set_avoidance_enabled", "get_avoidance_enabled");
63 ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "velocity", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_velocity", "get_velocity");
64 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.0,500,0.01,suffix:px"), "set_radius", "get_radius");
65 ADD_PROPERTY(PropertyInfo(Variant::PACKED_VECTOR2_ARRAY, "vertices"), "set_vertices", "get_vertices");
66 ADD_PROPERTY(PropertyInfo(Variant::INT, "avoidance_layers", PROPERTY_HINT_LAYERS_AVOIDANCE), "set_avoidance_layers", "get_avoidance_layers");
67}
68
69void NavigationObstacle2D::_notification(int p_what) {
70 switch (p_what) {
71 case NOTIFICATION_POST_ENTER_TREE: {
72 if (map_override.is_valid()) {
73 _update_map(map_override);
74 } else if (is_inside_tree()) {
75 _update_map(get_world_2d()->get_navigation_map());
76 } else {
77 _update_map(RID());
78 }
79 previous_transform = get_global_transform();
80 // need to trigger map controlled agent assignment somehow for the fake_agent since obstacles use no callback like regular agents
81 NavigationServer2D::get_singleton()->obstacle_set_avoidance_enabled(obstacle, avoidance_enabled);
82 _update_position(get_global_position());
83 set_physics_process_internal(true);
84 } break;
85
86 case NOTIFICATION_EXIT_TREE: {
87 set_physics_process_internal(false);
88 _update_map(RID());
89 } break;
90
91 case NOTIFICATION_PAUSED: {
92 if (!can_process()) {
93 map_before_pause = map_current;
94 _update_map(RID());
95 } else if (can_process() && !(map_before_pause == RID())) {
96 _update_map(map_before_pause);
97 map_before_pause = RID();
98 }
99 NavigationServer2D::get_singleton()->obstacle_set_paused(obstacle, !can_process());
100 } break;
101
102 case NOTIFICATION_UNPAUSED: {
103 if (!can_process()) {
104 map_before_pause = map_current;
105 _update_map(RID());
106 } else if (can_process() && !(map_before_pause == RID())) {
107 _update_map(map_before_pause);
108 map_before_pause = RID();
109 }
110 NavigationServer2D::get_singleton()->obstacle_set_paused(obstacle, !can_process());
111 } break;
112
113 case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
114 if (is_inside_tree()) {
115 _update_position(get_global_position());
116
117 if (velocity_submitted) {
118 velocity_submitted = false;
119 // only update if there is a noticeable change, else the rvo agent preferred velocity stays the same
120 if (!previous_velocity.is_equal_approx(velocity)) {
121 NavigationServer2D::get_singleton()->obstacle_set_velocity(obstacle, velocity);
122 }
123 previous_velocity = velocity;
124 }
125 }
126 } break;
127
128 case NOTIFICATION_DRAW: {
129#ifdef DEBUG_ENABLED
130 if (is_inside_tree()) {
131 bool is_debug_enabled = false;
132 if (Engine::get_singleton()->is_editor_hint()) {
133 is_debug_enabled = true;
134 } else if (NavigationServer2D::get_singleton()->get_debug_enabled() && NavigationServer2D::get_singleton()->get_debug_avoidance_enabled()) {
135 is_debug_enabled = true;
136 }
137
138 if (is_debug_enabled) {
139 _update_fake_agent_radius_debug();
140 _update_static_obstacle_debug();
141 }
142 }
143#endif // DEBUG_ENABLED
144 } break;
145 }
146}
147
148NavigationObstacle2D::NavigationObstacle2D() {
149 obstacle = NavigationServer2D::get_singleton()->obstacle_create();
150
151 set_radius(radius);
152 set_vertices(vertices);
153 set_avoidance_layers(avoidance_layers);
154 set_avoidance_enabled(avoidance_enabled);
155}
156
157NavigationObstacle2D::~NavigationObstacle2D() {
158 ERR_FAIL_NULL(NavigationServer2D::get_singleton());
159
160 NavigationServer2D::get_singleton()->free(obstacle);
161 obstacle = RID();
162}
163
164void NavigationObstacle2D::set_vertices(const Vector<Vector2> &p_vertices) {
165 vertices = p_vertices;
166 NavigationServer2D::get_singleton()->obstacle_set_vertices(obstacle, vertices);
167#ifdef DEBUG_ENABLED
168 queue_redraw();
169#endif // DEBUG_ENABLED
170}
171
172void NavigationObstacle2D::set_navigation_map(RID p_navigation_map) {
173 if (map_override == p_navigation_map) {
174 return;
175 }
176 map_override = p_navigation_map;
177 _update_map(map_override);
178}
179
180RID NavigationObstacle2D::get_navigation_map() const {
181 if (map_override.is_valid()) {
182 return map_override;
183 } else if (is_inside_tree()) {
184 return get_world_2d()->get_navigation_map();
185 }
186 return RID();
187}
188
189void NavigationObstacle2D::set_radius(real_t p_radius) {
190 ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive.");
191 if (Math::is_equal_approx(radius, p_radius)) {
192 return;
193 }
194
195 radius = p_radius;
196
197 NavigationServer2D::get_singleton()->obstacle_set_radius(obstacle, radius);
198#ifdef DEBUG_ENABLED
199 queue_redraw();
200#endif // DEBUG_ENABLED
201}
202
203void NavigationObstacle2D::set_avoidance_layers(uint32_t p_layers) {
204 if (avoidance_layers == p_layers) {
205 return;
206 }
207 avoidance_layers = p_layers;
208 NavigationServer2D::get_singleton()->obstacle_set_avoidance_layers(obstacle, avoidance_layers);
209}
210
211uint32_t NavigationObstacle2D::get_avoidance_layers() const {
212 return avoidance_layers;
213}
214
215void NavigationObstacle2D::set_avoidance_layer_value(int p_layer_number, bool p_value) {
216 ERR_FAIL_COND_MSG(p_layer_number < 1, "Avoidance layer number must be between 1 and 32 inclusive.");
217 ERR_FAIL_COND_MSG(p_layer_number > 32, "Avoidance layer number must be between 1 and 32 inclusive.");
218 uint32_t avoidance_layers_new = get_avoidance_layers();
219 if (p_value) {
220 avoidance_layers_new |= 1 << (p_layer_number - 1);
221 } else {
222 avoidance_layers_new &= ~(1 << (p_layer_number - 1));
223 }
224 set_avoidance_layers(avoidance_layers_new);
225}
226
227bool NavigationObstacle2D::get_avoidance_layer_value(int p_layer_number) const {
228 ERR_FAIL_COND_V_MSG(p_layer_number < 1, false, "Avoidance layer number must be between 1 and 32 inclusive.");
229 ERR_FAIL_COND_V_MSG(p_layer_number > 32, false, "Avoidance layer number must be between 1 and 32 inclusive.");
230 return get_avoidance_layers() & (1 << (p_layer_number - 1));
231}
232
233void NavigationObstacle2D::set_avoidance_enabled(bool p_enabled) {
234 if (avoidance_enabled == p_enabled) {
235 return;
236 }
237
238 avoidance_enabled = p_enabled;
239 NavigationServer2D::get_singleton()->obstacle_set_avoidance_enabled(obstacle, avoidance_enabled);
240#ifdef DEBUG_ENABLED
241 queue_redraw();
242#endif // DEBUG_ENABLED
243}
244
245bool NavigationObstacle2D::get_avoidance_enabled() const {
246 return avoidance_enabled;
247}
248
249void NavigationObstacle2D::set_velocity(const Vector2 p_velocity) {
250 velocity = p_velocity;
251 velocity_submitted = true;
252}
253
254void NavigationObstacle2D::_update_map(RID p_map) {
255 map_current = p_map;
256 NavigationServer2D::get_singleton()->obstacle_set_map(obstacle, p_map);
257}
258
259void NavigationObstacle2D::_update_position(const Vector2 p_position) {
260 NavigationServer2D::get_singleton()->obstacle_set_position(obstacle, p_position);
261#ifdef DEBUG_ENABLED
262 queue_redraw();
263#endif // DEBUG_ENABLED
264}
265
266#ifdef DEBUG_ENABLED
267void NavigationObstacle2D::_update_fake_agent_radius_debug() {
268 if (radius > 0.0 && NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_radius()) {
269 Color debug_radius_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_obstacles_radius_color();
270 RS::get_singleton()->canvas_item_add_circle(get_canvas_item(), Vector2(), radius, debug_radius_color);
271 }
272}
273#endif // DEBUG_ENABLED
274
275#ifdef DEBUG_ENABLED
276void NavigationObstacle2D::_update_static_obstacle_debug() {
277 if (get_vertices().size() > 2 && NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_static()) {
278 bool obstacle_pushes_inward = Geometry2D::is_polygon_clockwise(get_vertices());
279
280 Color debug_static_obstacle_face_color;
281
282 if (obstacle_pushes_inward) {
283 debug_static_obstacle_face_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_face_color();
284 } else {
285 debug_static_obstacle_face_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_face_color();
286 }
287
288 Vector<Vector2> debug_obstacle_polygon_vertices = get_vertices();
289
290 Vector<Color> debug_obstacle_polygon_colors;
291 debug_obstacle_polygon_colors.resize(debug_obstacle_polygon_vertices.size());
292 debug_obstacle_polygon_colors.fill(debug_static_obstacle_face_color);
293
294 RS::get_singleton()->canvas_item_add_polygon(get_canvas_item(), debug_obstacle_polygon_vertices, debug_obstacle_polygon_colors);
295
296 Color debug_static_obstacle_edge_color;
297
298 if (obstacle_pushes_inward) {
299 debug_static_obstacle_edge_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_edge_color();
300 } else {
301 debug_static_obstacle_edge_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_edge_color();
302 }
303
304 Vector<Vector2> debug_obstacle_line_vertices = get_vertices();
305 debug_obstacle_line_vertices.push_back(debug_obstacle_line_vertices[0]);
306 debug_obstacle_line_vertices.resize(debug_obstacle_line_vertices.size());
307
308 Vector<Color> debug_obstacle_line_colors;
309 debug_obstacle_line_colors.resize(debug_obstacle_line_vertices.size());
310 debug_obstacle_line_colors.fill(debug_static_obstacle_edge_color);
311
312 RS::get_singleton()->canvas_item_add_polyline(get_canvas_item(), debug_obstacle_line_vertices, debug_obstacle_line_colors, 4.0);
313 }
314}
315#endif // DEBUG_ENABLED
316