2014-02-10 02:10:30 +01:00
/*************************************************************************/
/* space_2d_sw.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
2017-01-01 22:01:57 +01:00
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
2017-04-08 00:11:42 +02:00
/* Copyright (c) 2014-2017 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. */
/*************************************************************************/
2014-02-19 15:57:14 +01:00
# include "space_2d_sw.h"
# include "collision_solver_2d_sw.h"
# include "physics_2d_server_sw.h"
2017-06-13 17:45:01 +02:00
_FORCE_INLINE_ static bool _match_object_type_query ( CollisionObject2DSW * p_object , uint32_t p_collision_layer , uint32_t p_type_mask ) {
2014-02-23 00:28:19 +01:00
2017-06-13 17:45:01 +02:00
if ( ( p_object - > get_collision_layer ( ) & p_collision_layer ) = = 0 )
2014-02-23 00:28:19 +01:00
return false ;
2017-03-05 16:44:50 +01:00
if ( p_object - > get_type ( ) = = CollisionObject2DSW : : TYPE_AREA )
return p_type_mask & Physics2DDirectSpaceState : : TYPE_MASK_AREA ;
2014-02-23 00:28:19 +01:00
2017-03-05 16:44:50 +01:00
Body2DSW * body = static_cast < Body2DSW * > ( p_object ) ;
2014-02-23 00:28:19 +01:00
2017-03-05 16:44:50 +01:00
return ( 1 < < body - > get_mode ( ) ) & p_type_mask ;
2014-02-23 00:28:19 +01:00
}
2017-06-13 17:45:01 +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_layer , uint32_t p_object_type_mask , bool p_pick_point ) {
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 + + ) {
2015-03-22 05:46:18 +01:00
2017-06-13 17:45:01 +02:00
if ( ! _match_object_type_query ( space - > intersection_query_results [ i ] , p_collision_layer , p_object_type_mask ) )
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 ;
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 ;
}
2017-06-13 17:45:01 +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_layer , uint32_t p_object_type_mask ) {
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
//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
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
2017-06-13 17:45:01 +02:00
if ( ! _match_object_type_query ( space - > intersection_query_results [ i ] , p_collision_layer , p_object_type_mask ) )
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 ;
}
2017-06-13 17:45:01 +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_layer , uint32_t p_object_type_mask ) {
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
2017-03-05 16:44:50 +01:00
int amount = space - > broadphase - > cull_aabb ( aabb , space - > intersection_query_results , p_result_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
2017-06-13 17:45:01 +02:00
if ( ! _match_object_type_query ( space - > intersection_query_results [ i ] , p_collision_layer , p_object_type_mask ) )
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 ;
}
2017-06-13 17:45:01 +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_layer , uint32_t p_object_type_mask ) {
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-01-14 12:26:56 +01:00
/*
if ( p_motion ! = Vector2 ( ) )
print_line ( p_motion ) ;
*/
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
2017-06-13 17:45:01 +02:00
if ( ! _match_object_type_query ( space - > intersection_query_results [ i ] , p_collision_layer , p_object_type_mask ) )
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
2015-01-14 01:19:11 +01:00
/*if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
const Body2DSW * body = static_cast < const Body2DSW * > ( col_obj ) ;
if ( body - > get_one_way_collision_direction ( ) ! = Vector2 ( ) & & p_motion . dot ( body - > get_one_way_collision_direction ( ) ) < = CMP_EPSILON ) {
print_line ( " failed in motion dir " ) ;
continue ;
}
} */
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
}
2014-02-23 00:28:19 +01:00
//test initial overlap
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
return false ;
}
//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
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < 8 ; i + + ) { //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
}
2017-06-13 17:45:01 +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_layer , uint32_t p_object_type_mask ) {
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 ;
cbk . ptr = r_results ;
CollisionSolver2DSW : : CallbackResult cbkres = NULL ;
Physics2DServerSW : : CollCbkData * cbkptr = NULL ;
if ( p_result_max > 0 ) {
cbkptr = & cbk ;
cbkres = Physics2DServerSW : : _shape_col_cbk ;
2014-02-23 00:28:19 +01:00
}
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-06-13 17:45:01 +02:00
if ( ! _match_object_type_query ( space - > intersection_query_results [ i ] , p_collision_layer , p_object_type_mask ) )
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 ) ) {
collided = p_result_max = = 0 | | 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 ;
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 ;
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 ( ) ;
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 ;
2014-02-23 00:28:19 +01:00
}
2014-02-19 15:57:14 +01:00
2017-06-13 17:45:01 +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_layer , uint32_t p_object_type_mask ) {
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 ;
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-06-13 17:45:01 +02:00
if ( ! _match_object_type_query ( space - > intersection_query_results [ i ] , p_collision_layer , p_object_type_mask ) )
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 ;
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
}
2017-03-05 16:44:50 +01:00
if ( rcd . best_len = = 0 )
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 ;
}
2017-03-05 16:44:50 +01:00
bool Space2DSW : : test_body_motion ( Body2DSW * p_body , const Transform2D & p_from , const Vector2 & p_motion , real_t p_margin , Physics2DServer : : MotionResult * r_result ) {
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 ;
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
2017-03-05 16:44:50 +01:00
if ( i = = 0 )
body_aabb = p_body - > get_shape_aabb ( i ) ;
2015-05-04 03:37:10 +02:00
else
2017-03-05 16:44:50 +01:00
body_aabb = body_aabb . merge ( p_body - > get_shape_aabb ( i ) ) ;
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-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 ;
cbk . ptr = sr ;
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
CollisionSolver2DSW : : CallbackResult cbkres = NULL ;
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
Physics2DServerSW : : CollCbkData * cbkptr = NULL ;
cbkptr = & cbk ;
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 ;
2017-01-11 04:52:51 +01:00
Transform2D body_shape_xform = body_transform * p_body - > get_shape_transform ( j ) ;
2015-04-20 01:50:55 +02:00
Shape2DSW * body_shape = p_body - > get_shape ( 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
2017-06-24 04:30:43 +02:00
if ( col_obj - > is_shape_set_as_one_way_collision ( j ) ) {
2015-05-11 00:24:09 +02:00
2017-06-24 04:30:43 +02:00
cbk . valid_dir = body_shape_xform . get_axis ( 1 ) . normalized ( ) ;
cbk . valid_depth = p_margin ; //only valid depth is the collision margin
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 ;
2015-04-20 01:50:55 +02:00
}
2017-03-05 16:44:50 +01:00
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 ) ) {
collided = cbk . amount > 0 ;
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 ] ;
2015-04-20 01:50:55 +02:00
2016-09-01 17:03:55 +02:00
#if 0
2016-08-31 22:58:51 +02:00
Vector2 rel = b - a ;
2017-02-14 00:25:05 +01:00
real_t d = rel . length ( ) ;
2016-08-31 22:58:51 +02:00
if ( d = = 0 )
continue ;
Vector2 n = rel / d ;
2017-02-14 00:25:05 +01:00
real_t traveled = n . dot ( recover_motion ) ;
2016-08-31 22:58:51 +02:00
a + = n * traveled ;
2017-02-14 00:25:05 +01:00
real_t d = a . distance_to ( b ) ;
2017-01-14 12:26:56 +01:00
if ( d < margin )
continue ;
2016-09-01 17:03:55 +02:00
# endif
2017-03-05 16:44:50 +01:00
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-03-05 16:44:50 +01:00
for ( int j = 0 ; j < p_body - > get_shape_count ( ) ; j + + ) {
2015-04-20 01:50:55 +02:00
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 ;
2017-01-11 04:52:51 +01:00
Transform2D body_shape_xform = body_transform * p_body - > get_shape_transform ( j ) ;
2015-04-20 01:50:55 +02:00
Shape2DSW * body_shape = p_body - > get_shape ( j ) ;
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 ] ;
int shape_idx = intersection_query_subindex_results [ i ] ;
2015-04-20 01:50:55 +02:00
2017-01-11 04:52:51 +01:00
Transform2D col_obj_xform = col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) ;
2015-04-20 01:50:55 +02:00
//test initial overlap, does it collide if going all the way?
2017-03-05 16:44:50 +01:00
if ( ! CollisionSolver2DSW : : solve ( body_shape , body_shape_xform , p_motion , col_obj - > get_shape ( shape_idx ) , col_obj_xform , Vector2 ( ) , NULL , NULL , NULL , 0 ) ) {
2015-04-20 01:50:55 +02:00
continue ;
}
//test initial overlap
2017-03-05 16:44:50 +01:00
if ( CollisionSolver2DSW : : solve ( body_shape , body_shape_xform , Vector2 ( ) , col_obj - > get_shape ( shape_idx ) , col_obj_xform , Vector2 ( ) , NULL , NULL , NULL , 0 ) ) {
2015-04-20 01:50:55 +02:00
2017-06-24 04:30:43 +02:00
if ( col_obj - > is_shape_set_as_one_way_collision ( j ) ) {
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
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 ) ;
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-03-05 16:44:50 +01:00
if ( col_obj - > get_type ( ) = = CollisionObject2DSW : : TYPE_BODY ) {
2015-04-20 01:50:55 +02:00
2017-03-05 16:44:50 +01:00
const Body2DSW * body = static_cast < const Body2DSW * > ( col_obj ) ;
2017-06-24 04:30:43 +02:00
if ( col_obj - > is_shape_set_as_one_way_collision ( j ) ) {
2015-04-20 01:50:55 +02:00
Vector2 cd [ 2 ] ;
Physics2DServerSW : : CollCbkData cbk ;
2017-03-05 16:44:50 +01:00
cbk . max = 1 ;
cbk . amount = 0 ;
cbk . ptr = cd ;
2017-06-24 04:30:43 +02:00
cbk . valid_dir = body_shape_xform . get_axis ( 1 ) . normalized ( ) ;
;
cbk . valid_depth = 10e20 ;
2017-03-05 16:44:50 +01:00
Vector2 sep = mnormal ; //important optimization for this to work fast enough
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 ) ;
if ( ! collided | | cbk . amount = = 0 ) {
2015-04-20 01:50:55 +02:00
continue ;
}
}
}
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 ;
best_shape = j ; //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 ;
best_shape = j ;
2015-04-20 01:50:55 +02:00
}
}
}
2017-03-05 16:44:50 +01:00
bool collided = false ;
if ( safe > = 1 ) {
2015-04-20 01:50:55 +02:00
//not collided
2017-03-05 16:44:50 +01:00
collided = false ;
2015-04-20 01:50:55 +02:00
if ( r_result ) {
2017-03-05 16:44:50 +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
}
} else {
//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 ;
2015-04-20 01:50:55 +02:00
2017-01-11 04:52:51 +01:00
Transform2D body_shape_xform = ugt * p_body - > get_shape_transform ( best_shape ) ;
2015-04-20 01:50:55 +02:00
Shape2DSW * body_shape = p_body - > get_shape ( best_shape ) ;
2017-06-04 00:25:13 +02:00
body_aabb . position + = p_motion * unsafe ;
2015-05-04 03:37:10 +02: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 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
2017-06-24 04:30:43 +02:00
if ( col_obj - > is_shape_set_as_one_way_collision ( shape_idx ) ) {
2015-04-20 01:50:55 +02:00
2017-06-24 04:30:43 +02:00
rcd . valid_dir = body_shape_xform . get_axis ( 1 ) . normalized ( ) ;
rcd . valid_depth = 10e20 ;
2015-04-20 01:50:55 +02:00
} else {
2017-03-05 16:44:50 +01:00
rcd . valid_dir = Vector2 ( ) ;
rcd . valid_depth = 0 ;
2015-04-20 01:50:55 +02:00
}
2017-03-05 16:44:50 +01:00
rcd . object = col_obj ;
rcd . shape = shape_idx ;
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 ) ;
2015-04-20 01:50:55 +02:00
if ( ! sc )
continue ;
}
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 ;
2017-06-24 17:33:52 +02:00
r_result - > collision_local_shape = best_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 ;
2015-04-20 01:50:55 +02:00
} else {
if ( r_result ) {
2017-03-05 16:44:50 +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
}
2017-03-05 16:44:50 +01:00
collided = false ;
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 ) {
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 ) {
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
b - > call_queries ( ) ;
state_query_list . remove ( state_query_list . first ( ) ) ;
}
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
a - > call_queries ( ) ;
monitor_query_list . remove ( monitor_query_list . first ( ) ) ;
}
}
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 ;
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 ;
}
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 ;
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 ) ;
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
}