1/**************************************************************************/
2/* godot_step_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 "godot_step_2d.h"
32
33#include "core/object/worker_thread_pool.h"
34#include "core/os/os.h"
35
36#define BODY_ISLAND_COUNT_RESERVE 128
37#define BODY_ISLAND_SIZE_RESERVE 512
38#define ISLAND_COUNT_RESERVE 128
39#define ISLAND_SIZE_RESERVE 512
40#define CONSTRAINT_COUNT_RESERVE 1024
41
42void GodotStep2D::_populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D *> &p_body_island, LocalVector<GodotConstraint2D *> &p_constraint_island) {
43 p_body->set_island_step(_step);
44
45 if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
46 // Only rigid bodies are tested for activation.
47 p_body_island.push_back(p_body);
48 }
49
50 for (const Pair<GodotConstraint2D *, int> &E : p_body->get_constraint_list()) {
51 GodotConstraint2D *constraint = const_cast<GodotConstraint2D *>(E.first);
52 if (constraint->get_island_step() == _step) {
53 continue; // Already processed.
54 }
55 constraint->set_island_step(_step);
56 p_constraint_island.push_back(constraint);
57 all_constraints.push_back(constraint);
58
59 for (int i = 0; i < constraint->get_body_count(); i++) {
60 if (i == E.second) {
61 continue;
62 }
63 GodotBody2D *other_body = constraint->get_body_ptr()[i];
64 if (other_body->get_island_step() == _step) {
65 continue; // Already processed.
66 }
67 if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) {
68 continue; // Static bodies don't connect islands.
69 }
70 _populate_island(other_body, p_body_island, p_constraint_island);
71 }
72 }
73}
74
75void GodotStep2D::_setup_constraint(uint32_t p_constraint_index, void *p_userdata) {
76 GodotConstraint2D *constraint = all_constraints[p_constraint_index];
77 constraint->setup(delta);
78}
79
80void GodotStep2D::_pre_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const {
81 uint32_t constraint_count = p_constraint_island.size();
82 uint32_t valid_constraint_count = 0;
83 for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
84 GodotConstraint2D *constraint = p_constraint_island[constraint_index];
85 if (p_constraint_island[constraint_index]->pre_solve(delta)) {
86 // Keep this constraint for solving.
87 p_constraint_island[valid_constraint_count++] = constraint;
88 }
89 }
90 p_constraint_island.resize(valid_constraint_count);
91}
92
93void GodotStep2D::_solve_island(uint32_t p_island_index, void *p_userdata) const {
94 const LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[p_island_index];
95
96 for (int i = 0; i < iterations; i++) {
97 uint32_t constraint_count = constraint_island.size();
98 for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
99 constraint_island[constraint_index]->solve(delta);
100 }
101 }
102}
103
104void GodotStep2D::_check_suspend(LocalVector<GodotBody2D *> &p_body_island) const {
105 bool can_sleep = true;
106
107 uint32_t body_count = p_body_island.size();
108 for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
109 GodotBody2D *body = p_body_island[body_index];
110
111 if (!body->sleep_test(delta)) {
112 can_sleep = false;
113 }
114 }
115
116 // Put all to sleep or wake up everyone.
117 for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
118 GodotBody2D *body = p_body_island[body_index];
119
120 bool active = body->is_active();
121
122 if (active == can_sleep) {
123 body->set_active(!can_sleep);
124 }
125 }
126}
127
128void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) {
129 p_space->lock(); // can't access space during this
130
131 p_space->setup(); //update inertias, etc
132
133 p_space->set_last_step(p_delta);
134
135 iterations = p_space->get_solver_iterations();
136 delta = p_delta;
137
138 const SelfList<GodotBody2D>::List *body_list = &p_space->get_active_body_list();
139
140 /* INTEGRATE FORCES */
141
142 uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec();
143 uint64_t profile_endtime = 0;
144
145 int active_count = 0;
146
147 const SelfList<GodotBody2D> *b = body_list->first();
148 while (b) {
149 b->self()->integrate_forces(p_delta);
150 b = b->next();
151 active_count++;
152 }
153
154 p_space->set_active_objects(active_count);
155
156 // Update the broadphase to register collision pairs.
157 p_space->update();
158
159 { //profile
160 profile_endtime = OS::get_singleton()->get_ticks_usec();
161 p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
162 profile_begtime = profile_endtime;
163 }
164
165 /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */
166
167 uint32_t island_count = 0;
168
169 const SelfList<GodotArea2D>::List &aml = p_space->get_moved_area_list();
170
171 while (aml.first()) {
172 for (GodotConstraint2D *E : aml.first()->self()->get_constraints()) {
173 GodotConstraint2D *constraint = E;
174 if (constraint->get_island_step() == _step) {
175 continue;
176 }
177 constraint->set_island_step(_step);
178
179 // Each constraint can be on a separate island for areas as there's no solving phase.
180 ++island_count;
181 if (constraint_islands.size() < island_count) {
182 constraint_islands.resize(island_count);
183 }
184 LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[island_count - 1];
185 constraint_island.clear();
186
187 all_constraints.push_back(constraint);
188 constraint_island.push_back(constraint);
189 }
190 p_space->area_remove_from_moved_list((SelfList<GodotArea2D> *)aml.first()); //faster to remove here
191 }
192
193 /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
194
195 b = body_list->first();
196
197 uint32_t body_island_count = 0;
198
199 while (b) {
200 GodotBody2D *body = b->self();
201
202 if (body->get_island_step() != _step) {
203 ++body_island_count;
204 if (body_islands.size() < body_island_count) {
205 body_islands.resize(body_island_count);
206 }
207 LocalVector<GodotBody2D *> &body_island = body_islands[body_island_count - 1];
208 body_island.clear();
209 body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
210
211 ++island_count;
212 if (constraint_islands.size() < island_count) {
213 constraint_islands.resize(island_count);
214 }
215 LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[island_count - 1];
216 constraint_island.clear();
217 constraint_island.reserve(ISLAND_SIZE_RESERVE);
218
219 _populate_island(body, body_island, constraint_island);
220
221 if (body_island.is_empty()) {
222 --body_island_count;
223 }
224
225 if (constraint_island.is_empty()) {
226 --island_count;
227 }
228 }
229 b = b->next();
230 }
231
232 p_space->set_island_count((int)island_count);
233
234 { //profile
235 profile_endtime = OS::get_singleton()->get_ticks_usec();
236 p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
237 profile_begtime = profile_endtime;
238 }
239
240 /* SETUP CONSTRAINTS / PROCESS COLLISIONS */
241
242 uint32_t total_constraint_count = all_constraints.size();
243 WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_setup_constraint, nullptr, total_constraint_count, -1, true, SNAME("Physics2DConstraintSetup"));
244 WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
245
246 { //profile
247 profile_endtime = OS::get_singleton()->get_ticks_usec();
248 p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime);
249 profile_begtime = profile_endtime;
250 }
251
252 /* PRE-SOLVE CONSTRAINT ISLANDS */
253
254 // Warning: This doesn't run on threads, because it involves thread-unsafe processing.
255 for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
256 _pre_solve_island(constraint_islands[island_index]);
257 }
258
259 /* SOLVE CONSTRAINT ISLANDS */
260
261 // Warning: _solve_island modifies the constraint islands for optimization purpose,
262 // their content is not reliable after these calls and shouldn't be used anymore.
263 group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics2DConstraintSolveIslands"));
264 WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
265
266 { //profile
267 profile_endtime = OS::get_singleton()->get_ticks_usec();
268 p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
269 profile_begtime = profile_endtime;
270 }
271
272 /* INTEGRATE VELOCITIES */
273
274 b = body_list->first();
275 while (b) {
276 const SelfList<GodotBody2D> *n = b->next();
277 b->self()->integrate_velocities(p_delta);
278 b = n; // in case it shuts itself down
279 }
280
281 /* SLEEP / WAKE UP ISLANDS */
282
283 for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
284 _check_suspend(body_islands[island_index]);
285 }
286
287 { //profile
288 profile_endtime = OS::get_singleton()->get_ticks_usec();
289 p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
290 //profile_begtime=profile_endtime;
291 }
292
293 all_constraints.clear();
294
295 p_space->unlock();
296 _step++;
297}
298
299GodotStep2D::GodotStep2D() {
300 body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
301 constraint_islands.reserve(ISLAND_COUNT_RESERVE);
302 all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
303}
304
305GodotStep2D::~GodotStep2D() {
306}
307