1/**************************************************************************/
2/* nav_agent.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 "nav_agent.h"
32
33#include "nav_map.h"
34
35NavAgent::NavAgent() {
36}
37
38void NavAgent::set_avoidance_enabled(bool p_enabled) {
39 avoidance_enabled = p_enabled;
40 _update_rvo_agent_properties();
41}
42
43void NavAgent::set_use_3d_avoidance(bool p_enabled) {
44 use_3d_avoidance = p_enabled;
45 _update_rvo_agent_properties();
46}
47
48void NavAgent::_update_rvo_agent_properties() {
49 if (use_3d_avoidance) {
50 rvo_agent_3d.neighborDist_ = neighbor_distance;
51 rvo_agent_3d.maxNeighbors_ = max_neighbors;
52 rvo_agent_3d.timeHorizon_ = time_horizon_agents;
53 rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles;
54 rvo_agent_3d.radius_ = radius;
55 rvo_agent_3d.maxSpeed_ = max_speed;
56 rvo_agent_3d.position_ = RVO3D::Vector3(position.x, position.y, position.z);
57 // Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing.
58 //rvo_agent_3d.velocity_ = RVO3D::Vector3(velocity.x, velocity.y ,velocity.z);
59 rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z);
60 rvo_agent_3d.height_ = height;
61 rvo_agent_3d.avoidance_layers_ = avoidance_layers;
62 rvo_agent_3d.avoidance_mask_ = avoidance_mask;
63 rvo_agent_3d.avoidance_priority_ = avoidance_priority;
64 } else {
65 rvo_agent_2d.neighborDist_ = neighbor_distance;
66 rvo_agent_2d.maxNeighbors_ = max_neighbors;
67 rvo_agent_2d.timeHorizon_ = time_horizon_agents;
68 rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles;
69 rvo_agent_2d.radius_ = radius;
70 rvo_agent_2d.maxSpeed_ = max_speed;
71 rvo_agent_2d.position_ = RVO2D::Vector2(position.x, position.z);
72 rvo_agent_2d.elevation_ = position.y;
73 // Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing.
74 //rvo_agent_2d.velocity_ = RVO2D::Vector2(velocity.x, velocity.z);
75 rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z);
76 rvo_agent_2d.height_ = height;
77 rvo_agent_2d.avoidance_layers_ = avoidance_layers;
78 rvo_agent_2d.avoidance_mask_ = avoidance_mask;
79 rvo_agent_2d.avoidance_priority_ = avoidance_priority;
80 }
81
82 if (map != nullptr) {
83 if (avoidance_enabled) {
84 map->set_agent_as_controlled(this);
85 } else {
86 map->remove_agent_as_controlled(this);
87 }
88 }
89 agent_dirty = true;
90}
91
92void NavAgent::set_map(NavMap *p_map) {
93 if (map == p_map) {
94 return;
95 }
96
97 if (map) {
98 map->remove_agent(this);
99 }
100
101 map = p_map;
102 agent_dirty = true;
103
104 if (map) {
105 map->add_agent(this);
106 if (avoidance_enabled) {
107 map->set_agent_as_controlled(this);
108 }
109 }
110}
111
112bool NavAgent::is_map_changed() {
113 if (map) {
114 bool is_changed = map->get_map_update_id() != map_update_id;
115 map_update_id = map->get_map_update_id();
116 return is_changed;
117 } else {
118 return false;
119 }
120}
121
122void NavAgent::set_avoidance_callback(Callable p_callback) {
123 avoidance_callback = p_callback;
124}
125
126bool NavAgent::has_avoidance_callback() const {
127 return avoidance_callback.is_valid();
128}
129
130void NavAgent::dispatch_avoidance_callback() {
131 if (!avoidance_callback.is_valid()) {
132 return;
133 }
134
135 Vector3 new_velocity;
136
137 if (use_3d_avoidance) {
138 new_velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
139 } else {
140 new_velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y());
141 }
142
143 if (clamp_speed) {
144 new_velocity = new_velocity.limit_length(max_speed);
145 }
146
147 // Invoke the callback with the new velocity.
148 Variant args[] = { new_velocity };
149 const Variant *args_p[] = { &args[0] };
150 Variant return_value;
151 Callable::CallError call_error;
152
153 avoidance_callback.callp(args_p, 1, return_value, call_error);
154}
155
156void NavAgent::set_neighbor_distance(real_t p_neighbor_distance) {
157 neighbor_distance = p_neighbor_distance;
158 if (use_3d_avoidance) {
159 rvo_agent_3d.neighborDist_ = neighbor_distance;
160 } else {
161 rvo_agent_2d.neighborDist_ = neighbor_distance;
162 }
163 agent_dirty = true;
164}
165
166void NavAgent::set_max_neighbors(int p_max_neighbors) {
167 max_neighbors = p_max_neighbors;
168 if (use_3d_avoidance) {
169 rvo_agent_3d.maxNeighbors_ = max_neighbors;
170 } else {
171 rvo_agent_2d.maxNeighbors_ = max_neighbors;
172 }
173 agent_dirty = true;
174}
175
176void NavAgent::set_time_horizon_agents(real_t p_time_horizon) {
177 time_horizon_agents = p_time_horizon;
178 if (use_3d_avoidance) {
179 rvo_agent_3d.timeHorizon_ = time_horizon_agents;
180 } else {
181 rvo_agent_2d.timeHorizon_ = time_horizon_agents;
182 }
183 agent_dirty = true;
184}
185
186void NavAgent::set_time_horizon_obstacles(real_t p_time_horizon) {
187 time_horizon_obstacles = p_time_horizon;
188 if (use_3d_avoidance) {
189 rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles;
190 } else {
191 rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles;
192 }
193 agent_dirty = true;
194}
195
196void NavAgent::set_radius(real_t p_radius) {
197 radius = p_radius;
198 if (use_3d_avoidance) {
199 rvo_agent_3d.radius_ = radius;
200 } else {
201 rvo_agent_2d.radius_ = radius;
202 }
203 agent_dirty = true;
204}
205
206void NavAgent::set_height(real_t p_height) {
207 height = p_height;
208 if (use_3d_avoidance) {
209 rvo_agent_3d.height_ = height;
210 } else {
211 rvo_agent_2d.height_ = height;
212 }
213 agent_dirty = true;
214}
215
216void NavAgent::set_max_speed(real_t p_max_speed) {
217 max_speed = p_max_speed;
218 if (avoidance_enabled) {
219 if (use_3d_avoidance) {
220 rvo_agent_3d.maxSpeed_ = max_speed;
221 } else {
222 rvo_agent_2d.maxSpeed_ = max_speed;
223 }
224 }
225 agent_dirty = true;
226}
227
228void NavAgent::set_position(const Vector3 p_position) {
229 position = p_position;
230 if (avoidance_enabled) {
231 if (use_3d_avoidance) {
232 rvo_agent_3d.position_ = RVO3D::Vector3(p_position.x, p_position.y, p_position.z);
233 } else {
234 rvo_agent_2d.elevation_ = p_position.y;
235 rvo_agent_2d.position_ = RVO2D::Vector2(p_position.x, p_position.z);
236 }
237 }
238 agent_dirty = true;
239}
240
241void NavAgent::set_target_position(const Vector3 p_target_position) {
242 target_position = p_target_position;
243}
244
245void NavAgent::set_velocity(const Vector3 p_velocity) {
246 // Sets the "wanted" velocity for an agent as a suggestion
247 // This velocity is not guaranteed, RVO simulation will only try to fulfill it
248 velocity = p_velocity;
249 if (avoidance_enabled) {
250 if (use_3d_avoidance) {
251 rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z);
252 } else {
253 rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z);
254 }
255 }
256 agent_dirty = true;
257}
258
259void NavAgent::set_velocity_forced(const Vector3 p_velocity) {
260 // This function replaces the internal rvo simulation velocity
261 // should only be used after the agent was teleported
262 // as it destroys consistency in movement in cramped situations
263 // use velocity instead to update with a safer "suggestion"
264 velocity_forced = p_velocity;
265 if (avoidance_enabled) {
266 if (use_3d_avoidance) {
267 rvo_agent_3d.velocity_ = RVO3D::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
268 } else {
269 rvo_agent_2d.velocity_ = RVO2D::Vector2(p_velocity.x, p_velocity.z);
270 }
271 }
272 agent_dirty = true;
273}
274
275void NavAgent::update() {
276 // Updates this agent with the calculated results from the rvo simulation
277 if (avoidance_enabled) {
278 if (use_3d_avoidance) {
279 velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
280 } else {
281 velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y());
282 }
283 }
284}
285
286void NavAgent::set_avoidance_mask(uint32_t p_mask) {
287 avoidance_mask = p_mask;
288 if (use_3d_avoidance) {
289 rvo_agent_3d.avoidance_mask_ = avoidance_mask;
290 } else {
291 rvo_agent_2d.avoidance_mask_ = avoidance_mask;
292 }
293 agent_dirty = true;
294}
295
296void NavAgent::set_avoidance_layers(uint32_t p_layers) {
297 avoidance_layers = p_layers;
298 if (use_3d_avoidance) {
299 rvo_agent_3d.avoidance_layers_ = avoidance_layers;
300 } else {
301 rvo_agent_2d.avoidance_layers_ = avoidance_layers;
302 }
303 agent_dirty = true;
304}
305
306void NavAgent::set_avoidance_priority(real_t p_priority) {
307 ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
308 ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
309 avoidance_priority = p_priority;
310 if (use_3d_avoidance) {
311 rvo_agent_3d.avoidance_priority_ = avoidance_priority;
312 } else {
313 rvo_agent_2d.avoidance_priority_ = avoidance_priority;
314 }
315 agent_dirty = true;
316};
317
318bool NavAgent::check_dirty() {
319 const bool was_dirty = agent_dirty;
320 agent_dirty = false;
321 return was_dirty;
322}
323
324const Dictionary NavAgent::get_avoidance_data() const {
325 // Returns debug data from RVO simulation internals of this agent.
326 Dictionary _avoidance_data;
327 if (use_3d_avoidance) {
328 _avoidance_data["max_neighbors"] = int(rvo_agent_3d.maxNeighbors_);
329 _avoidance_data["max_speed"] = float(rvo_agent_3d.maxSpeed_);
330 _avoidance_data["neighbor_distance"] = float(rvo_agent_3d.neighborDist_);
331 _avoidance_data["new_velocity"] = Vector3(rvo_agent_3d.newVelocity_.x(), rvo_agent_3d.newVelocity_.y(), rvo_agent_3d.newVelocity_.z());
332 _avoidance_data["velocity"] = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
333 _avoidance_data["position"] = Vector3(rvo_agent_3d.position_.x(), rvo_agent_3d.position_.y(), rvo_agent_3d.position_.z());
334 _avoidance_data["prefered_velocity"] = Vector3(rvo_agent_3d.prefVelocity_.x(), rvo_agent_3d.prefVelocity_.y(), rvo_agent_3d.prefVelocity_.z());
335 _avoidance_data["radius"] = float(rvo_agent_3d.radius_);
336 _avoidance_data["time_horizon_agents"] = float(rvo_agent_3d.timeHorizon_);
337 _avoidance_data["time_horizon_obstacles"] = 0.0;
338 _avoidance_data["height"] = float(rvo_agent_3d.height_);
339 _avoidance_data["avoidance_layers"] = int(rvo_agent_3d.avoidance_layers_);
340 _avoidance_data["avoidance_mask"] = int(rvo_agent_3d.avoidance_mask_);
341 _avoidance_data["avoidance_priority"] = float(rvo_agent_3d.avoidance_priority_);
342 } else {
343 _avoidance_data["max_neighbors"] = int(rvo_agent_2d.maxNeighbors_);
344 _avoidance_data["max_speed"] = float(rvo_agent_2d.maxSpeed_);
345 _avoidance_data["neighbor_distance"] = float(rvo_agent_2d.neighborDist_);
346 _avoidance_data["new_velocity"] = Vector3(rvo_agent_2d.newVelocity_.x(), 0.0, rvo_agent_2d.newVelocity_.y());
347 _avoidance_data["velocity"] = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y());
348 _avoidance_data["position"] = Vector3(rvo_agent_2d.position_.x(), 0.0, rvo_agent_2d.position_.y());
349 _avoidance_data["prefered_velocity"] = Vector3(rvo_agent_2d.prefVelocity_.x(), 0.0, rvo_agent_2d.prefVelocity_.y());
350 _avoidance_data["radius"] = float(rvo_agent_2d.radius_);
351 _avoidance_data["time_horizon_agents"] = float(rvo_agent_2d.timeHorizon_);
352 _avoidance_data["time_horizon_obstacles"] = float(rvo_agent_2d.timeHorizonObst_);
353 _avoidance_data["height"] = float(rvo_agent_2d.height_);
354 _avoidance_data["avoidance_layers"] = int(rvo_agent_2d.avoidance_layers_);
355 _avoidance_data["avoidance_mask"] = int(rvo_agent_2d.avoidance_mask_);
356 _avoidance_data["avoidance_priority"] = float(rvo_agent_2d.avoidance_priority_);
357 }
358 return _avoidance_data;
359}
360
361void NavAgent::set_paused(bool p_paused) {
362 if (paused == p_paused) {
363 return;
364 }
365
366 paused = p_paused;
367
368 if (map) {
369 if (paused) {
370 map->remove_agent_as_controlled(this);
371 } else {
372 map->set_agent_as_controlled(this);
373 }
374 }
375}
376
377bool NavAgent::get_paused() const {
378 return paused;
379}
380