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