1/**************************************************************************/
2/* godot_space_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_space_2d.h"
32
33#include "godot_collision_solver_2d.h"
34#include "godot_physics_server_2d.h"
35
36#include "core/os/os.h"
37#include "core/templates/pair.h"
38
39#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001
40#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
41
42_FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject2D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
43 if (!(p_object->get_collision_layer() & p_collision_mask)) {
44 return false;
45 }
46
47 if (p_object->get_type() == GodotCollisionObject2D::TYPE_AREA && !p_collide_with_areas) {
48 return false;
49 }
50
51 if (p_object->get_type() == GodotCollisionObject2D::TYPE_BODY && !p_collide_with_bodies) {
52 return false;
53 }
54
55 return true;
56}
57
58int GodotPhysicsDirectSpaceState2D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
59 if (p_result_max <= 0) {
60 return 0;
61 }
62
63 Rect2 aabb;
64 aabb.position = p_parameters.position - Vector2(0.00001, 0.00001);
65 aabb.size = Vector2(0.00002, 0.00002);
66
67 int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
68
69 int cc = 0;
70
71 for (int i = 0; i < amount; i++) {
72 if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
73 continue;
74 }
75
76 if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
77 continue;
78 }
79
80 const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
81
82 if (p_parameters.pick_point && !col_obj->is_pickable()) {
83 continue;
84 }
85
86 if (col_obj->get_canvas_instance_id() != p_parameters.canvas_instance_id) {
87 continue;
88 }
89
90 int shape_idx = space->intersection_query_subindex_results[i];
91
92 GodotShape2D *shape = col_obj->get_shape(shape_idx);
93
94 Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_parameters.position);
95
96 if (!shape->contains_point(local_point)) {
97 continue;
98 }
99
100 if (cc >= p_result_max) {
101 continue;
102 }
103
104 r_results[cc].collider_id = col_obj->get_instance_id();
105 if (r_results[cc].collider_id.is_valid()) {
106 r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id);
107 }
108 r_results[cc].rid = col_obj->get_self();
109 r_results[cc].shape = shape_idx;
110
111 cc++;
112 }
113
114 return cc;
115}
116
117bool GodotPhysicsDirectSpaceState2D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) {
118 ERR_FAIL_COND_V(space->locked, false);
119
120 Vector2 begin, end;
121 Vector2 normal;
122 begin = p_parameters.from;
123 end = p_parameters.to;
124 normal = (end - begin).normalized();
125
126 int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
127
128 //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
129
130 bool collided = false;
131 Vector2 res_point, res_normal;
132 int res_shape = -1;
133 const GodotCollisionObject2D *res_obj = nullptr;
134 real_t min_d = 1e10;
135
136 for (int i = 0; i < amount; i++) {
137 if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
138 continue;
139 }
140
141 if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
142 continue;
143 }
144
145 const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
146
147 int shape_idx = space->intersection_query_subindex_results[i];
148 Transform2D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
149
150 Vector2 local_from = inv_xform.xform(begin);
151 Vector2 local_to = inv_xform.xform(end);
152
153 const GodotShape2D *shape = col_obj->get_shape(shape_idx);
154
155 Vector2 shape_point, shape_normal;
156
157 if (shape->contains_point(local_from)) {
158 if (p_parameters.hit_from_inside) {
159 // Hit shape at starting point.
160 min_d = 0;
161 res_point = begin;
162 res_normal = Vector2();
163 res_shape = shape_idx;
164 res_obj = col_obj;
165 collided = true;
166 break;
167 } else {
168 // Ignore shape when starting inside.
169 continue;
170 }
171 }
172
173 if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) {
174 Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
175 shape_point = xform.xform(shape_point);
176
177 real_t ld = normal.dot(shape_point);
178
179 if (ld < min_d) {
180 min_d = ld;
181 res_point = shape_point;
182 res_normal = inv_xform.basis_xform_inv(shape_normal).normalized();
183 res_shape = shape_idx;
184 res_obj = col_obj;
185 collided = true;
186 }
187 }
188 }
189
190 if (!collided) {
191 return false;
192 }
193 ERR_FAIL_NULL_V(res_obj, false); // Shouldn't happen but silences warning.
194
195 r_result.collider_id = res_obj->get_instance_id();
196 if (r_result.collider_id.is_valid()) {
197 r_result.collider = ObjectDB::get_instance(r_result.collider_id);
198 }
199 r_result.normal = res_normal;
200 r_result.position = res_point;
201 r_result.rid = res_obj->get_self();
202 r_result.shape = res_shape;
203
204 return true;
205}
206
207int GodotPhysicsDirectSpaceState2D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
208 if (p_result_max <= 0) {
209 return 0;
210 }
211
212 GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
213 ERR_FAIL_COND_V(!shape, 0);
214
215 Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
216 aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
217 aabb = aabb.grow(p_parameters.margin);
218
219 int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
220
221 int cc = 0;
222
223 for (int i = 0; i < amount; i++) {
224 if (cc >= p_result_max) {
225 break;
226 }
227
228 if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
229 continue;
230 }
231
232 if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
233 continue;
234 }
235
236 const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
237 int shape_idx = space->intersection_query_subindex_results[i];
238
239 if (!GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) {
240 continue;
241 }
242
243 r_results[cc].collider_id = col_obj->get_instance_id();
244 if (r_results[cc].collider_id.is_valid()) {
245 r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id);
246 }
247 r_results[cc].rid = col_obj->get_self();
248 r_results[cc].shape = shape_idx;
249
250 cc++;
251 }
252
253 return cc;
254}
255
256bool GodotPhysicsDirectSpaceState2D::cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe) {
257 GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
258 ERR_FAIL_COND_V(!shape, false);
259
260 Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
261 aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
262 aabb = aabb.grow(p_parameters.margin);
263
264 int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
265
266 real_t best_safe = 1;
267 real_t best_unsafe = 1;
268
269 for (int i = 0; i < amount; i++) {
270 if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
271 continue;
272 }
273
274 if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
275 continue; //ignore excluded
276 }
277
278 const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
279 int shape_idx = space->intersection_query_subindex_results[i];
280
281 Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
282 //test initial overlap, does it collide if going all the way?
283 if (!GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) {
284 continue;
285 }
286
287 //test initial overlap, ignore objects it's inside of.
288 if (GodotCollisionSolver2D::solve(shape, p_parameters.transform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) {
289 continue;
290 }
291
292 Vector2 mnormal = p_parameters.motion.normalized();
293
294 //just do kinematic solving
295 real_t low = 0.0;
296 real_t hi = 1.0;
297 real_t fraction_coeff = 0.5;
298 for (int j = 0; j < 8; j++) { //steps should be customizable..
299 real_t fraction = low + (hi - low) * fraction_coeff;
300
301 Vector2 sep = mnormal; //important optimization for this to work fast enough
302 bool collided = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_parameters.margin);
303
304 if (collided) {
305 hi = fraction;
306 if ((j == 0) || (low > 0.0)) { // Did it not collide before?
307 // When alternating or first iteration, use dichotomy.
308 fraction_coeff = 0.5;
309 } else {
310 // When colliding again, converge faster towards low fraction
311 // for more accurate results with long motions that collide near the start.
312 fraction_coeff = 0.25;
313 }
314 } else {
315 low = fraction;
316 if ((j == 0) || (hi < 1.0)) { // Did it collide before?
317 // When alternating or first iteration, use dichotomy.
318 fraction_coeff = 0.5;
319 } else {
320 // When not colliding again, converge faster towards high fraction
321 // for more accurate results with long motions that collide near the end.
322 fraction_coeff = 0.75;
323 }
324 }
325 }
326
327 if (low < best_safe) {
328 best_safe = low;
329 best_unsafe = hi;
330 }
331 }
332
333 p_closest_safe = best_safe;
334 p_closest_unsafe = best_unsafe;
335
336 return true;
337}
338
339bool GodotPhysicsDirectSpaceState2D::collide_shape(const ShapeParameters &p_parameters, Vector2 *r_results, int p_result_max, int &r_result_count) {
340 if (p_result_max <= 0) {
341 return false;
342 }
343
344 GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
345 ERR_FAIL_COND_V(!shape, 0);
346
347 Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
348 aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
349 aabb = aabb.grow(p_parameters.margin);
350
351 int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
352
353 bool collided = false;
354 r_result_count = 0;
355
356 GodotPhysicsServer2D::CollCbkData cbk;
357 cbk.max = p_result_max;
358 cbk.amount = 0;
359 cbk.passed = 0;
360 cbk.ptr = r_results;
361 GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk;
362
363 GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk;
364
365 for (int i = 0; i < amount; i++) {
366 if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
367 continue;
368 }
369
370 const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
371
372 if (p_parameters.exclude.has(col_obj->get_self())) {
373 continue;
374 }
375
376 int shape_idx = space->intersection_query_subindex_results[i];
377
378 cbk.valid_dir = Vector2();
379 cbk.valid_depth = 0;
380
381 if (GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) {
382 collided = cbk.amount > 0;
383 }
384 }
385
386 r_result_count = cbk.amount;
387
388 return collided;
389}
390
391struct _RestCallbackData2D {
392 const GodotCollisionObject2D *object = nullptr;
393 const GodotCollisionObject2D *best_object = nullptr;
394 int local_shape = 0;
395 int best_local_shape = 0;
396 int shape = 0;
397 int best_shape = 0;
398 Vector2 best_contact;
399 Vector2 best_normal;
400 real_t best_len = 0.0;
401 Vector2 valid_dir;
402 real_t valid_depth = 0.0;
403 real_t min_allowed_depth = 0.0;
404};
405
406static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
407 _RestCallbackData2D *rd = static_cast<_RestCallbackData2D *>(p_userdata);
408
409 Vector2 contact_rel = p_point_B - p_point_A;
410 real_t len = contact_rel.length();
411
412 if (len < rd->min_allowed_depth) {
413 return;
414 }
415
416 if (len <= rd->best_len) {
417 return;
418 }
419
420 Vector2 normal = contact_rel / len;
421
422 if (rd->valid_dir != Vector2()) {
423 if (len > rd->valid_depth) {
424 return;
425 }
426
427 if (rd->valid_dir.dot(normal) > -CMP_EPSILON) {
428 return;
429 }
430 }
431
432 rd->best_len = len;
433 rd->best_contact = p_point_B;
434 rd->best_normal = normal;
435 rd->best_object = rd->object;
436 rd->best_shape = rd->shape;
437 rd->best_local_shape = rd->local_shape;
438}
439
440bool GodotPhysicsDirectSpaceState2D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) {
441 GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
442 ERR_FAIL_COND_V(!shape, 0);
443
444 real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE);
445
446 Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
447 aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
448 aabb = aabb.grow(margin);
449
450 int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
451
452 _RestCallbackData2D rcd;
453
454 // Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
455 real_t motion_length = p_parameters.motion.length();
456 real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
457 rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
458
459 for (int i = 0; i < amount; i++) {
460 if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
461 continue;
462 }
463
464 const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
465
466 if (p_parameters.exclude.has(col_obj->get_self())) {
467 continue;
468 }
469
470 int shape_idx = space->intersection_query_subindex_results[i];
471
472 rcd.valid_dir = Vector2();
473 rcd.object = col_obj;
474 rcd.shape = shape_idx;
475 rcd.local_shape = 0;
476 bool sc = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, margin);
477 if (!sc) {
478 continue;
479 }
480 }
481
482 if (rcd.best_len == 0 || !rcd.best_object) {
483 return false;
484 }
485
486 r_info->collider_id = rcd.best_object->get_instance_id();
487 r_info->shape = rcd.best_shape;
488 r_info->normal = rcd.best_normal;
489 r_info->point = rcd.best_contact;
490 r_info->rid = rcd.best_object->get_self();
491 if (rcd.best_object->get_type() == GodotCollisionObject2D::TYPE_BODY) {
492 const GodotBody2D *body = static_cast<const GodotBody2D *>(rcd.best_object);
493 Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass());
494 r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
495
496 } else {
497 r_info->linear_velocity = Vector2();
498 }
499
500 return true;
501}
502
503////////////////////////////////////////////////////////////////////////////////////////////////////////////
504
505int GodotSpace2D::_cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb) {
506 int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results);
507
508 for (int i = 0; i < amount; i++) {
509 bool keep = true;
510
511 if (intersection_query_results[i] == p_body) {
512 keep = false;
513 } else if (intersection_query_results[i]->get_type() == GodotCollisionObject2D::TYPE_AREA) {
514 keep = false;
515 } else if (!p_body->collides_with(static_cast<GodotBody2D *>(intersection_query_results[i]))) {
516 keep = false;
517 } else if (static_cast<GodotBody2D *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
518 keep = false;
519 }
520
521 if (!keep) {
522 if (i < amount - 1) {
523 SWAP(intersection_query_results[i], intersection_query_results[amount - 1]);
524 SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]);
525 }
526
527 amount--;
528 i--;
529 }
530 }
531
532 return amount;
533}
534
535bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result) {
536 //give me back regular physics engine logic
537 //this is madness
538 //and most people using this function will think
539 //what it does is simpler than using physics
540 //this took about a week to get right..
541 //but is it right? who knows at this point..
542
543 if (r_result) {
544 r_result->collider_id = ObjectID();
545 r_result->collider_shape = 0;
546 }
547
548 Rect2 body_aabb;
549
550 bool shapes_found = false;
551
552 for (int i = 0; i < p_body->get_shape_count(); i++) {
553 if (p_body->is_shape_disabled(i)) {
554 continue;
555 }
556
557 if (!shapes_found) {
558 body_aabb = p_body->get_shape_aabb(i);
559 shapes_found = true;
560 } else {
561 body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
562 }
563 }
564
565 if (!shapes_found) {
566 if (r_result) {
567 *r_result = PhysicsServer2D::MotionResult();
568 r_result->travel = p_parameters.motion;
569 }
570 return false;
571 }
572
573 real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE);
574
575 // Undo the currently transform the physics server is aware of and apply the provided one
576 body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb));
577 body_aabb = body_aabb.grow(margin);
578
579 static const int max_excluded_shape_pairs = 32;
580 ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs];
581 int excluded_shape_pair_count = 0;
582
583 real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
584
585 real_t motion_length = p_parameters.motion.length();
586 Vector2 motion_normal = p_parameters.motion / motion_length;
587
588 Transform2D body_transform = p_parameters.from;
589
590 bool recovered = false;
591
592 {
593 //STEP 1, FREE BODY IF STUCK
594
595 const int max_results = 32;
596 int recover_attempts = 4;
597 Vector2 sr[max_results * 2];
598 real_t priorities[max_results];
599
600 do {
601 GodotPhysicsServer2D::CollCbkData cbk;
602 cbk.max = max_results;
603 cbk.amount = 0;
604 cbk.passed = 0;
605 cbk.ptr = sr;
606 cbk.invalid_by_dir = 0;
607 excluded_shape_pair_count = 0; //last step is the one valid
608
609 GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk;
610 GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk;
611 int priority_amount = 0;
612
613 bool collided = false;
614
615 int amount = _cull_aabb_for_body(p_body, body_aabb);
616
617 for (int j = 0; j < p_body->get_shape_count(); j++) {
618 if (p_body->is_shape_disabled(j)) {
619 continue;
620 }
621
622 GodotShape2D *body_shape = p_body->get_shape(j);
623 Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);
624
625 for (int i = 0; i < amount; i++) {
626 const GodotCollisionObject2D *col_obj = intersection_query_results[i];
627 if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
628 continue;
629 }
630 if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
631 continue;
632 }
633
634 int shape_idx = intersection_query_subindex_results[i];
635
636 Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
637
638 if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
639 cbk.valid_dir = col_obj_shape_xform.columns[1].normalized();
640
641 real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
642 cbk.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
643 cbk.invalid_by_dir = 0;
644
645 if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
646 const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj);
647 if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) {
648 //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
649 Vector2 lv = b->get_linear_velocity();
650 //compute displacement from linear velocity
651 Vector2 motion = lv * last_step;
652 real_t motion_len = motion.length();
653 motion.normalize();
654 cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0);
655 }
656 }
657 } else {
658 cbk.valid_dir = Vector2();
659 cbk.valid_depth = 0;
660 cbk.invalid_by_dir = 0;
661 }
662
663 int current_passed = cbk.passed; //save how many points passed collision
664 bool did_collide = false;
665
666 GodotShape2D *against_shape = col_obj->get_shape(shape_idx);
667 if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, margin)) {
668 did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
669 }
670 while (cbk.amount > priority_amount) {
671 priorities[priority_amount] = col_obj->get_collision_priority();
672 priority_amount++;
673 }
674
675 if (!did_collide && cbk.invalid_by_dir > 0) {
676 //this shape must be excluded
677 if (excluded_shape_pair_count < max_excluded_shape_pairs) {
678 ExcludedShapeSW esp;
679 esp.local_shape = body_shape;
680 esp.against_object = col_obj;
681 esp.against_shape_index = shape_idx;
682 excluded_shape_pairs[excluded_shape_pair_count++] = esp;
683 }
684 }
685
686 if (did_collide) {
687 collided = true;
688 }
689 }
690 }
691
692 if (!collided) {
693 break;
694 }
695
696 real_t inv_total_weight = 0.0;
697 for (int i = 0; i < cbk.amount; i++) {
698 inv_total_weight += priorities[i];
699 }
700 inv_total_weight = Math::is_zero_approx(inv_total_weight) ? 1.0 : (real_t)cbk.amount / inv_total_weight;
701
702 recovered = true;
703
704 Vector2 recover_motion;
705 for (int i = 0; i < cbk.amount; i++) {
706 Vector2 a = sr[i * 2 + 0];
707 Vector2 b = sr[i * 2 + 1];
708
709 // Compute plane on b towards a.
710 Vector2 n = (a - b).normalized();
711 real_t d = n.dot(b);
712
713 // Compute depth on recovered motion.
714 real_t depth = n.dot(a + recover_motion) - d;
715 if (depth > min_contact_depth + CMP_EPSILON) {
716 // Only recover if there is penetration.
717 recover_motion -= n * (depth - min_contact_depth) * 0.4 * priorities[i] * inv_total_weight;
718 }
719 }
720
721 if (recover_motion == Vector2()) {
722 collided = false;
723 break;
724 }
725
726 body_transform.columns[2] += recover_motion;
727 body_aabb.position += recover_motion;
728
729 recover_attempts--;
730
731 } while (recover_attempts);
732 }
733
734 real_t safe = 1.0;
735 real_t unsafe = 1.0;
736 int best_shape = -1;
737
738 {
739 // STEP 2 ATTEMPT MOTION
740
741 Rect2 motion_aabb = body_aabb;
742 motion_aabb.position += p_parameters.motion;
743 motion_aabb = motion_aabb.merge(body_aabb);
744
745 int amount = _cull_aabb_for_body(p_body, motion_aabb);
746
747 for (int body_shape_idx = 0; body_shape_idx < p_body->get_shape_count(); body_shape_idx++) {
748 if (p_body->is_shape_disabled(body_shape_idx)) {
749 continue;
750 }
751
752 GodotShape2D *body_shape = p_body->get_shape(body_shape_idx);
753
754 // Colliding separation rays allows to properly snap to the ground,
755 // otherwise it's not needed in regular motion.
756 if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) {
757 // When slide on slope is on, separation ray shape acts like a regular shape.
758 if (!static_cast<GodotSeparationRayShape2D *>(body_shape)->get_slide_on_slope()) {
759 continue;
760 }
761 }
762
763 Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx);
764
765 bool stuck = false;
766
767 real_t best_safe = 1;
768 real_t best_unsafe = 1;
769
770 for (int i = 0; i < amount; i++) {
771 const GodotCollisionObject2D *col_obj = intersection_query_results[i];
772 if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
773 continue;
774 }
775 if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
776 continue;
777 }
778
779 int col_shape_idx = intersection_query_subindex_results[i];
780 GodotShape2D *against_shape = col_obj->get_shape(col_shape_idx);
781
782 bool excluded = false;
783
784 for (int k = 0; k < excluded_shape_pair_count; k++) {
785 if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == col_shape_idx) {
786 excluded = true;
787 break;
788 }
789 }
790
791 if (excluded) {
792 continue;
793 }
794
795 Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx);
796 //test initial overlap, does it collide if going all the way?
797 if (!GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
798 continue;
799 }
800
801 //test initial overlap
802 if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
803 if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
804 Vector2 direction = col_obj_shape_xform.columns[1].normalized();
805 if (motion_normal.dot(direction) < 0) {
806 continue;
807 }
808 }
809
810 stuck = true;
811 break;
812 }
813
814 //just do kinematic solving
815 real_t low = 0.0;
816 real_t hi = 1.0;
817 real_t fraction_coeff = 0.5;
818 for (int k = 0; k < 8; k++) { //steps should be customizable..
819 real_t fraction = low + (hi - low) * fraction_coeff;
820
821 Vector2 sep = motion_normal; //important optimization for this to work fast enough
822 bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
823
824 if (collided) {
825 hi = fraction;
826 if ((k == 0) || (low > 0.0)) { // Did it not collide before?
827 // When alternating or first iteration, use dichotomy.
828 fraction_coeff = 0.5;
829 } else {
830 // When colliding again, converge faster towards low fraction
831 // for more accurate results with long motions that collide near the start.
832 fraction_coeff = 0.25;
833 }
834 } else {
835 low = fraction;
836 if ((k == 0) || (hi < 1.0)) { // Did it collide before?
837 // When alternating or first iteration, use dichotomy.
838 fraction_coeff = 0.5;
839 } else {
840 // When not colliding again, converge faster towards high fraction
841 // for more accurate results with long motions that collide near the end.
842 fraction_coeff = 0.75;
843 }
844 }
845 }
846
847 if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
848 Vector2 cd[2];
849 GodotPhysicsServer2D::CollCbkData cbk;
850 cbk.max = 1;
851 cbk.amount = 0;
852 cbk.passed = 0;
853 cbk.ptr = cd;
854 cbk.valid_dir = col_obj_shape_xform.columns[1].normalized();
855
856 cbk.valid_depth = 10e20;
857
858 Vector2 sep = motion_normal; //important optimization for this to work fast enough
859 bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), GodotPhysicsServer2D::_shape_col_cbk, &cbk, &sep, 0);
860 if (!collided || cbk.amount == 0) {
861 continue;
862 }
863 }
864
865 if (low < best_safe) {
866 best_safe = low;
867 best_unsafe = hi;
868 }
869 }
870
871 if (stuck) {
872 safe = 0;
873 unsafe = 0;
874 best_shape = body_shape_idx; //sadly it's the best
875 break;
876 }
877 if (best_safe == 1.0) {
878 continue;
879 }
880 if (best_safe < safe) {
881 safe = best_safe;
882 unsafe = best_unsafe;
883 best_shape = body_shape_idx;
884 }
885 }
886 }
887
888 bool collided = false;
889
890 if ((p_parameters.recovery_as_collision && recovered) || (safe < 1)) {
891 if (safe >= 1) {
892 best_shape = -1; //no best shape with cast, reset to -1
893 }
894
895 //it collided, let's get the rest info in unsafe advance
896 Transform2D ugt = body_transform;
897 ugt.columns[2] += p_parameters.motion * unsafe;
898
899 _RestCallbackData2D rcd;
900
901 // Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
902 rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
903
904 body_aabb.position += p_parameters.motion * unsafe;
905 int amount = _cull_aabb_for_body(p_body, body_aabb);
906
907 int from_shape = best_shape != -1 ? best_shape : 0;
908 int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
909
910 for (int j = from_shape; j < to_shape; j++) {
911 if (p_body->is_shape_disabled(j)) {
912 continue;
913 }
914
915 Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j);
916 GodotShape2D *body_shape = p_body->get_shape(j);
917
918 for (int i = 0; i < amount; i++) {
919 const GodotCollisionObject2D *col_obj = intersection_query_results[i];
920 if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
921 continue;
922 }
923 if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
924 continue;
925 }
926
927 int shape_idx = intersection_query_subindex_results[i];
928
929 GodotShape2D *against_shape = col_obj->get_shape(shape_idx);
930
931 bool excluded = false;
932 for (int k = 0; k < excluded_shape_pair_count; k++) {
933 if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == shape_idx) {
934 excluded = true;
935 break;
936 }
937 }
938 if (excluded) {
939 continue;
940 }
941
942 Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
943
944 if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
945 rcd.valid_dir = col_obj_shape_xform.columns[1].normalized();
946
947 real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
948 rcd.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
949
950 if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
951 const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj);
952 if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) {
953 //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
954 Vector2 lv = b->get_linear_velocity();
955 //compute displacement from linear velocity
956 Vector2 motion = lv * last_step;
957 real_t motion_len = motion.length();
958 motion.normalize();
959 rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0);
960 }
961 }
962 } else {
963 rcd.valid_dir = Vector2();
964 rcd.valid_depth = 0;
965 }
966
967 rcd.object = col_obj;
968 rcd.shape = shape_idx;
969 rcd.local_shape = j;
970 bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, margin);
971 if (!sc) {
972 continue;
973 }
974 }
975 }
976
977 if (rcd.best_len != 0) {
978 if (r_result) {
979 r_result->collider = rcd.best_object->get_self();
980 r_result->collider_id = rcd.best_object->get_instance_id();
981 r_result->collider_shape = rcd.best_shape;
982 r_result->collision_local_shape = rcd.best_local_shape;
983 r_result->collision_normal = rcd.best_normal;
984 r_result->collision_point = rcd.best_contact;
985 r_result->collision_depth = rcd.best_len;
986 r_result->collision_safe_fraction = safe;
987 r_result->collision_unsafe_fraction = unsafe;
988
989 const GodotBody2D *body = static_cast<const GodotBody2D *>(rcd.best_object);
990 Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass());
991 r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
992
993 r_result->travel = safe * p_parameters.motion;
994 r_result->remainder = p_parameters.motion - safe * p_parameters.motion;
995 r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin());
996 }
997
998 collided = true;
999 }
1000 }
1001
1002 if (!collided && r_result) {
1003 r_result->travel = p_parameters.motion;
1004 r_result->remainder = Vector2();
1005 r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin());
1006 }
1007
1008 return collided;
1009}
1010
1011// Assumes a valid collision pair, this should have been checked beforehand in the BVH or octree.
1012void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self) {
1013 GodotCollisionObject2D::Type type_A = A->get_type();
1014 GodotCollisionObject2D::Type type_B = B->get_type();
1015 if (type_A > type_B) {
1016 SWAP(A, B);
1017 SWAP(p_subindex_A, p_subindex_B);
1018 SWAP(type_A, type_B);
1019 }
1020
1021 GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self);
1022 self->collision_pairs++;
1023
1024 if (type_A == GodotCollisionObject2D::TYPE_AREA) {
1025 GodotArea2D *area = static_cast<GodotArea2D *>(A);
1026 if (type_B == GodotCollisionObject2D::TYPE_AREA) {
1027 GodotArea2D *area_b = static_cast<GodotArea2D *>(B);
1028 GodotArea2Pair2D *area2_pair = memnew(GodotArea2Pair2D(area_b, p_subindex_B, area, p_subindex_A));
1029 return area2_pair;
1030 } else {
1031 GodotBody2D *body = static_cast<GodotBody2D *>(B);
1032 GodotAreaPair2D *area_pair = memnew(GodotAreaPair2D(body, p_subindex_B, area, p_subindex_A));
1033 return area_pair;
1034 }
1035
1036 } else {
1037 GodotBodyPair2D *b = memnew(GodotBodyPair2D(static_cast<GodotBody2D *>(A), p_subindex_A, static_cast<GodotBody2D *>(B), p_subindex_B));
1038 return b;
1039 }
1040}
1041
1042void GodotSpace2D::_broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self) {
1043 if (!p_data) {
1044 return;
1045 }
1046
1047 GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self);
1048 self->collision_pairs--;
1049 GodotConstraint2D *c = static_cast<GodotConstraint2D *>(p_data);
1050 memdelete(c);
1051}
1052
1053const SelfList<GodotBody2D>::List &GodotSpace2D::get_active_body_list() const {
1054 return active_list;
1055}
1056
1057void GodotSpace2D::body_add_to_active_list(SelfList<GodotBody2D> *p_body) {
1058 active_list.add(p_body);
1059}
1060
1061void GodotSpace2D::body_remove_from_active_list(SelfList<GodotBody2D> *p_body) {
1062 active_list.remove(p_body);
1063}
1064
1065void GodotSpace2D::body_add_to_mass_properties_update_list(SelfList<GodotBody2D> *p_body) {
1066 mass_properties_update_list.add(p_body);
1067}
1068
1069void GodotSpace2D::body_remove_from_mass_properties_update_list(SelfList<GodotBody2D> *p_body) {
1070 mass_properties_update_list.remove(p_body);
1071}
1072
1073GodotBroadPhase2D *GodotSpace2D::get_broadphase() {
1074 return broadphase;
1075}
1076
1077void GodotSpace2D::add_object(GodotCollisionObject2D *p_object) {
1078 ERR_FAIL_COND(objects.has(p_object));
1079 objects.insert(p_object);
1080}
1081
1082void GodotSpace2D::remove_object(GodotCollisionObject2D *p_object) {
1083 ERR_FAIL_COND(!objects.has(p_object));
1084 objects.erase(p_object);
1085}
1086
1087const HashSet<GodotCollisionObject2D *> &GodotSpace2D::get_objects() const {
1088 return objects;
1089}
1090
1091void GodotSpace2D::body_add_to_state_query_list(SelfList<GodotBody2D> *p_body) {
1092 state_query_list.add(p_body);
1093}
1094
1095void GodotSpace2D::body_remove_from_state_query_list(SelfList<GodotBody2D> *p_body) {
1096 state_query_list.remove(p_body);
1097}
1098
1099void GodotSpace2D::area_add_to_monitor_query_list(SelfList<GodotArea2D> *p_area) {
1100 monitor_query_list.add(p_area);
1101}
1102
1103void GodotSpace2D::area_remove_from_monitor_query_list(SelfList<GodotArea2D> *p_area) {
1104 monitor_query_list.remove(p_area);
1105}
1106
1107void GodotSpace2D::area_add_to_moved_list(SelfList<GodotArea2D> *p_area) {
1108 area_moved_list.add(p_area);
1109}
1110
1111void GodotSpace2D::area_remove_from_moved_list(SelfList<GodotArea2D> *p_area) {
1112 area_moved_list.remove(p_area);
1113}
1114
1115const SelfList<GodotArea2D>::List &GodotSpace2D::get_moved_area_list() const {
1116 return area_moved_list;
1117}
1118
1119void GodotSpace2D::call_queries() {
1120 while (state_query_list.first()) {
1121 GodotBody2D *b = state_query_list.first()->self();
1122 state_query_list.remove(state_query_list.first());
1123 b->call_queries();
1124 }
1125
1126 while (monitor_query_list.first()) {
1127 GodotArea2D *a = monitor_query_list.first()->self();
1128 monitor_query_list.remove(monitor_query_list.first());
1129 a->call_queries();
1130 }
1131}
1132
1133void GodotSpace2D::setup() {
1134 contact_debug_count = 0;
1135
1136 while (mass_properties_update_list.first()) {
1137 mass_properties_update_list.first()->self()->update_mass_properties();
1138 mass_properties_update_list.remove(mass_properties_update_list.first());
1139 }
1140}
1141
1142void GodotSpace2D::update() {
1143 broadphase->update();
1144}
1145
1146void GodotSpace2D::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value) {
1147 switch (p_param) {
1148 case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
1149 contact_recycle_radius = p_value;
1150 break;
1151 case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION:
1152 contact_max_separation = p_value;
1153 break;
1154 case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION:
1155 contact_max_allowed_penetration = p_value;
1156 break;
1157 case PhysicsServer2D::SPACE_PARAM_CONTACT_DEFAULT_BIAS:
1158 contact_bias = p_value;
1159 break;
1160 case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
1161 body_linear_velocity_sleep_threshold = p_value;
1162 break;
1163 case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
1164 body_angular_velocity_sleep_threshold = p_value;
1165 break;
1166 case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP:
1167 body_time_to_sleep = p_value;
1168 break;
1169 case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
1170 constraint_bias = p_value;
1171 break;
1172 case PhysicsServer2D::SPACE_PARAM_SOLVER_ITERATIONS:
1173 solver_iterations = p_value;
1174 break;
1175 }
1176}
1177
1178real_t GodotSpace2D::get_param(PhysicsServer2D::SpaceParameter p_param) const {
1179 switch (p_param) {
1180 case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
1181 return contact_recycle_radius;
1182 case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION:
1183 return contact_max_separation;
1184 case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION:
1185 return contact_max_allowed_penetration;
1186 case PhysicsServer2D::SPACE_PARAM_CONTACT_DEFAULT_BIAS:
1187 return contact_bias;
1188 case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
1189 return body_linear_velocity_sleep_threshold;
1190 case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
1191 return body_angular_velocity_sleep_threshold;
1192 case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP:
1193 return body_time_to_sleep;
1194 case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
1195 return constraint_bias;
1196 case PhysicsServer2D::SPACE_PARAM_SOLVER_ITERATIONS:
1197 return solver_iterations;
1198 }
1199 return 0;
1200}
1201
1202void GodotSpace2D::lock() {
1203 locked = true;
1204}
1205
1206void GodotSpace2D::unlock() {
1207 locked = false;
1208}
1209
1210bool GodotSpace2D::is_locked() const {
1211 return locked;
1212}
1213
1214GodotPhysicsDirectSpaceState2D *GodotSpace2D::get_direct_state() {
1215 return direct_access;
1216}
1217
1218GodotSpace2D::GodotSpace2D() {
1219 body_linear_velocity_sleep_threshold = GLOBAL_GET("physics/2d/sleep_threshold_linear");
1220 body_angular_velocity_sleep_threshold = GLOBAL_GET("physics/2d/sleep_threshold_angular");
1221 body_time_to_sleep = GLOBAL_GET("physics/2d/time_before_sleep");
1222 solver_iterations = GLOBAL_GET("physics/2d/solver/solver_iterations");
1223 contact_recycle_radius = GLOBAL_GET("physics/2d/solver/contact_recycle_radius");
1224 contact_max_separation = GLOBAL_GET("physics/2d/solver/contact_max_separation");
1225 contact_max_allowed_penetration = GLOBAL_GET("physics/2d/solver/contact_max_allowed_penetration");
1226 contact_bias = GLOBAL_GET("physics/2d/solver/default_contact_bias");
1227 constraint_bias = GLOBAL_GET("physics/2d/solver/default_constraint_bias");
1228
1229 broadphase = GodotBroadPhase2D::create_func();
1230 broadphase->set_pair_callback(_broadphase_pair, this);
1231 broadphase->set_unpair_callback(_broadphase_unpair, this);
1232
1233 direct_access = memnew(GodotPhysicsDirectSpaceState2D);
1234 direct_access->space = this;
1235}
1236
1237GodotSpace2D::~GodotSpace2D() {
1238 memdelete(broadphase);
1239 memdelete(direct_access);
1240}
1241