2014-02-10 02:10:30 +01:00
/*************************************************************************/
/* space_2d_sw.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
2017-08-27 14:16:55 +02:00
/* https://godotengine.org */
2014-02-10 02:10:30 +01:00
/*************************************************************************/
2020-01-01 11:16:22 +01:00
/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */
2014-02-10 02:10:30 +01:00
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
2018-01-05 00:50:27 +01:00
2014-02-19 15:57:14 +01:00
# include "space_2d_sw.h"
2017-08-27 21:07:15 +02:00
2014-02-19 15:57:14 +01:00
# include "collision_solver_2d_sw.h"
2018-11-02 19:44:29 +01:00
# include "core/os/os.h"
2018-09-11 18:13:45 +02:00
# include "core/pair.h"
2014-02-19 15:57:14 +01:00
# include "physics_2d_server_sw.h"
2020-07-27 22:46:04 +02:00
_FORCE_INLINE_ static bool _can_collide_with ( CollisionObject2DSW * p_object , uint32_t p_collision_mask , bool p_collide_with_bodies , bool p_collide_with_areas ) {
2014-02-23 00:28:19 +01:00
2020-07-27 22:46:04 +02:00
if ( ! ( p_object - > get_collision_layer ( ) & p_collision_mask ) ) {
2018-08-21 20:30:41 +02:00
return false ;
}
if ( p_object - > get_type ( ) = = CollisionObject2DSW : : TYPE_AREA & & ! p_collide_with_areas )
return false ;
if ( p_object - > get_type ( ) = = CollisionObject2DSW : : TYPE_BODY & & ! p_collide_with_bodies )
return false ;
return true ;
2014-02-23 00:28:19 +01:00
}
2018-08-25 00:03:26 +02:00
int Physics2DDirectSpaceStateSW : : _intersect_point_impl ( const Vector2 & p_point , ShapeResult * r_results , int p_result_max , const Set < RID > & p_exclude , uint32_t p_collision_mask , bool p_collide_with_bodies , bool p_collide_with_areas , bool p_pick_point , bool p_filter_by_canvas , ObjectID p_canvas_instance_id ) {
2015-03-22 05:46:18 +01:00
2017-03-05 16:44:50 +01:00
if ( p_result_max < = 0 )
2015-03-22 05:46:18 +01:00
return 0 ;
Rect2 aabb ;
2017-06-04 00:25:13 +02:00
aabb . position = p_point - Vector2 ( 0.00001 , 0.00001 ) ;
2017-03-05 16:44:50 +01:00
aabb . size = Vector2 ( 0.00002 , 0.00002 ) ;
2015-03-22 05:46:18 +01:00
2017-03-05 16:44:50 +01:00
int amount = space - > broadphase - > cull_aabb ( aabb , space - > intersection_query_results , Space2DSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
2015-03-22 05:46:18 +01:00
2017-03-05 16:44:50 +01:00
int cc = 0 ;
2015-03-22 05:46:18 +01:00
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < amount ; i + + ) {
2020-07-27 22:46:04 +02:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2015-03-22 05:46:18 +01:00
continue ;
2017-03-05 16:44:50 +01:00
if ( p_exclude . has ( space - > intersection_query_results [ i ] - > get_self ( ) ) )
2015-03-22 05:46:18 +01:00
continue ;
2017-03-05 16:44:50 +01:00
const CollisionObject2DSW * col_obj = space - > intersection_query_results [ i ] ;
2015-03-22 05:46:18 +01:00
2016-02-01 02:30:16 +01:00
if ( p_pick_point & & ! col_obj - > is_pickable ( ) )
2015-03-22 05:46:18 +01:00
continue ;
2018-08-25 00:03:26 +02:00
if ( p_filter_by_canvas & & col_obj - > get_canvas_instance_id ( ) ! = p_canvas_instance_id )
continue ;
2017-03-05 16:44:50 +01:00
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
2015-03-22 05:46:18 +01:00
2017-03-05 16:44:50 +01:00
Shape2DSW * shape = col_obj - > get_shape ( shape_idx ) ;
2015-03-22 05:46:18 +01:00
Vector2 local_point = ( col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) ) . affine_inverse ( ) . xform ( p_point ) ;
if ( ! shape - > contains_point ( local_point ) )
continue ;
2017-03-05 16:44:50 +01:00
if ( cc > = p_result_max )
2016-09-27 04:40:06 +02:00
continue ;
2017-03-05 16:44:50 +01:00
r_results [ cc ] . collider_id = col_obj - > get_instance_id ( ) ;
if ( r_results [ cc ] . collider_id ! = 0 )
r_results [ cc ] . collider = ObjectDB : : get_instance ( r_results [ cc ] . collider_id ) ;
r_results [ cc ] . rid = col_obj - > get_self ( ) ;
r_results [ cc ] . shape = shape_idx ;
r_results [ cc ] . metadata = col_obj - > get_shape_metadata ( shape_idx ) ;
2015-03-22 05:46:18 +01:00
cc + + ;
}
return cc ;
}
2018-08-25 00:03:26 +02:00
int Physics2DDirectSpaceStateSW : : intersect_point ( const Vector2 & p_point , ShapeResult * r_results , int p_result_max , const Set < RID > & p_exclude , uint32_t p_collision_mask , bool p_collide_with_bodies , bool p_collide_with_areas , bool p_pick_point ) {
return _intersect_point_impl ( p_point , r_results , p_result_max , p_exclude , p_collision_mask , p_collide_with_bodies , p_collide_with_areas , p_pick_point ) ;
}
int Physics2DDirectSpaceStateSW : : intersect_point_on_canvas ( const Vector2 & p_point , ObjectID p_canvas_instance_id , ShapeResult * r_results , int p_result_max , const Set < RID > & p_exclude , uint32_t p_collision_mask , bool p_collide_with_bodies , bool p_collide_with_areas , bool p_pick_point ) {
return _intersect_point_impl ( p_point , r_results , p_result_max , p_exclude , p_collision_mask , p_collide_with_bodies , p_collide_with_areas , p_pick_point , true , p_canvas_instance_id ) ;
}
2018-08-21 20:30:41 +02:00
bool Physics2DDirectSpaceStateSW : : intersect_ray ( const Vector2 & p_from , const Vector2 & p_to , RayResult & r_result , const Set < RID > & p_exclude , uint32_t p_collision_mask , bool p_collide_with_bodies , bool p_collide_with_areas ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
ERR_FAIL_COND_V ( space - > locked , false ) ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
Vector2 begin , end ;
2014-02-19 15:57:14 +01:00
Vector2 normal ;
2017-03-05 16:44:50 +01:00
begin = p_from ;
end = p_to ;
normal = ( end - begin ) . normalized ( ) ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
int amount = space - > broadphase - > cull_segment ( begin , end , space - > intersection_query_results , Space2DSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
2014-02-19 15:57:14 +01:00
2018-09-13 03:38:39 +02:00
//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
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
bool collided = false ;
Vector2 res_point , res_normal ;
2014-02-19 15:57:14 +01:00
int res_shape ;
const CollisionObject2DSW * res_obj ;
2017-03-05 16:44:50 +01:00
real_t min_d = 1e10 ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < amount ; i + + ) {
2014-02-19 15:57:14 +01:00
2018-08-21 20:30:41 +02:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2014-02-23 00:28:19 +01:00
continue ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
if ( p_exclude . has ( space - > intersection_query_results [ i ] - > get_self ( ) ) )
2014-02-19 15:57:14 +01:00
continue ;
2017-03-05 16:44:50 +01:00
const CollisionObject2DSW * col_obj = space - > intersection_query_results [ i ] ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
2017-01-11 04:52:51 +01:00
Transform2D inv_xform = col_obj - > get_shape_inv_transform ( shape_idx ) * col_obj - > get_inv_transform ( ) ;
2014-02-19 15:57:14 +01:00
Vector2 local_from = inv_xform . xform ( begin ) ;
Vector2 local_to = inv_xform . xform ( end ) ;
/*local_from = col_obj->get_inv_transform().xform(begin);
local_from = col_obj - > get_shape_inv_transform ( shape_idx ) . xform ( local_from ) ;
local_to = col_obj - > get_inv_transform ( ) . xform ( end ) ;
local_to = col_obj - > get_shape_inv_transform ( shape_idx ) . xform ( local_to ) ; */
const Shape2DSW * shape = col_obj - > get_shape ( shape_idx ) ;
2017-03-05 16:44:50 +01:00
Vector2 shape_point , shape_normal ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
if ( shape - > intersect_segment ( local_from , local_to , shape_point , shape_normal ) ) {
2015-01-14 01:19:11 +01:00
2017-01-11 04:52:51 +01:00
Transform2D xform = col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) ;
2017-03-05 16:44:50 +01:00
shape_point = xform . xform ( shape_point ) ;
2014-02-19 15:57:14 +01:00
real_t ld = normal . dot ( shape_point ) ;
2017-03-05 16:44:50 +01:00
if ( ld < min_d ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
min_d = ld ;
res_point = shape_point ;
res_normal = inv_xform . basis_xform_inv ( shape_normal ) . normalized ( ) ;
res_shape = shape_idx ;
res_obj = col_obj ;
collided = true ;
2014-02-19 15:57:14 +01:00
}
}
}
if ( ! collided )
return false ;
2017-03-05 16:44:50 +01:00
r_result . collider_id = res_obj - > get_instance_id ( ) ;
if ( r_result . collider_id ! = 0 )
r_result . collider = ObjectDB : : get_instance ( r_result . collider_id ) ;
r_result . normal = res_normal ;
r_result . metadata = res_obj - > get_shape_metadata ( res_shape ) ;
r_result . position = res_point ;
r_result . rid = res_obj - > get_self ( ) ;
r_result . shape = res_shape ;
2014-02-19 15:57:14 +01:00
return true ;
}
2018-08-21 20:30:41 +02:00
int Physics2DDirectSpaceStateSW : : intersect_shape ( const RID & p_shape , const Transform2D & p_xform , const Vector2 & p_motion , real_t p_margin , ShapeResult * r_results , int p_result_max , const Set < RID > & p_exclude , uint32_t p_collision_mask , bool p_collide_with_bodies , bool p_collide_with_areas ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
if ( p_result_max < = 0 )
2014-02-19 15:57:14 +01:00
return 0 ;
2016-01-04 04:00:33 +01:00
Shape2DSW * shape = Physics2DServerSW : : singletonsw - > shape_owner . get ( p_shape ) ;
2017-03-05 16:44:50 +01:00
ERR_FAIL_COND_V ( ! shape , 0 ) ;
2014-02-19 15:57:14 +01:00
Rect2 aabb = p_xform . xform ( shape - > get_aabb ( ) ) ;
2017-03-05 16:44:50 +01:00
aabb = aabb . grow ( p_margin ) ;
2014-02-19 15:57:14 +01:00
2018-04-22 10:16:55 +02:00
int amount = space - > broadphase - > cull_aabb ( aabb , space - > intersection_query_results , Space2DSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
int cc = 0 ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < amount ; i + + ) {
2014-02-19 15:57:14 +01:00
2018-04-22 10:16:55 +02:00
if ( cc > = p_result_max )
break ;
2018-08-21 20:30:41 +02:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2014-02-23 00:28:19 +01:00
continue ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
if ( p_exclude . has ( space - > intersection_query_results [ i ] - > get_self ( ) ) )
2014-02-19 15:57:14 +01:00
continue ;
2017-03-05 16:44:50 +01:00
const CollisionObject2DSW * col_obj = space - > intersection_query_results [ i ] ;
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
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 ) )
2014-02-19 15:57:14 +01:00
continue ;
2017-03-05 16:44:50 +01:00
r_results [ cc ] . collider_id = col_obj - > get_instance_id ( ) ;
if ( r_results [ cc ] . collider_id ! = 0 )
r_results [ cc ] . collider = ObjectDB : : get_instance ( r_results [ cc ] . collider_id ) ;
r_results [ cc ] . rid = col_obj - > get_self ( ) ;
r_results [ cc ] . shape = shape_idx ;
r_results [ cc ] . metadata = col_obj - > get_shape_metadata ( shape_idx ) ;
2014-02-19 15:57:14 +01:00
cc + + ;
}
return cc ;
}
2018-08-21 20:30:41 +02:00
bool Physics2DDirectSpaceStateSW : : cast_motion ( const RID & p_shape , const Transform2D & p_xform , const Vector2 & p_motion , real_t p_margin , real_t & p_closest_safe , real_t & p_closest_unsafe , const Set < RID > & p_exclude , uint32_t p_collision_mask , bool p_collide_with_bodies , bool p_collide_with_areas ) {
2014-02-19 15:57:14 +01:00
2016-01-04 04:00:33 +01:00
Shape2DSW * shape = Physics2DServerSW : : singletonsw - > shape_owner . get ( p_shape ) ;
2017-03-05 16:44:50 +01:00
ERR_FAIL_COND_V ( ! shape , false ) ;
2014-02-23 00:28:19 +01:00
Rect2 aabb = p_xform . xform ( shape - > get_aabb ( ) ) ;
2017-06-04 00:25:13 +02:00
aabb = aabb . merge ( Rect2 ( aabb . position + p_motion , aabb . size ) ) ; //motion
2017-03-05 16:44:50 +01:00
aabb = aabb . grow ( p_margin ) ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
int amount = space - > broadphase - > cull_aabb ( aabb , space - > intersection_query_results , Space2DSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
2014-02-23 00:28:19 +01:00
2017-03-05 16:44:50 +01:00
real_t best_safe = 1 ;
real_t best_unsafe = 1 ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < amount ; i + + ) {
2014-02-19 15:57:14 +01:00
2018-08-21 20:30:41 +02:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2014-02-23 00:28:19 +01:00
continue ;
2017-03-05 16:44:50 +01:00
if ( p_exclude . has ( space - > intersection_query_results [ i ] - > get_self ( ) ) )
2014-02-23 00:28:19 +01:00
continue ; //ignore excluded
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
const CollisionObject2DSW * col_obj = space - > intersection_query_results [ i ] ;
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
2014-02-23 00:28:19 +01:00
2017-01-11 04:52:51 +01:00
Transform2D col_obj_xform = col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) ;
2014-02-23 00:28:19 +01:00
//test initial overlap, does it collide if going all the way?
2017-03-05 16:44:50 +01:00
if ( ! CollisionSolver2DSW : : solve ( shape , p_xform , p_motion , col_obj - > get_shape ( shape_idx ) , col_obj_xform , Vector2 ( ) , NULL , NULL , NULL , p_margin ) ) {
2014-02-23 00:28:19 +01:00
continue ;
2014-02-19 15:57:14 +01:00
}
2020-11-27 15:35:10 +01:00
//test initial overlap, ignore objects it's inside of.
2017-03-05 16:44:50 +01:00
if ( CollisionSolver2DSW : : solve ( shape , p_xform , Vector2 ( ) , col_obj - > get_shape ( shape_idx ) , col_obj_xform , Vector2 ( ) , NULL , NULL , NULL , p_margin ) ) {
2014-02-23 00:28:19 +01:00
2020-11-27 15:35:10 +01:00
continue ;
2014-02-23 00:28:19 +01:00
}
//just do kinematic solving
2017-03-05 16:44:50 +01:00
real_t low = 0 ;
real_t hi = 1 ;
Vector2 mnormal = p_motion . normalized ( ) ;
2014-02-23 00:28:19 +01:00
2019-02-12 21:10:08 +01:00
for ( int j = 0 ; j < 8 ; j + + ) { //steps should be customizable..
2014-02-23 00:28:19 +01:00
2017-03-05 16:44:50 +01:00
real_t ofs = ( low + hi ) * 0.5 ;
2014-02-23 00:28:19 +01:00
2017-03-05 16:44:50 +01:00
Vector2 sep = mnormal ; //important optimization for this to work fast enough
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 ) ;
2014-02-23 00:28:19 +01:00
if ( collided ) {
2017-03-05 16:44:50 +01:00
hi = ofs ;
2014-02-23 00:28:19 +01:00
} else {
2017-03-05 16:44:50 +01:00
low = ofs ;
2014-02-23 00:28:19 +01:00
}
}
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
if ( low < best_safe ) {
best_safe = low ;
best_unsafe = hi ;
2014-02-23 00:28:19 +01:00
}
2014-02-19 15:57:14 +01:00
}
2014-02-23 00:28:19 +01:00
2017-03-05 16:44:50 +01:00
p_closest_safe = best_safe ;
p_closest_unsafe = best_unsafe ;
2014-02-23 00:28:19 +01:00
return true ;
2014-02-19 15:57:14 +01:00
}
2018-08-21 20:30:41 +02:00
bool Physics2DDirectSpaceStateSW : : collide_shape ( RID p_shape , const Transform2D & p_shape_xform , const Vector2 & p_motion , real_t p_margin , Vector2 * r_results , int p_result_max , int & r_result_count , const Set < RID > & p_exclude , uint32_t p_collision_mask , bool p_collide_with_bodies , bool p_collide_with_areas ) {
2014-02-23 00:28:19 +01:00
2017-03-05 16:44:50 +01:00
if ( p_result_max < = 0 )
2014-02-23 00:28:19 +01:00
return 0 ;
2014-02-19 15:57:14 +01:00
2016-01-04 04:00:33 +01:00
Shape2DSW * shape = Physics2DServerSW : : singletonsw - > shape_owner . get ( p_shape ) ;
2017-03-05 16:44:50 +01:00
ERR_FAIL_COND_V ( ! shape , 0 ) ;
2014-02-19 15:57:14 +01:00
2014-02-23 00:28:19 +01:00
Rect2 aabb = p_shape_xform . xform ( shape - > get_aabb ( ) ) ;
2017-06-04 00:25:13 +02:00
aabb = aabb . merge ( Rect2 ( aabb . position + p_motion , aabb . size ) ) ; //motion
2017-03-05 16:44:50 +01:00
aabb = aabb . grow ( p_margin ) ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
int amount = space - > broadphase - > cull_aabb ( aabb , space - > intersection_query_results , Space2DSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
bool collided = false ;
r_result_count = 0 ;
2014-02-23 00:28:19 +01:00
Physics2DServerSW : : CollCbkData cbk ;
2017-03-05 16:44:50 +01:00
cbk . max = p_result_max ;
cbk . amount = 0 ;
2019-01-18 18:15:05 +01:00
cbk . passed = 0 ;
2017-03-05 16:44:50 +01:00
cbk . ptr = r_results ;
2019-08-07 12:54:30 +02:00
CollisionSolver2DSW : : CallbackResult cbkres = Physics2DServerSW : : _shape_col_cbk ;
2017-03-05 16:44:50 +01:00
2019-08-07 12:54:30 +02:00
Physics2DServerSW : : CollCbkData * cbkptr = & cbk ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < amount ; i + + ) {
2014-02-19 15:57:14 +01:00
2018-08-21 20:30:41 +02:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2014-02-23 00:28:19 +01:00
continue ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
const CollisionObject2DSW * col_obj = space - > intersection_query_results [ i ] ;
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
if ( p_exclude . has ( col_obj - > get_self ( ) ) )
2014-02-23 00:28:19 +01:00
continue ;
2017-06-24 04:30:43 +02:00
cbk . valid_dir = Vector2 ( ) ;
cbk . valid_depth = 0 ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
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 ) ) {
2019-04-08 11:03:37 +02:00
collided = cbk . amount > 0 ;
2014-02-19 15:57:14 +01:00
}
2014-02-23 00:28:19 +01:00
}
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
r_result_count = cbk . amount ;
2014-02-19 15:57:14 +01:00
2014-02-23 00:28:19 +01:00
return collided ;
}
2014-02-19 15:57:14 +01:00
2014-09-03 04:13:40 +02:00
struct _RestCallbackData2D {
2014-02-19 15:57:14 +01:00
2014-02-23 00:28:19 +01:00
const CollisionObject2DSW * object ;
const CollisionObject2DSW * best_object ;
2018-11-15 01:17:36 +01:00
int local_shape ;
int best_local_shape ;
2014-02-23 00:28:19 +01:00
int shape ;
int best_shape ;
Vector2 best_contact ;
Vector2 best_normal ;
2017-02-14 00:25:05 +01:00
real_t best_len ;
2015-01-14 01:19:11 +01:00
Vector2 valid_dir ;
2017-02-14 00:25:05 +01:00
real_t valid_depth ;
2019-02-16 17:45:01 +01:00
real_t min_allowed_depth ;
2014-02-23 00:28:19 +01:00
} ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
static void _rest_cbk_result ( const Vector2 & p_point_A , const Vector2 & p_point_B , void * p_userdata ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
_RestCallbackData2D * rd = ( _RestCallbackData2D * ) p_userdata ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
if ( rd - > valid_dir ! = Vector2 ( ) ) {
2017-06-24 04:30:43 +02:00
if ( p_point_A . distance_squared_to ( p_point_B ) > rd - > valid_depth * rd - > valid_depth )
return ;
if ( rd - > valid_dir . dot ( ( p_point_A - p_point_B ) . normalized ( ) ) < Math_PI * 0.25 )
return ;
2015-01-14 01:19:11 +01:00
}
2014-02-23 00:28:19 +01:00
Vector2 contact_rel = p_point_B - p_point_A ;
2017-02-14 00:25:05 +01:00
real_t len = contact_rel . length ( ) ;
2019-02-16 17:45:01 +01:00
if ( len < rd - > min_allowed_depth )
return ;
2014-02-23 00:28:19 +01:00
if ( len < = rd - > best_len )
return ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
rd - > best_len = len ;
rd - > best_contact = p_point_B ;
rd - > best_normal = contact_rel / len ;
rd - > best_object = rd - > object ;
rd - > best_shape = rd - > shape ;
2018-11-15 01:17:36 +01:00
rd - > best_local_shape = rd - > local_shape ;
2014-02-23 00:28:19 +01:00
}
2014-02-19 15:57:14 +01:00
2018-08-21 20:30:41 +02:00
bool Physics2DDirectSpaceStateSW : : rest_info ( RID p_shape , const Transform2D & p_shape_xform , const Vector2 & p_motion , real_t p_margin , ShapeRestInfo * r_info , const Set < RID > & p_exclude , uint32_t p_collision_mask , bool p_collide_with_bodies , bool p_collide_with_areas ) {
2014-02-19 15:57:14 +01:00
2016-01-04 04:00:33 +01:00
Shape2DSW * shape = Physics2DServerSW : : singletonsw - > shape_owner . get ( p_shape ) ;
2017-03-05 16:44:50 +01:00
ERR_FAIL_COND_V ( ! shape , 0 ) ;
2014-02-19 15:57:14 +01:00
2014-02-23 00:28:19 +01:00
Rect2 aabb = p_shape_xform . xform ( shape - > get_aabb ( ) ) ;
2017-06-04 00:25:13 +02:00
aabb = aabb . merge ( Rect2 ( aabb . position + p_motion , aabb . size ) ) ; //motion
2017-03-05 16:44:50 +01:00
aabb = aabb . grow ( p_margin ) ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
int amount = space - > broadphase - > cull_aabb ( aabb , space - > intersection_query_results , Space2DSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
2014-02-23 00:28:19 +01:00
2014-09-03 04:13:40 +02:00
_RestCallbackData2D rcd ;
2017-03-05 16:44:50 +01:00
rcd . best_len = 0 ;
rcd . best_object = NULL ;
rcd . best_shape = 0 ;
2019-02-16 17:45:01 +01:00
rcd . min_allowed_depth = space - > test_motion_min_contact_depth ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < amount ; i + + ) {
2014-02-19 15:57:14 +01:00
2018-08-21 20:30:41 +02:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2014-02-23 00:28:19 +01:00
continue ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
const CollisionObject2DSW * col_obj = space - > intersection_query_results [ i ] ;
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
if ( p_exclude . has ( col_obj - > get_self ( ) ) )
2014-02-19 15:57:14 +01:00
continue ;
2017-06-24 04:30:43 +02:00
rcd . valid_dir = Vector2 ( ) ;
rcd . valid_depth = 0 ;
2017-03-05 16:44:50 +01:00
rcd . object = col_obj ;
rcd . shape = shape_idx ;
2018-11-15 01:17:36 +01:00
rcd . local_shape = 0 ;
2017-03-05 16:44:50 +01:00
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 ) ;
2014-02-23 00:28:19 +01:00
if ( ! sc )
continue ;
2014-02-19 15:57:14 +01:00
}
2019-06-26 15:08:25 +02:00
if ( rcd . best_len = = 0 | | ! rcd . best_object )
2014-02-23 00:28:19 +01:00
return false ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
r_info - > collider_id = rcd . best_object - > get_instance_id ( ) ;
r_info - > shape = rcd . best_shape ;
r_info - > normal = rcd . best_normal ;
r_info - > point = rcd . best_contact ;
r_info - > rid = rcd . best_object - > get_self ( ) ;
r_info - > metadata = rcd . best_object - > get_shape_metadata ( rcd . best_shape ) ;
if ( rcd . best_object - > get_type ( ) = = CollisionObject2DSW : : TYPE_BODY ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
const Body2DSW * body = static_cast < const Body2DSW * > ( rcd . best_object ) ;
Vector2 rel_vec = r_info - > point - body - > get_transform ( ) . get_origin ( ) ;
2014-02-23 00:28:19 +01:00
r_info - > linear_velocity = Vector2 ( - body - > get_angular_velocity ( ) * rel_vec . y , body - > get_angular_velocity ( ) * rel_vec . x ) + body - > get_linear_velocity ( ) ;
2014-02-19 15:57:14 +01:00
2014-02-23 00:28:19 +01:00
} else {
2017-03-05 16:44:50 +01:00
r_info - > linear_velocity = Vector2 ( ) ;
2014-02-23 00:28:19 +01:00
}
return true ;
2014-02-19 15:57:14 +01:00
}
Physics2DDirectSpaceStateSW : : Physics2DDirectSpaceStateSW ( ) {
2017-03-05 16:44:50 +01:00
space = NULL ;
2014-02-19 15:57:14 +01:00
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////
2017-03-05 16:44:50 +01:00
int Space2DSW : : _cull_aabb_for_body ( Body2DSW * p_body , const Rect2 & p_aabb ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
int amount = broadphase - > cull_aabb ( p_aabb , intersection_query_results , INTERSECTION_QUERY_MAX , intersection_query_subindex_results ) ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < amount ; i + + ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
bool keep = true ;
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
if ( intersection_query_results [ i ] = = p_body )
keep = false ;
else if ( intersection_query_results [ i ] - > get_type ( ) = = CollisionObject2DSW : : TYPE_AREA )
keep = false ;
else if ( ( static_cast < Body2DSW * > ( intersection_query_results [ i ] ) - > test_collision_mask ( p_body ) ) = = 0 )
keep = false ;
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 ( ) ) )
keep = false ;
2017-06-24 04:30:43 +02:00
else if ( static_cast < Body2DSW * > ( intersection_query_results [ i ] ) - > is_shape_set_as_disabled ( intersection_query_subindex_results [ i ] ) )
2017-03-05 16:44:50 +01:00
keep = false ;
2015-04-20 01:50:55 +02:00
if ( ! keep ) {
2017-03-05 16:44:50 +01:00
if ( i < amount - 1 ) {
SWAP ( intersection_query_results [ i ] , intersection_query_results [ amount - 1 ] ) ;
SWAP ( intersection_query_subindex_results [ i ] , intersection_query_subindex_results [ amount - 1 ] ) ;
2015-04-20 01:50:55 +02:00
}
amount - - ;
i - - ;
}
}
2015-05-04 03:37:10 +02:00
return amount ;
}
2018-07-17 01:04:07 +02:00
int Space2DSW : : test_body_ray_separation ( Body2DSW * p_body , const Transform2D & p_transform , bool p_infinite_inertia , Vector2 & r_recover_motion , Physics2DServer : : SeparationResult * r_results , int p_result_max , real_t p_margin ) {
Rect2 body_aabb ;
2018-11-02 19:44:29 +01:00
bool shapes_found = false ;
2018-07-17 01:04:07 +02:00
for ( int i = 0 ; i < p_body - > get_shape_count ( ) ; i + + ) {
2018-11-02 19:44:29 +01:00
if ( p_body - > is_shape_set_as_disabled ( i ) )
continue ;
2019-02-21 21:20:44 +01:00
if ( p_body - > get_shape ( i ) - > get_type ( ) ! = Physics2DServer : : SHAPE_RAY )
continue ;
2018-11-02 19:44:29 +01:00
if ( ! shapes_found ) {
2018-07-17 01:04:07 +02:00
body_aabb = p_body - > get_shape_aabb ( i ) ;
2018-11-02 19:44:29 +01:00
shapes_found = true ;
} else {
2018-07-17 01:04:07 +02:00
body_aabb = body_aabb . merge ( p_body - > get_shape_aabb ( i ) ) ;
2018-11-02 19:44:29 +01:00
}
}
if ( ! shapes_found ) {
return 0 ;
2018-07-17 01:04:07 +02:00
}
// Undo the currently transform the physics server is aware of and apply the provided one
body_aabb = p_transform . xform ( p_body - > get_inv_transform ( ) . xform ( body_aabb ) ) ;
body_aabb = body_aabb . grow ( p_margin ) ;
Transform2D body_transform = p_transform ;
for ( int i = 0 ; i < p_result_max ; i + + ) {
//reset results
r_results [ i ] . collision_depth = 0 ;
}
int rays_found = 0 ;
{
// raycast AND separate
const int max_results = 32 ;
int recover_attempts = 4 ;
Vector2 sr [ max_results * 2 ] ;
Physics2DServerSW : : CollCbkData cbk ;
cbk . max = max_results ;
Physics2DServerSW : : CollCbkData * cbkptr = & cbk ;
CollisionSolver2DSW : : CallbackResult cbkres = Physics2DServerSW : : _shape_col_cbk ;
do {
Vector2 recover_motion ;
bool collided = false ;
int amount = _cull_aabb_for_body ( p_body , body_aabb ) ;
for ( int j = 0 ; j < p_body - > get_shape_count ( ) ; j + + ) {
if ( p_body - > is_shape_set_as_disabled ( j ) )
continue ;
Shape2DSW * body_shape = p_body - > get_shape ( j ) ;
if ( body_shape - > get_type ( ) ! = Physics2DServer : : SHAPE_RAY )
continue ;
Transform2D body_shape_xform = body_transform * p_body - > get_shape_transform ( j ) ;
for ( int i = 0 ; i < amount ; i + + ) {
const CollisionObject2DSW * col_obj = intersection_query_results [ i ] ;
int shape_idx = intersection_query_subindex_results [ i ] ;
cbk . amount = 0 ;
2019-01-18 18:15:05 +01:00
cbk . passed = 0 ;
2018-07-17 01:04:07 +02:00
cbk . ptr = sr ;
cbk . invalid_by_dir = 0 ;
if ( CollisionObject2DSW : : TYPE_BODY = = col_obj - > get_type ( ) ) {
const Body2DSW * b = static_cast < const Body2DSW * > ( col_obj ) ;
if ( p_infinite_inertia & & Physics2DServer : : BODY_MODE_STATIC ! = b - > get_mode ( ) & & Physics2DServer : : BODY_MODE_KINEMATIC ! = b - > get_mode ( ) ) {
continue ;
}
}
2018-11-10 14:12:35 +01:00
Transform2D col_obj_shape_xform = col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) ;
2019-02-16 14:06:38 +01:00
/*
* There is no point in supporting one way collisions with ray shapes , as they will always collide in the desired
* direction . Use a short ray shape if you want to achieve a similar effect .
*
2018-07-17 01:04:07 +02:00
if ( col_obj - > is_shape_set_as_one_way_collision ( shape_idx ) ) {
2018-11-10 14:12:35 +01:00
cbk . valid_dir = col_obj_shape_xform . get_axis ( 1 ) . normalized ( ) ;
2018-07-17 01:04:07 +02:00
cbk . valid_depth = p_margin ; //only valid depth is the collision margin
cbk . invalid_by_dir = 0 ;
} else {
2019-02-16 14:06:38 +01:00
*/
cbk . valid_dir = Vector2 ( ) ;
cbk . valid_depth = 0 ;
cbk . invalid_by_dir = 0 ;
/*
2018-07-17 01:04:07 +02:00
}
2019-02-16 14:06:38 +01:00
*/
2018-07-17 01:04:07 +02:00
Shape2DSW * against_shape = col_obj - > get_shape ( shape_idx ) ;
2018-11-10 14:12:35 +01:00
if ( CollisionSolver2DSW : : solve ( body_shape , body_shape_xform , Vector2 ( ) , against_shape , col_obj_shape_xform , Vector2 ( ) , cbkres , cbkptr , NULL , p_margin ) ) {
2018-07-17 01:04:07 +02:00
if ( cbk . amount > 0 ) {
collided = true ;
}
2019-02-16 20:14:57 +01:00
int ray_index = - 1 ; //reuse shape
for ( int k = 0 ; k < rays_found ; k + + ) {
if ( r_results [ ray_index ] . collision_local_shape = = j ) {
ray_index = k ;
}
}
if ( ray_index = = - 1 & & rays_found < p_result_max ) {
ray_index = rays_found ;
rays_found + + ;
}
if ( ray_index ! = - 1 ) {
2019-02-21 21:20:44 +01:00
2018-07-17 01:04:07 +02:00
Physics2DServer : : SeparationResult & result = r_results [ ray_index ] ;
for ( int k = 0 ; k < cbk . amount ; k + + ) {
Vector2 a = sr [ k * 2 + 0 ] ;
Vector2 b = sr [ k * 2 + 1 ] ;
recover_motion + = ( b - a ) * 0.4 ;
float depth = a . distance_to ( b ) ;
if ( depth > result . collision_depth ) {
result . collision_depth = depth ;
result . collision_point = b ;
result . collision_normal = ( b - a ) . normalized ( ) ;
2019-02-16 20:14:57 +01:00
result . collision_local_shape = j ;
result . collider_shape = shape_idx ;
2018-07-17 01:04:07 +02:00
result . collider = col_obj - > get_self ( ) ;
result . collider_id = col_obj - > get_instance_id ( ) ;
result . collider_metadata = col_obj - > get_shape_metadata ( shape_idx ) ;
if ( col_obj - > get_type ( ) = = CollisionObject2DSW : : TYPE_BODY ) {
Body2DSW * body = ( Body2DSW * ) col_obj ;
Vector2 rel_vec = b - body - > get_transform ( ) . get_origin ( ) ;
result . collider_velocity = Vector2 ( - body - > get_angular_velocity ( ) * rel_vec . y , body - > get_angular_velocity ( ) * rel_vec . x ) + body - > get_linear_velocity ( ) ;
}
}
}
}
}
}
}
if ( ! collided | | recover_motion = = Vector2 ( ) ) {
break ;
}
body_transform . elements [ 2 ] + = recover_motion ;
body_aabb . position + = recover_motion ;
recover_attempts - - ;
} while ( recover_attempts ) ;
}
//optimize results (remove non colliding)
for ( int i = 0 ; i < rays_found ; i + + ) {
if ( r_results [ i ] . collision_depth = = 0 ) {
rays_found - - ;
SWAP ( r_results [ i ] , r_results [ rays_found ] ) ;
}
}
r_recover_motion = body_transform . elements [ 2 ] - p_transform . elements [ 2 ] ;
return rays_found ;
}
bool Space2DSW : : test_body_motion ( Body2DSW * p_body , const Transform2D & p_from , const Vector2 & p_motion , bool p_infinite_inertia , real_t p_margin , Physics2DServer : : MotionResult * r_result , bool p_exclude_raycast_shapes ) {
2015-05-04 03:37:10 +02:00
//give me back regular physics engine logic
//this is madness
//and most people using this function will think
//what it does is simpler than using physics
//this took about a week to get right..
//but is it right? who knows at this point..
2016-09-01 17:03:55 +02:00
if ( r_result ) {
2017-03-05 16:44:50 +01:00
r_result - > collider_id = 0 ;
r_result - > collider_shape = 0 ;
2016-09-01 17:03:55 +02:00
}
2015-05-04 03:37:10 +02:00
Rect2 body_aabb ;
2018-11-02 19:44:29 +01:00
bool shapes_found = false ;
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < p_body - > get_shape_count ( ) ; i + + ) {
2015-05-04 03:37:10 +02:00
2018-11-02 19:44:29 +01:00
if ( p_body - > is_shape_set_as_disabled ( i ) )
continue ;
2019-02-21 21:20:44 +01:00
if ( p_exclude_raycast_shapes & & p_body - > get_shape ( i ) - > get_type ( ) = = Physics2DServer : : SHAPE_RAY )
continue ;
2018-11-02 19:44:29 +01:00
if ( ! shapes_found ) {
2017-03-05 16:44:50 +01:00
body_aabb = p_body - > get_shape_aabb ( i ) ;
2018-11-02 19:44:29 +01:00
shapes_found = true ;
} else {
2017-03-05 16:44:50 +01:00
body_aabb = body_aabb . merge ( p_body - > get_shape_aabb ( i ) ) ;
2018-11-02 19:44:29 +01:00
}
}
if ( ! shapes_found ) {
2019-02-16 15:06:17 +01:00
if ( r_result ) {
* r_result = Physics2DServer : : MotionResult ( ) ;
r_result - > motion = p_motion ;
}
2018-11-02 19:44:29 +01:00
return false ;
2015-05-04 03:37:10 +02:00
}
2017-03-06 05:23:00 +01:00
// Undo the currently transform the physics server is aware of and apply the provided one
body_aabb = p_from . xform ( p_body - > get_inv_transform ( ) . xform ( body_aabb ) ) ;
2017-03-05 16:44:50 +01:00
body_aabb = body_aabb . grow ( p_margin ) ;
2015-05-04 03:37:10 +02:00
2017-09-01 03:55:01 +02:00
static const int max_excluded_shape_pairs = 32 ;
2017-09-17 14:38:09 +02:00
ExcludedShapeSW excluded_shape_pairs [ max_excluded_shape_pairs ] ;
2017-09-01 03:55:01 +02:00
int excluded_shape_pair_count = 0 ;
2018-11-15 01:17:36 +01:00
float separation_margin = MIN ( p_margin , MAX ( 0.0 , p_motion . length ( ) - CMP_EPSILON ) ) ; //don't separate by more than the intended motion
2017-01-11 04:52:51 +01:00
Transform2D body_transform = p_from ;
2015-04-20 01:50:55 +02:00
{
//STEP 1, FREE BODY IF STUCK
const int max_results = 32 ;
2017-03-05 16:44:50 +01:00
int recover_attempts = 4 ;
Vector2 sr [ max_results * 2 ] ;
2015-04-20 01:50:55 +02:00
do {
Physics2DServerSW : : CollCbkData cbk ;
2017-03-05 16:44:50 +01:00
cbk . max = max_results ;
cbk . amount = 0 ;
2019-01-18 18:15:05 +01:00
cbk . passed = 0 ;
2017-03-05 16:44:50 +01:00
cbk . ptr = sr ;
2017-09-01 03:55:01 +02:00
cbk . invalid_by_dir = 0 ;
excluded_shape_pair_count = 0 ; //last step is the one valid
2015-04-20 01:50:55 +02:00
2017-08-21 21:15:36 +02:00
Physics2DServerSW : : CollCbkData * cbkptr = & cbk ;
CollisionSolver2DSW : : CallbackResult cbkres = Physics2DServerSW : : _shape_col_cbk ;
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
bool collided = false ;
2017-02-15 12:29:46 +01:00
2017-03-05 16:44:50 +01:00
int amount = _cull_aabb_for_body ( p_body , body_aabb ) ;
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
for ( int j = 0 ; j < p_body - > get_shape_count ( ) ; j + + ) {
2017-06-24 04:30:43 +02:00
if ( p_body - > is_shape_set_as_disabled ( j ) )
2015-04-20 01:50:55 +02:00
continue ;
Shape2DSW * body_shape = p_body - > get_shape ( j ) ;
2018-07-17 01:04:07 +02:00
if ( p_exclude_raycast_shapes & & body_shape - > get_type ( ) = = Physics2DServer : : SHAPE_RAY ) {
continue ;
}
Transform2D body_shape_xform = body_transform * p_body - > get_shape_transform ( j ) ;
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < amount ; i + + ) {
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
const CollisionObject2DSW * col_obj = intersection_query_results [ i ] ;
int shape_idx = intersection_query_subindex_results [ i ] ;
2015-04-20 01:50:55 +02:00
2018-02-16 19:06:00 +01:00
if ( CollisionObject2DSW : : TYPE_BODY = = col_obj - > get_type ( ) ) {
const Body2DSW * b = static_cast < const Body2DSW * > ( col_obj ) ;
if ( p_infinite_inertia & & Physics2DServer : : BODY_MODE_STATIC ! = b - > get_mode ( ) & & Physics2DServer : : BODY_MODE_KINEMATIC ! = b - > get_mode ( ) ) {
continue ;
}
}
2018-11-10 14:12:35 +01:00
Transform2D col_obj_shape_xform = col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) ;
2017-07-25 03:56:15 +02:00
if ( col_obj - > is_shape_set_as_one_way_collision ( shape_idx ) ) {
2015-05-11 00:24:09 +02:00
2018-11-10 14:12:35 +01:00
cbk . valid_dir = col_obj_shape_xform . get_axis ( 1 ) . normalized ( ) ;
2019-01-18 18:15:05 +01:00
float owc_margin = col_obj - > get_shape_one_way_collision_margin ( shape_idx ) ;
2019-05-19 12:34:40 +02:00
cbk . valid_depth = MAX ( owc_margin , p_margin ) ; //user specified, but never less than actual margin or it won't work
2017-09-01 03:55:01 +02:00
cbk . invalid_by_dir = 0 ;
2018-11-02 20:22:23 +01:00
if ( col_obj - > get_type ( ) = = CollisionObject2DSW : : TYPE_BODY ) {
const Body2DSW * b = static_cast < const Body2DSW * > ( col_obj ) ;
2018-11-03 17:30:08 +01:00
if ( b - > get_mode ( ) = = Physics2DServer : : BODY_MODE_KINEMATIC | | b - > get_mode ( ) = = Physics2DServer : : BODY_MODE_RIGID ) {
2018-11-03 17:23:25 +01:00
//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
2018-11-02 20:22:23 +01:00
Vector2 lv = b - > get_linear_velocity ( ) ;
//compute displacement from linear velocity
Vector2 motion = lv * Physics2DDirectBodyStateSW : : singleton - > step ;
float motion_len = motion . length ( ) ;
motion . normalize ( ) ;
cbk . valid_depth + = motion_len * MAX ( motion . dot ( - cbk . valid_dir ) , 0.0 ) ;
}
}
2015-04-20 01:50:55 +02:00
} else {
2017-03-05 16:44:50 +01:00
cbk . valid_dir = Vector2 ( ) ;
cbk . valid_depth = 0 ;
2017-09-01 03:55:01 +02:00
cbk . invalid_by_dir = 0 ;
2015-04-20 01:50:55 +02:00
}
2019-01-18 18:15:05 +01:00
int current_passed = cbk . passed ; //save how many points passed collision
2018-11-02 19:44:29 +01:00
bool did_collide = false ;
2017-09-01 03:55:01 +02:00
Shape2DSW * against_shape = col_obj - > get_shape ( shape_idx ) ;
2018-11-15 01:17:36 +01:00
if ( CollisionSolver2DSW : : solve ( body_shape , body_shape_xform , Vector2 ( ) , against_shape , col_obj_shape_xform , Vector2 ( ) , cbkres , cbkptr , NULL , separation_margin ) ) {
2019-01-18 18:15:05 +01:00
did_collide = cbk . passed > current_passed ; //more passed, so collision actually existed
2015-04-20 01:50:55 +02:00
}
2017-09-01 03:55:01 +02:00
2018-11-02 19:44:29 +01:00
if ( ! did_collide & & cbk . invalid_by_dir > 0 ) {
2017-09-01 03:55:01 +02:00
//this shape must be excluded
if ( excluded_shape_pair_count < max_excluded_shape_pairs ) {
2017-09-17 14:38:09 +02:00
ExcludedShapeSW esp ;
esp . local_shape = body_shape ;
esp . against_object = col_obj ;
2017-09-17 15:18:09 +02:00
esp . against_shape_index = shape_idx ;
2017-09-17 14:38:09 +02:00
excluded_shape_pairs [ excluded_shape_pair_count + + ] = esp ;
2017-09-01 03:55:01 +02:00
}
}
2018-11-02 19:44:29 +01:00
if ( did_collide ) {
collided = true ;
}
2015-04-20 01:50:55 +02:00
}
}
2017-02-15 12:29:46 +01:00
if ( ! collided ) {
2015-04-20 01:50:55 +02:00
break ;
2017-02-15 12:29:46 +01:00
}
2015-04-20 01:50:55 +02:00
Vector2 recover_motion ;
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < cbk . amount ; i + + ) {
2017-02-15 12:29:46 +01:00
2017-03-05 16:44:50 +01:00
Vector2 a = sr [ i * 2 + 0 ] ;
Vector2 b = sr [ i * 2 + 1 ] ;
recover_motion + = ( b - a ) * 0.4 ;
2015-04-20 01:50:55 +02:00
}
2017-03-05 16:44:50 +01:00
if ( recover_motion = = Vector2 ( ) ) {
collided = false ;
2015-04-20 01:50:55 +02:00
break ;
}
2017-03-05 16:44:50 +01:00
body_transform . elements [ 2 ] + = recover_motion ;
2017-06-04 00:25:13 +02:00
body_aabb . position + = recover_motion ;
2015-04-20 01:50:55 +02:00
recover_attempts - - ;
} while ( recover_attempts ) ;
}
2017-02-14 00:25:05 +01:00
real_t safe = 1.0 ;
real_t unsafe = 1.0 ;
2017-03-05 16:44:50 +01:00
int best_shape = - 1 ;
2015-04-20 01:50:55 +02:00
{
// STEP 2 ATTEMPT MOTION
2017-03-05 16:44:50 +01:00
Rect2 motion_aabb = body_aabb ;
2017-06-04 00:25:13 +02:00
motion_aabb . position + = p_motion ;
2017-03-05 16:44:50 +01:00
motion_aabb = motion_aabb . merge ( body_aabb ) ;
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
int amount = _cull_aabb_for_body ( p_body , motion_aabb ) ;
2015-04-20 01:50:55 +02:00
2017-11-08 21:45:49 +01:00
for ( int body_shape_idx = 0 ; body_shape_idx < p_body - > get_shape_count ( ) ; body_shape_idx + + ) {
2015-04-20 01:50:55 +02:00
2017-11-08 21:45:49 +01:00
if ( p_body - > is_shape_set_as_disabled ( body_shape_idx ) )
2015-04-20 01:50:55 +02:00
continue ;
2017-11-08 21:45:49 +01:00
Shape2DSW * body_shape = p_body - > get_shape ( body_shape_idx ) ;
2018-07-17 01:04:07 +02:00
if ( p_exclude_raycast_shapes & & body_shape - > get_type ( ) = = Physics2DServer : : SHAPE_RAY ) {
continue ;
}
Transform2D body_shape_xform = body_transform * p_body - > get_shape_transform ( body_shape_idx ) ;
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
bool stuck = false ;
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
real_t best_safe = 1 ;
real_t best_unsafe = 1 ;
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < amount ; i + + ) {
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
const CollisionObject2DSW * col_obj = intersection_query_results [ i ] ;
2017-11-08 21:45:49 +01:00
int col_shape_idx = intersection_query_subindex_results [ i ] ;
Shape2DSW * against_shape = col_obj - > get_shape ( col_shape_idx ) ;
2017-09-01 03:55:01 +02:00
2018-02-16 19:06:00 +01:00
if ( CollisionObject2DSW : : TYPE_BODY = = col_obj - > get_type ( ) ) {
const Body2DSW * b = static_cast < const Body2DSW * > ( col_obj ) ;
if ( p_infinite_inertia & & Physics2DServer : : BODY_MODE_STATIC ! = b - > get_mode ( ) & & Physics2DServer : : BODY_MODE_KINEMATIC ! = b - > get_mode ( ) ) {
continue ;
}
}
2017-09-01 03:55:01 +02:00
bool excluded = false ;
for ( int k = 0 ; k < excluded_shape_pair_count ; k + + ) {
2017-11-08 21:45:49 +01:00
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 ) {
2017-09-01 03:55:01 +02:00
excluded = true ;
break ;
}
}
if ( excluded ) {
continue ;
}
2015-04-20 01:50:55 +02:00
2018-11-10 14:12:35 +01:00
Transform2D col_obj_shape_xform = col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( col_shape_idx ) ;
2015-04-20 01:50:55 +02:00
//test initial overlap, does it collide if going all the way?
2018-11-10 14:12:35 +01:00
if ( ! CollisionSolver2DSW : : solve ( body_shape , body_shape_xform , p_motion , against_shape , col_obj_shape_xform , Vector2 ( ) , NULL , NULL , NULL , 0 ) ) {
2015-04-20 01:50:55 +02:00
continue ;
}
//test initial overlap
2018-11-10 14:12:35 +01:00
if ( CollisionSolver2DSW : : solve ( body_shape , body_shape_xform , Vector2 ( ) , against_shape , col_obj_shape_xform , Vector2 ( ) , NULL , NULL , NULL , 0 ) ) {
2015-04-20 01:50:55 +02:00
2017-11-08 21:45:49 +01:00
if ( col_obj - > is_shape_set_as_one_way_collision ( col_shape_idx ) ) {
2017-06-24 04:30:43 +02:00
continue ;
2015-04-20 01:50:55 +02:00
}
2017-03-05 16:44:50 +01:00
stuck = true ;
2015-04-20 01:50:55 +02:00
break ;
}
//just do kinematic solving
2017-03-05 16:44:50 +01:00
real_t low = 0 ;
real_t hi = 1 ;
Vector2 mnormal = p_motion . normalized ( ) ;
2015-04-20 01:50:55 +02:00
2017-06-24 04:30:43 +02:00
for ( int k = 0 ; k < 8 ; k + + ) { //steps should be customizable..
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
real_t ofs = ( low + hi ) * 0.5 ;
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
Vector2 sep = mnormal ; //important optimization for this to work fast enough
2018-11-10 14:12:35 +01:00
bool collided = CollisionSolver2DSW : : solve ( body_shape , body_shape_xform , p_motion * ofs , against_shape , col_obj_shape_xform , Vector2 ( ) , NULL , NULL , & sep , 0 ) ;
2015-04-20 01:50:55 +02:00
if ( collided ) {
2017-03-05 16:44:50 +01:00
hi = ofs ;
2015-04-20 01:50:55 +02:00
} else {
2017-03-05 16:44:50 +01:00
low = ofs ;
2015-04-20 01:50:55 +02:00
}
}
2017-11-08 21:45:49 +01:00
if ( col_obj - > is_shape_set_as_one_way_collision ( col_shape_idx ) ) {
2015-04-20 01:50:55 +02:00
2017-08-16 12:36:26 +02:00
Vector2 cd [ 2 ] ;
Physics2DServerSW : : CollCbkData cbk ;
cbk . max = 1 ;
cbk . amount = 0 ;
2019-01-18 18:15:05 +01:00
cbk . passed = 0 ;
2017-08-16 12:36:26 +02:00
cbk . ptr = cd ;
2018-11-10 14:12:35 +01:00
cbk . valid_dir = col_obj_shape_xform . get_axis ( 1 ) . normalized ( ) ;
2017-09-01 03:55:01 +02:00
2017-08-16 12:36:26 +02:00
cbk . valid_depth = 10e20 ;
2015-04-20 01:50:55 +02:00
2017-08-16 12:36:26 +02:00
Vector2 sep = mnormal ; //important optimization for this to work fast enough
2018-11-10 14:12:35 +01:00
bool collided = CollisionSolver2DSW : : solve ( body_shape , body_shape_xform , p_motion * ( hi + contact_max_allowed_penetration ) , col_obj - > get_shape ( col_shape_idx ) , col_obj_shape_xform , Vector2 ( ) , Physics2DServerSW : : _shape_col_cbk , & cbk , & sep , 0 ) ;
2017-08-16 12:36:26 +02:00
if ( ! collided | | cbk . amount = = 0 ) {
continue ;
2015-04-20 01:50:55 +02:00
}
}
2017-03-05 16:44:50 +01:00
if ( low < best_safe ) {
best_safe = low ;
best_unsafe = hi ;
2015-04-20 01:50:55 +02:00
}
}
if ( stuck ) {
2017-03-05 16:44:50 +01:00
safe = 0 ;
unsafe = 0 ;
2017-11-08 21:45:49 +01:00
best_shape = body_shape_idx ; //sadly it's the best
2015-04-20 01:50:55 +02:00
break ;
}
2017-03-05 16:44:50 +01:00
if ( best_safe = = 1.0 ) {
2015-04-20 01:50:55 +02:00
continue ;
}
if ( best_safe < safe ) {
2017-03-05 16:44:50 +01:00
safe = best_safe ;
unsafe = best_unsafe ;
2017-11-08 21:45:49 +01:00
best_shape = body_shape_idx ;
2015-04-20 01:50:55 +02:00
}
}
}
2017-03-05 16:44:50 +01:00
bool collided = false ;
if ( safe > = 1 ) {
2018-11-15 01:17:36 +01:00
best_shape = - 1 ; //no best shape with cast, reset to -1
}
2015-04-20 01:50:55 +02:00
2018-11-15 01:17:36 +01:00
{
2015-04-20 01:50:55 +02:00
//it collided, let's get the rest info in unsafe advance
2017-01-11 04:52:51 +01:00
Transform2D ugt = body_transform ;
2017-03-05 16:44:50 +01:00
ugt . elements [ 2 ] + = p_motion * unsafe ;
2015-04-20 01:50:55 +02:00
_RestCallbackData2D rcd ;
2017-03-05 16:44:50 +01:00
rcd . best_len = 0 ;
rcd . best_object = NULL ;
rcd . best_shape = 0 ;
2019-02-16 17:45:01 +01:00
rcd . min_allowed_depth = test_motion_min_contact_depth ;
2015-04-20 01:50:55 +02:00
2018-11-15 01:17:36 +01:00
//optimization
int from_shape = best_shape ! = - 1 ? best_shape : 0 ;
int to_shape = best_shape ! = - 1 ? best_shape + 1 : p_body - > get_shape_count ( ) ;
2015-04-20 01:50:55 +02:00
2018-11-15 01:17:36 +01:00
for ( int j = from_shape ; j < to_shape ; j + + ) {
2019-02-16 20:32:30 +01:00
if ( p_body - > is_shape_set_as_disabled ( j ) )
continue ;
2018-11-15 01:17:36 +01:00
Transform2D body_shape_xform = ugt * p_body - > get_shape_transform ( j ) ;
Shape2DSW * body_shape = p_body - > get_shape ( j ) ;
2015-05-04 03:37:10 +02:00
2019-02-21 21:20:44 +01:00
if ( p_exclude_raycast_shapes & & body_shape - > get_type ( ) = = Physics2DServer : : SHAPE_RAY ) {
continue ;
}
2018-11-15 01:17:36 +01:00
body_aabb . position + = p_motion * unsafe ;
2015-04-20 01:50:55 +02:00
2018-11-15 01:17:36 +01:00
int amount = _cull_aabb_for_body ( p_body , body_aabb ) ;
2015-04-20 01:50:55 +02:00
2018-11-15 01:17:36 +01:00
for ( int i = 0 ; i < amount ; i + + ) {
2015-04-20 01:50:55 +02:00
2018-11-15 01:17:36 +01:00
const CollisionObject2DSW * col_obj = intersection_query_results [ i ] ;
int shape_idx = intersection_query_subindex_results [ i ] ;
if ( CollisionObject2DSW : : TYPE_BODY = = col_obj - > get_type ( ) ) {
const Body2DSW * b = static_cast < const Body2DSW * > ( col_obj ) ;
if ( p_infinite_inertia & & Physics2DServer : : BODY_MODE_STATIC ! = b - > get_mode ( ) & & Physics2DServer : : BODY_MODE_KINEMATIC ! = b - > get_mode ( ) ) {
continue ;
}
2018-02-16 19:06:00 +01:00
}
2018-11-15 01:17:36 +01:00
Shape2DSW * against_shape = col_obj - > get_shape ( shape_idx ) ;
2017-09-01 03:55:01 +02:00
2018-11-15 01:17:36 +01:00
bool excluded = false ;
for ( int k = 0 ; k < excluded_shape_pair_count ; k + + ) {
2017-09-01 03:55:01 +02:00
2018-11-15 01:17:36 +01:00
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 ) {
excluded = true ;
break ;
}
2017-09-01 03:55:01 +02:00
}
2018-11-15 01:17:36 +01:00
if ( excluded )
continue ;
2017-09-01 03:55:01 +02:00
2018-11-15 01:17:36 +01:00
Transform2D col_obj_shape_xform = col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) ;
2018-11-10 14:12:35 +01:00
2018-11-15 01:17:36 +01:00
if ( col_obj - > is_shape_set_as_one_way_collision ( shape_idx ) ) {
2015-04-20 01:50:55 +02:00
2018-11-15 01:17:36 +01:00
rcd . valid_dir = col_obj_shape_xform . get_axis ( 1 ) . normalized ( ) ;
rcd . valid_depth = 10e20 ;
} else {
rcd . valid_dir = Vector2 ( ) ;
rcd . valid_depth = 0 ;
}
2015-04-20 01:50:55 +02:00
2018-11-15 01:17:36 +01:00
rcd . object = col_obj ;
rcd . shape = shape_idx ;
rcd . local_shape = j ;
bool sc = CollisionSolver2DSW : : solve ( body_shape , body_shape_xform , Vector2 ( ) , against_shape , col_obj_shape_xform , Vector2 ( ) , _rest_cbk_result , & rcd , NULL , p_margin ) ;
if ( ! sc )
continue ;
}
2015-04-20 01:50:55 +02:00
}
2017-03-05 16:44:50 +01:00
if ( rcd . best_len ! = 0 ) {
2015-04-20 01:50:55 +02:00
if ( r_result ) {
2017-03-05 16:44:50 +01:00
r_result - > collider = rcd . best_object - > get_self ( ) ;
r_result - > collider_id = rcd . best_object - > get_instance_id ( ) ;
r_result - > collider_shape = rcd . best_shape ;
2018-11-15 01:17:36 +01:00
r_result - > collision_local_shape = rcd . best_local_shape ;
2017-03-05 16:44:50 +01:00
r_result - > collision_normal = rcd . best_normal ;
r_result - > collision_point = rcd . best_contact ;
r_result - > collider_metadata = rcd . best_object - > get_shape_metadata ( rcd . best_shape ) ;
const Body2DSW * body = static_cast < const Body2DSW * > ( rcd . best_object ) ;
Vector2 rel_vec = r_result - > collision_point - body - > get_transform ( ) . get_origin ( ) ;
2015-04-20 01:50:55 +02:00
r_result - > collider_velocity = Vector2 ( - body - > get_angular_velocity ( ) * rel_vec . y , body - > get_angular_velocity ( ) * rel_vec . x ) + body - > get_linear_velocity ( ) ;
2017-03-05 16:44:50 +01:00
r_result - > motion = safe * p_motion ;
r_result - > remainder = p_motion - safe * p_motion ;
r_result - > motion + = ( body_transform . get_origin ( ) - p_from . get_origin ( ) ) ;
2015-04-20 01:50:55 +02:00
}
2017-03-05 16:44:50 +01:00
collided = true ;
2018-11-15 01:17:36 +01:00
}
}
2015-04-20 01:50:55 +02:00
2018-11-15 01:17:36 +01:00
if ( ! collided & & r_result ) {
2015-04-20 01:50:55 +02:00
2018-11-15 01:17:36 +01:00
r_result - > motion = p_motion ;
r_result - > remainder = Vector2 ( ) ;
r_result - > motion + = ( body_transform . get_origin ( ) - p_from . get_origin ( ) ) ;
2015-04-20 01:50:55 +02:00
}
return collided ;
}
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
void * Space2DSW : : _broadphase_pair ( CollisionObject2DSW * A , int p_subindex_A , CollisionObject2DSW * B , int p_subindex_B , void * p_self ) {
2020-06-08 18:18:23 +02:00
if ( ! A - > test_collision_mask ( B ) ) {
return nullptr ;
}
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
CollisionObject2DSW : : Type type_A = A - > get_type ( ) ;
CollisionObject2DSW : : Type type_B = B - > get_type ( ) ;
if ( type_A > type_B ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
SWAP ( A , B ) ;
SWAP ( p_subindex_A , p_subindex_B ) ;
SWAP ( type_A , type_B ) ;
2014-02-19 15:57:14 +01:00
}
2017-03-05 16:44:50 +01:00
Space2DSW * self = ( Space2DSW * ) p_self ;
2014-09-03 04:13:40 +02:00
self - > collision_pairs + + ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
if ( type_A = = CollisionObject2DSW : : TYPE_AREA ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
Area2DSW * area = static_cast < Area2DSW * > ( A ) ;
if ( type_B = = CollisionObject2DSW : : TYPE_AREA ) {
2015-03-17 04:45:25 +01:00
2017-03-05 16:44:50 +01:00
Area2DSW * area_b = static_cast < Area2DSW * > ( B ) ;
Area2Pair2DSW * area2_pair = memnew ( Area2Pair2DSW ( area_b , p_subindex_B , area , p_subindex_A ) ) ;
2015-03-17 04:45:25 +01:00
return area2_pair ;
} else {
2017-03-05 16:44:50 +01:00
Body2DSW * body = static_cast < Body2DSW * > ( B ) ;
AreaPair2DSW * area_pair = memnew ( AreaPair2DSW ( body , p_subindex_B , area , p_subindex_A ) ) ;
2015-03-17 04:45:25 +01:00
return area_pair ;
}
2014-02-19 15:57:14 +01:00
} else {
2017-03-05 16:44:50 +01:00
BodyPair2DSW * b = memnew ( BodyPair2DSW ( ( Body2DSW * ) A , p_subindex_A , ( Body2DSW * ) B , p_subindex_B ) ) ;
2014-02-19 15:57:14 +01:00
return b ;
}
return NULL ;
}
2017-03-05 16:44:50 +01:00
void Space2DSW : : _broadphase_unpair ( CollisionObject2DSW * A , int p_subindex_A , CollisionObject2DSW * B , int p_subindex_B , void * p_data , void * p_self ) {
2020-06-08 18:18:23 +02:00
if ( ! p_data ) {
return ;
}
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
Space2DSW * self = ( Space2DSW * ) p_self ;
2014-09-03 04:13:40 +02:00
self - > collision_pairs - - ;
2017-03-05 16:44:50 +01:00
Constraint2DSW * c = ( Constraint2DSW * ) p_data ;
2014-02-19 15:57:14 +01:00
memdelete ( c ) ;
}
2017-03-05 16:44:50 +01:00
const SelfList < Body2DSW > : : List & Space2DSW : : get_active_body_list ( ) const {
2014-02-19 15:57:14 +01:00
return active_list ;
}
2017-03-05 16:44:50 +01:00
void Space2DSW : : body_add_to_active_list ( SelfList < Body2DSW > * p_body ) {
2014-02-19 15:57:14 +01:00
active_list . add ( p_body ) ;
}
2017-03-05 16:44:50 +01:00
void Space2DSW : : body_remove_from_active_list ( SelfList < Body2DSW > * p_body ) {
2014-02-19 15:57:14 +01:00
active_list . remove ( p_body ) ;
}
2017-03-05 16:44:50 +01:00
void Space2DSW : : body_add_to_inertia_update_list ( SelfList < Body2DSW > * p_body ) {
2014-02-19 15:57:14 +01:00
inertia_update_list . add ( p_body ) ;
}
2017-03-05 16:44:50 +01:00
void Space2DSW : : body_remove_from_inertia_update_list ( SelfList < Body2DSW > * p_body ) {
2014-02-19 15:57:14 +01:00
inertia_update_list . remove ( p_body ) ;
}
BroadPhase2DSW * Space2DSW : : get_broadphase ( ) {
return broadphase ;
}
void Space2DSW : : add_object ( CollisionObject2DSW * p_object ) {
2017-03-05 16:44:50 +01:00
ERR_FAIL_COND ( objects . has ( p_object ) ) ;
2014-02-19 15:57:14 +01:00
objects . insert ( p_object ) ;
}
void Space2DSW : : remove_object ( CollisionObject2DSW * p_object ) {
2017-03-05 16:44:50 +01:00
ERR_FAIL_COND ( ! objects . has ( p_object ) ) ;
2014-02-19 15:57:14 +01:00
objects . erase ( p_object ) ;
}
2017-03-05 16:44:50 +01:00
const Set < CollisionObject2DSW * > & Space2DSW : : get_objects ( ) const {
2014-02-19 15:57:14 +01:00
return objects ;
}
2017-03-05 16:44:50 +01:00
void Space2DSW : : body_add_to_state_query_list ( SelfList < Body2DSW > * p_body ) {
2014-02-19 15:57:14 +01:00
state_query_list . add ( p_body ) ;
}
2017-03-05 16:44:50 +01:00
void Space2DSW : : body_remove_from_state_query_list ( SelfList < Body2DSW > * p_body ) {
2014-02-19 15:57:14 +01:00
state_query_list . remove ( p_body ) ;
}
2017-03-05 16:44:50 +01:00
void Space2DSW : : area_add_to_monitor_query_list ( SelfList < Area2DSW > * p_area ) {
2014-02-19 15:57:14 +01:00
monitor_query_list . add ( p_area ) ;
}
2017-03-05 16:44:50 +01:00
void Space2DSW : : area_remove_from_monitor_query_list ( SelfList < Area2DSW > * p_area ) {
2014-02-19 15:57:14 +01:00
monitor_query_list . remove ( p_area ) ;
}
2017-03-05 16:44:50 +01:00
void Space2DSW : : area_add_to_moved_list ( SelfList < Area2DSW > * p_area ) {
2014-02-19 15:57:14 +01:00
area_moved_list . add ( p_area ) ;
}
2017-03-05 16:44:50 +01:00
void Space2DSW : : area_remove_from_moved_list ( SelfList < Area2DSW > * p_area ) {
2014-02-19 15:57:14 +01:00
area_moved_list . remove ( p_area ) ;
}
2017-03-05 16:44:50 +01:00
const SelfList < Area2DSW > : : List & Space2DSW : : get_moved_area_list ( ) const {
2014-02-19 15:57:14 +01:00
return area_moved_list ;
}
void Space2DSW : : call_queries ( ) {
2017-03-05 16:44:50 +01:00
while ( state_query_list . first ( ) ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
Body2DSW * b = state_query_list . first ( ) - > self ( ) ;
2014-02-19 15:57:14 +01:00
state_query_list . remove ( state_query_list . first ( ) ) ;
2017-11-10 12:31:25 +01:00
b - > call_queries ( ) ;
2014-02-19 15:57:14 +01:00
}
2017-03-05 16:44:50 +01:00
while ( monitor_query_list . first ( ) ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
Area2DSW * a = monitor_query_list . first ( ) - > self ( ) ;
2014-02-19 15:57:14 +01:00
monitor_query_list . remove ( monitor_query_list . first ( ) ) ;
2017-11-10 12:31:25 +01:00
a - > call_queries ( ) ;
2014-02-19 15:57:14 +01:00
}
}
void Space2DSW : : setup ( ) {
2017-03-05 16:44:50 +01:00
contact_debug_count = 0 ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
while ( inertia_update_list . first ( ) ) {
2014-02-19 15:57:14 +01:00
inertia_update_list . first ( ) - > self ( ) - > update_inertias ( ) ;
inertia_update_list . remove ( inertia_update_list . first ( ) ) ;
}
}
void Space2DSW : : update ( ) {
broadphase - > update ( ) ;
}
void Space2DSW : : set_param ( Physics2DServer : : SpaceParameter p_param , real_t p_value ) {
2017-03-05 16:44:50 +01:00
switch ( p_param ) {
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
case Physics2DServer : : SPACE_PARAM_CONTACT_RECYCLE_RADIUS : contact_recycle_radius = p_value ; break ;
case Physics2DServer : : SPACE_PARAM_CONTACT_MAX_SEPARATION : contact_max_separation = p_value ; break ;
case Physics2DServer : : SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION : contact_max_allowed_penetration = p_value ; break ;
2017-07-08 17:12:18 +02:00
case Physics2DServer : : SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD : body_linear_velocity_sleep_threshold = p_value ; break ;
case Physics2DServer : : SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD : body_angular_velocity_sleep_threshold = p_value ; break ;
2017-03-05 16:44:50 +01:00
case Physics2DServer : : SPACE_PARAM_BODY_TIME_TO_SLEEP : body_time_to_sleep = p_value ; break ;
case Physics2DServer : : SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS : constraint_bias = p_value ; break ;
2019-02-16 17:45:01 +01:00
case Physics2DServer : : SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH : test_motion_min_contact_depth = p_value ; break ;
2014-02-19 15:57:14 +01:00
}
}
real_t Space2DSW : : get_param ( Physics2DServer : : SpaceParameter p_param ) const {
2017-03-05 16:44:50 +01:00
switch ( p_param ) {
2014-02-19 15:57:14 +01:00
case Physics2DServer : : SPACE_PARAM_CONTACT_RECYCLE_RADIUS : return contact_recycle_radius ;
case Physics2DServer : : SPACE_PARAM_CONTACT_MAX_SEPARATION : return contact_max_separation ;
case Physics2DServer : : SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION : return contact_max_allowed_penetration ;
2017-07-08 17:12:18 +02:00
case Physics2DServer : : SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD : return body_linear_velocity_sleep_threshold ;
case Physics2DServer : : SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD : return body_angular_velocity_sleep_threshold ;
2014-02-19 15:57:14 +01:00
case Physics2DServer : : SPACE_PARAM_BODY_TIME_TO_SLEEP : return body_time_to_sleep ;
case Physics2DServer : : SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS : return constraint_bias ;
2019-02-16 17:45:01 +01:00
case Physics2DServer : : SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH : return test_motion_min_contact_depth ;
2014-02-19 15:57:14 +01:00
}
return 0 ;
}
void Space2DSW : : lock ( ) {
2017-03-05 16:44:50 +01:00
locked = true ;
2014-02-19 15:57:14 +01:00
}
void Space2DSW : : unlock ( ) {
2017-03-05 16:44:50 +01:00
locked = false ;
2014-02-19 15:57:14 +01:00
}
bool Space2DSW : : is_locked ( ) const {
return locked ;
}
Physics2DDirectSpaceStateSW * Space2DSW : : get_direct_state ( ) {
return direct_access ;
}
Space2DSW : : Space2DSW ( ) {
2017-03-05 16:44:50 +01:00
collision_pairs = 0 ;
active_objects = 0 ;
island_count = 0 ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
contact_debug_count = 0 ;
2014-09-03 04:13:40 +02:00
2017-03-05 16:44:50 +01:00
locked = false ;
contact_recycle_radius = 1.0 ;
contact_max_separation = 1.5 ;
contact_max_allowed_penetration = 0.3 ;
2019-02-16 17:45:01 +01:00
test_motion_min_contact_depth = 0.005 ;
2016-10-03 03:42:17 +02:00
constraint_bias = 0.2 ;
2017-07-15 12:01:46 +02:00
body_linear_velocity_sleep_threshold = GLOBAL_DEF ( " physics/2d/sleep_threshold_linear " , 2.0 ) ;
2017-07-08 17:12:18 +02:00
body_angular_velocity_sleep_threshold = GLOBAL_DEF ( " physics/2d/sleep_threshold_angular " , ( 8.0 / 180.0 * Math_PI ) ) ;
2017-03-05 16:44:50 +01:00
body_time_to_sleep = GLOBAL_DEF ( " physics/2d/time_before_sleep " , 0.5 ) ;
2018-10-05 18:43:53 +02:00
ProjectSettings : : get_singleton ( ) - > set_custom_property_info ( " physics/2d/time_before_sleep " , PropertyInfo ( Variant : : REAL , " physics/2d/time_before_sleep " , PROPERTY_HINT_RANGE , " 0,5,0.01,or_greater " ) ) ;
2014-02-19 15:57:14 +01:00
broadphase = BroadPhase2DSW : : create_func ( ) ;
2017-03-05 16:44:50 +01:00
broadphase - > set_pair_callback ( _broadphase_pair , this ) ;
broadphase - > set_unpair_callback ( _broadphase_unpair , this ) ;
area = NULL ;
2015-09-20 18:03:46 +02:00
2017-03-05 16:44:50 +01:00
direct_access = memnew ( Physics2DDirectSpaceStateSW ) ;
direct_access - > space = this ;
2015-09-20 18:03:46 +02:00
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < ELAPSED_TIME_MAX ; i + + )
elapsed_time [ i ] = 0 ;
2014-02-19 15:57:14 +01:00
}
Space2DSW : : ~ Space2DSW ( ) {
memdelete ( broadphase ) ;
2017-03-05 16:44:50 +01:00
memdelete ( direct_access ) ;
2014-02-19 15:57:14 +01:00
}