1/**************************************************************************/
2/* godot_step_3d.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 "godot_step_3d.h"
32
33#include "godot_joint_3d.h"
34
35#include "core/object/worker_thread_pool.h"
36#include "core/os/os.h"
37
38#define BODY_ISLAND_COUNT_RESERVE 128
39#define BODY_ISLAND_SIZE_RESERVE 512
40#define ISLAND_COUNT_RESERVE 128
41#define ISLAND_SIZE_RESERVE 512
42#define CONSTRAINT_COUNT_RESERVE 1024
43
44void GodotStep3D::_populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) {
45 p_body->set_island_step(_step);
46
47 if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
48 // Only rigid bodies are tested for activation.
49 p_body_island.push_back(p_body);
50 }
51
52 for (const KeyValue<GodotConstraint3D *, int> &E : p_body->get_constraint_map()) {
53 GodotConstraint3D *constraint = const_cast<GodotConstraint3D *>(E.key);
54 if (constraint->get_island_step() == _step) {
55 continue; // Already processed.
56 }
57 constraint->set_island_step(_step);
58 p_constraint_island.push_back(constraint);
59
60 all_constraints.push_back(constraint);
61
62 // Find connected rigid bodies.
63 for (int i = 0; i < constraint->get_body_count(); i++) {
64 if (i == E.value) {
65 continue;
66 }
67 GodotBody3D *other_body = constraint->get_body_ptr()[i];
68 if (other_body->get_island_step() == _step) {
69 continue; // Already processed.
70 }
71 if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) {
72 continue; // Static bodies don't connect islands.
73 }
74 _populate_island(other_body, p_body_island, p_constraint_island);
75 }
76
77 // Find connected soft bodies.
78 for (int i = 0; i < constraint->get_soft_body_count(); i++) {
79 GodotSoftBody3D *soft_body = constraint->get_soft_body_ptr(i);
80 if (soft_body->get_island_step() == _step) {
81 continue; // Already processed.
82 }
83 _populate_island_soft_body(soft_body, p_body_island, p_constraint_island);
84 }
85 }
86}
87
88void GodotStep3D::_populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) {
89 p_soft_body->set_island_step(_step);
90
91 for (const GodotConstraint3D *E : p_soft_body->get_constraints()) {
92 GodotConstraint3D *constraint = const_cast<GodotConstraint3D *>(E);
93 if (constraint->get_island_step() == _step) {
94 continue; // Already processed.
95 }
96 constraint->set_island_step(_step);
97 p_constraint_island.push_back(constraint);
98
99 all_constraints.push_back(constraint);
100
101 // Find connected rigid bodies.
102 for (int i = 0; i < constraint->get_body_count(); i++) {
103 GodotBody3D *body = constraint->get_body_ptr()[i];
104 if (body->get_island_step() == _step) {
105 continue; // Already processed.
106 }
107 if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) {
108 continue; // Static bodies don't connect islands.
109 }
110 _populate_island(body, p_body_island, p_constraint_island);
111 }
112 }
113}
114
115void GodotStep3D::_setup_constraint(uint32_t p_constraint_index, void *p_userdata) {
116 GodotConstraint3D *constraint = all_constraints[p_constraint_index];
117 constraint->setup(delta);
118}
119
120void GodotStep3D::_pre_solve_island(LocalVector<GodotConstraint3D *> &p_constraint_island) const {
121 uint32_t constraint_count = p_constraint_island.size();
122 uint32_t valid_constraint_count = 0;
123 for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
124 GodotConstraint3D *constraint = p_constraint_island[constraint_index];
125 if (p_constraint_island[constraint_index]->pre_solve(delta)) {
126 // Keep this constraint for solving.
127 p_constraint_island[valid_constraint_count++] = constraint;
128 }
129 }
130 p_constraint_island.resize(valid_constraint_count);
131}
132
133void GodotStep3D::_solve_island(uint32_t p_island_index, void *p_userdata) {
134 LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[p_island_index];
135
136 int current_priority = 1;
137
138 uint32_t constraint_count = constraint_island.size();
139 while (constraint_count > 0) {
140 for (int i = 0; i < iterations; i++) {
141 // Go through all iterations.
142 for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
143 constraint_island[constraint_index]->solve(delta);
144 }
145 }
146
147 // Check priority to keep only higher priority constraints.
148 uint32_t priority_constraint_count = 0;
149 ++current_priority;
150 for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
151 GodotConstraint3D *constraint = constraint_island[constraint_index];
152 if (constraint->get_priority() >= current_priority) {
153 // Keep this constraint for the next iteration.
154 constraint_island[priority_constraint_count++] = constraint;
155 }
156 }
157 constraint_count = priority_constraint_count;
158 }
159}
160
161void GodotStep3D::_check_suspend(const LocalVector<GodotBody3D *> &p_body_island) const {
162 bool can_sleep = true;
163
164 uint32_t body_count = p_body_island.size();
165 for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
166 GodotBody3D *body = p_body_island[body_index];
167
168 if (!body->sleep_test(delta)) {
169 can_sleep = false;
170 }
171 }
172
173 // Put all to sleep or wake up everyone.
174 for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
175 GodotBody3D *body = p_body_island[body_index];
176
177 bool active = body->is_active();
178
179 if (active == can_sleep) {
180 body->set_active(!can_sleep);
181 }
182 }
183}
184
185void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) {
186 p_space->lock(); // can't access space during this
187
188 p_space->setup(); //update inertias, etc
189
190 p_space->set_last_step(p_delta);
191
192 iterations = p_space->get_solver_iterations();
193 delta = p_delta;
194
195 const SelfList<GodotBody3D>::List *body_list = &p_space->get_active_body_list();
196
197 const SelfList<GodotSoftBody3D>::List *soft_body_list = &p_space->get_active_soft_body_list();
198
199 /* INTEGRATE FORCES */
200
201 uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec();
202 uint64_t profile_endtime = 0;
203
204 int active_count = 0;
205
206 const SelfList<GodotBody3D> *b = body_list->first();
207 while (b) {
208 b->self()->integrate_forces(p_delta);
209 b = b->next();
210 active_count++;
211 }
212
213 /* UPDATE SOFT BODY MOTION */
214
215 const SelfList<GodotSoftBody3D> *sb = soft_body_list->first();
216 while (sb) {
217 sb->self()->predict_motion(p_delta);
218 sb = sb->next();
219 active_count++;
220 }
221
222 p_space->set_active_objects(active_count);
223
224 // Update the broadphase to register collision pairs.
225 p_space->update();
226
227 { //profile
228 profile_endtime = OS::get_singleton()->get_ticks_usec();
229 p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
230 profile_begtime = profile_endtime;
231 }
232
233 /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */
234
235 uint32_t island_count = 0;
236
237 const SelfList<GodotArea3D>::List &aml = p_space->get_moved_area_list();
238
239 while (aml.first()) {
240 for (GodotConstraint3D *E : aml.first()->self()->get_constraints()) {
241 GodotConstraint3D *constraint = E;
242 if (constraint->get_island_step() == _step) {
243 continue;
244 }
245 constraint->set_island_step(_step);
246
247 // Each constraint can be on a separate island for areas as there's no solving phase.
248 ++island_count;
249 if (constraint_islands.size() < island_count) {
250 constraint_islands.resize(island_count);
251 }
252 LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1];
253 constraint_island.clear();
254
255 all_constraints.push_back(constraint);
256 constraint_island.push_back(constraint);
257 }
258 p_space->area_remove_from_moved_list((SelfList<GodotArea3D> *)aml.first()); //faster to remove here
259 }
260
261 /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
262
263 b = body_list->first();
264
265 uint32_t body_island_count = 0;
266
267 while (b) {
268 GodotBody3D *body = b->self();
269
270 if (body->get_island_step() != _step) {
271 ++body_island_count;
272 if (body_islands.size() < body_island_count) {
273 body_islands.resize(body_island_count);
274 }
275 LocalVector<GodotBody3D *> &body_island = body_islands[body_island_count - 1];
276 body_island.clear();
277 body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
278
279 ++island_count;
280 if (constraint_islands.size() < island_count) {
281 constraint_islands.resize(island_count);
282 }
283 LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1];
284 constraint_island.clear();
285 constraint_island.reserve(ISLAND_SIZE_RESERVE);
286
287 _populate_island(body, body_island, constraint_island);
288
289 if (body_island.is_empty()) {
290 --body_island_count;
291 }
292
293 if (constraint_island.is_empty()) {
294 --island_count;
295 }
296 }
297 b = b->next();
298 }
299
300 /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */
301
302 sb = soft_body_list->first();
303 while (sb) {
304 GodotSoftBody3D *soft_body = sb->self();
305
306 if (soft_body->get_island_step() != _step) {
307 ++body_island_count;
308 if (body_islands.size() < body_island_count) {
309 body_islands.resize(body_island_count);
310 }
311 LocalVector<GodotBody3D *> &body_island = body_islands[body_island_count - 1];
312 body_island.clear();
313 body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
314
315 ++island_count;
316 if (constraint_islands.size() < island_count) {
317 constraint_islands.resize(island_count);
318 }
319 LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1];
320 constraint_island.clear();
321 constraint_island.reserve(ISLAND_SIZE_RESERVE);
322
323 _populate_island_soft_body(soft_body, body_island, constraint_island);
324
325 if (body_island.is_empty()) {
326 --body_island_count;
327 }
328
329 if (constraint_island.is_empty()) {
330 --island_count;
331 }
332 }
333 sb = sb->next();
334 }
335
336 p_space->set_island_count((int)island_count);
337
338 { //profile
339 profile_endtime = OS::get_singleton()->get_ticks_usec();
340 p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
341 profile_begtime = profile_endtime;
342 }
343
344 /* SETUP CONSTRAINTS / PROCESS COLLISIONS */
345
346 uint32_t total_constraint_count = all_constraints.size();
347 WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep3D::_setup_constraint, nullptr, total_constraint_count, -1, true, SNAME("Physics3DConstraintSetup"));
348 WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
349
350 { //profile
351 profile_endtime = OS::get_singleton()->get_ticks_usec();
352 p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime);
353 profile_begtime = profile_endtime;
354 }
355
356 /* PRE-SOLVE CONSTRAINT ISLANDS */
357
358 // Warning: This doesn't run on threads, because it involves thread-unsafe processing.
359 for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
360 _pre_solve_island(constraint_islands[island_index]);
361 }
362
363 /* SOLVE CONSTRAINT ISLANDS */
364
365 // Warning: _solve_island modifies the constraint islands for optimization purpose,
366 // their content is not reliable after these calls and shouldn't be used anymore.
367 group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep3D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics3DConstraintSolveIslands"));
368 WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
369
370 { //profile
371 profile_endtime = OS::get_singleton()->get_ticks_usec();
372 p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
373 profile_begtime = profile_endtime;
374 }
375
376 /* INTEGRATE VELOCITIES */
377
378 b = body_list->first();
379 while (b) {
380 const SelfList<GodotBody3D> *n = b->next();
381 b->self()->integrate_velocities(p_delta);
382 b = n;
383 }
384
385 /* SLEEP / WAKE UP ISLANDS */
386
387 for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
388 _check_suspend(body_islands[island_index]);
389 }
390
391 /* UPDATE SOFT BODY CONSTRAINTS */
392
393 sb = soft_body_list->first();
394 while (sb) {
395 sb->self()->solve_constraints(p_delta);
396 sb = sb->next();
397 }
398
399 { //profile
400 profile_endtime = OS::get_singleton()->get_ticks_usec();
401 p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
402 profile_begtime = profile_endtime;
403 }
404
405 all_constraints.clear();
406
407 p_space->unlock();
408 _step++;
409}
410
411GodotStep3D::GodotStep3D() {
412 body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
413 constraint_islands.reserve(ISLAND_COUNT_RESERVE);
414 all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
415}
416
417GodotStep3D::~GodotStep3D() {
418}
419