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