2015-10-09 02:00:40 +08:00
/*************************************************************************/
/* space_sw.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
2017-08-27 20:16:55 +08:00
/* https://godotengine.org */
2015-10-09 02:00:40 +08:00
/*************************************************************************/
2019-01-01 19:53:14 +08:00
/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */
2015-10-09 02:00:40 +08: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 07:50:27 +08:00
2015-10-09 02:00:40 +08:00
# include "space_sw.h"
2017-08-28 03:07:15 +08:00
2015-10-09 02:00:40 +08:00
# include "collision_solver_sw.h"
2018-09-12 00:13:45 +08:00
# include "core/project_settings.h"
2015-10-09 02:00:40 +08:00
# include "physics_server_sw.h"
2018-08-22 02:30:41 +08:00
_FORCE_INLINE_ static bool _can_collide_with ( CollisionObjectSW * p_object , uint32_t p_collision_mask , bool p_collide_with_bodies , bool p_collide_with_areas ) {
2015-10-09 02:00:40 +08:00
2018-08-22 02:30:41 +08:00
if ( ! ( p_object - > get_collision_layer ( ) & p_collision_mask ) ) {
return false ;
}
if ( p_object - > get_type ( ) = = CollisionObjectSW : : TYPE_AREA & & ! p_collide_with_areas )
return false ;
if ( p_object - > get_type ( ) = = CollisionObjectSW : : TYPE_BODY & & ! p_collide_with_bodies )
return false ;
return true ;
2015-10-09 02:00:40 +08:00
}
2018-08-21 23:37:51 +08:00
int PhysicsDirectSpaceStateSW : : intersect_point ( const Vector3 & 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 ) {
2017-07-15 12:23:10 +08:00
ERR_FAIL_COND_V ( space - > locked , false ) ;
int amount = space - > broadphase - > cull_point ( p_point , space - > intersection_query_results , SpaceSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
int cc = 0 ;
//Transform ai = p_xform.affine_inverse();
for ( int i = 0 ; i < amount ; i + + ) {
if ( cc > = p_result_max )
break ;
2018-08-22 02:30:41 +08:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2017-07-15 12:23:10 +08:00
continue ;
//area can't be picked by ray (default)
if ( p_exclude . has ( space - > intersection_query_results [ i ] - > get_self ( ) ) )
continue ;
const CollisionObjectSW * col_obj = space - > intersection_query_results [ i ] ;
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
Transform inv_xform = col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) ;
inv_xform . affine_invert ( ) ;
if ( ! col_obj - > get_shape ( shape_idx ) - > intersect_point ( inv_xform . xform ( p_point ) ) )
continue ;
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 ) ;
else
r_results [ cc ] . collider = NULL ;
r_results [ cc ] . rid = col_obj - > get_self ( ) ;
r_results [ cc ] . shape = shape_idx ;
cc + + ;
}
return cc ;
}
2018-08-21 23:37:51 +08:00
bool PhysicsDirectSpaceStateSW : : intersect_ray ( const Vector3 & p_from , const Vector3 & 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 , bool p_pick_ray ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
ERR_FAIL_COND_V ( space - > locked , false ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
Vector3 begin , end ;
2015-10-09 02:00:40 +08:00
Vector3 normal ;
2017-03-05 23:44:50 +08:00
begin = p_from ;
end = p_to ;
normal = ( end - begin ) . normalized ( ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
int amount = space - > broadphase - > cull_segment ( begin , end , space - > intersection_query_results , SpaceSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
2015-10-09 02:00:40 +08:00
2018-09-13 09:38:39 +08: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
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
bool collided = false ;
Vector3 res_point , res_normal ;
2015-10-09 02:00:40 +08:00
int res_shape ;
const CollisionObjectSW * res_obj ;
2017-03-05 23:44:50 +08:00
real_t min_d = 1e10 ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
for ( int i = 0 ; i < amount ; i + + ) {
2015-10-09 02:00:40 +08:00
2018-08-22 02:30:41 +08:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2015-10-09 02:00:40 +08:00
continue ;
2019-06-26 21:08:25 +08:00
if ( p_pick_ray & & ! ( space - > intersection_query_results [ i ] - > is_ray_pickable ( ) ) )
2015-10-09 02:00:40 +08:00
continue ;
2017-03-05 23:44:50 +08:00
if ( p_exclude . has ( space - > intersection_query_results [ i ] - > get_self ( ) ) )
2015-10-09 02:00:40 +08:00
continue ;
2017-03-05 23:44:50 +08:00
const CollisionObjectSW * col_obj = space - > intersection_query_results [ i ] ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
2015-10-09 02:00:40 +08:00
Transform inv_xform = col_obj - > get_shape_inv_transform ( shape_idx ) * col_obj - > get_inv_transform ( ) ;
Vector3 local_from = inv_xform . xform ( begin ) ;
Vector3 local_to = inv_xform . xform ( end ) ;
const ShapeSW * shape = col_obj - > get_shape ( shape_idx ) ;
2017-03-05 23:44:50 +08:00
Vector3 shape_point , shape_normal ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
if ( shape - > intersect_segment ( local_from , local_to , shape_point , shape_normal ) ) {
2015-10-09 02:00:40 +08:00
Transform xform = col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) ;
2017-03-05 23:44:50 +08:00
shape_point = xform . xform ( shape_point ) ;
2015-10-09 02:00:40 +08:00
real_t ld = normal . dot ( shape_point ) ;
2017-03-05 23:44:50 +08:00
if ( ld < min_d ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08: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 ;
2015-10-09 02:00:40 +08:00
}
}
}
if ( ! collided )
return false ;
2017-03-05 23:44:50 +08: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 ) ;
2015-10-09 02:00:40 +08:00
else
2017-03-05 23:44:50 +08:00
r_result . collider = NULL ;
r_result . normal = res_normal ;
r_result . position = res_point ;
r_result . rid = res_obj - > get_self ( ) ;
r_result . shape = res_shape ;
2015-10-09 02:00:40 +08:00
return true ;
}
2018-08-21 23:37:51 +08:00
int PhysicsDirectSpaceStateSW : : intersect_shape ( const RID & p_shape , const Transform & p_xform , 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 ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
if ( p_result_max < = 0 )
2015-10-09 02:00:40 +08:00
return 0 ;
2017-03-05 23:44:50 +08:00
ShapeSW * shape = static_cast < PhysicsServerSW * > ( PhysicsServer : : get_singleton ( ) ) - > shape_owner . get ( p_shape ) ;
ERR_FAIL_COND_V ( ! shape , 0 ) ;
2015-10-09 02:00:40 +08:00
2017-11-17 10:09:00 +08:00
AABB aabb = p_xform . xform ( shape - > get_aabb ( ) ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
int amount = space - > broadphase - > cull_aabb ( aabb , space - > intersection_query_results , SpaceSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
int cc = 0 ;
2015-10-09 02:00:40 +08:00
//Transform ai = p_xform.affine_inverse();
2017-03-05 23:44:50 +08:00
for ( int i = 0 ; i < amount ; i + + ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
if ( cc > = p_result_max )
2015-10-09 02:00:40 +08:00
break ;
2018-08-22 02:30:41 +08:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2015-10-09 02:00:40 +08:00
continue ;
2017-03-25 04:45:31 +08:00
//area can't be picked by ray (default)
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
if ( p_exclude . has ( space - > intersection_query_results [ i ] - > get_self ( ) ) )
2015-10-09 02:00:40 +08:00
continue ;
2017-03-05 23:44:50 +08:00
const CollisionObjectSW * col_obj = space - > intersection_query_results [ i ] ;
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
if ( ! CollisionSolverSW : : solve_static ( shape , p_xform , col_obj - > get_shape ( shape_idx ) , col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) , NULL , NULL , NULL , p_margin , 0 ) )
2015-10-09 02:00:40 +08:00
continue ;
2015-12-04 16:31:01 +08:00
if ( r_results ) {
2017-03-05 23:44:50 +08: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 ) ;
2015-12-04 16:31:01 +08:00
else
2017-03-05 23:44:50 +08:00
r_results [ cc ] . collider = NULL ;
r_results [ cc ] . rid = col_obj - > get_self ( ) ;
r_results [ cc ] . shape = shape_idx ;
2015-12-04 16:31:01 +08:00
}
2015-11-22 21:14:07 +08:00
2015-12-04 16:31:01 +08:00
cc + + ;
2015-10-09 02:00:40 +08:00
}
return cc ;
}
2018-08-21 23:37:51 +08:00
bool PhysicsDirectSpaceStateSW : : cast_motion ( const RID & p_shape , const Transform & p_xform , const Vector3 & 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 , ShapeRestInfo * r_info ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
ShapeSW * shape = static_cast < PhysicsServerSW * > ( PhysicsServer : : get_singleton ( ) ) - > shape_owner . get ( p_shape ) ;
ERR_FAIL_COND_V ( ! shape , false ) ;
2015-10-09 02:00:40 +08:00
2017-11-17 10:09:00 +08:00
AABB aabb = p_xform . xform ( shape - > get_aabb ( ) ) ;
aabb = aabb . merge ( AABB ( aabb . position + p_motion , aabb . size ) ) ; //motion
2017-03-05 23:44:50 +08:00
aabb = aabb . grow ( p_margin ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
int amount = space - > broadphase - > cull_aabb ( aabb , space - > intersection_query_results , SpaceSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
real_t best_safe = 1 ;
real_t best_unsafe = 1 ;
2015-10-09 02:00:40 +08:00
Transform xform_inv = p_xform . affine_inverse ( ) ;
MotionShapeSW mshape ;
2017-03-05 23:44:50 +08:00
mshape . shape = shape ;
mshape . motion = xform_inv . basis . xform ( p_motion ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
bool best_first = true ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
Vector3 closest_A , closest_B ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
for ( int i = 0 ; i < amount ; i + + ) {
2015-10-09 02:00:40 +08:00
2018-08-22 02:30:41 +08:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2015-10-09 02:00:40 +08:00
continue ;
2017-03-05 23:44:50 +08:00
if ( p_exclude . has ( space - > intersection_query_results [ i ] - > get_self ( ) ) )
2015-10-09 02:00:40 +08:00
continue ; //ignore excluded
2017-03-05 23:44:50 +08:00
const CollisionObjectSW * col_obj = space - > intersection_query_results [ i ] ;
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
Vector3 point_A , point_B ;
Vector3 sep_axis = p_motion . normalized ( ) ;
2015-10-09 02:00:40 +08:00
Transform col_obj_xform = col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) ;
//test initial overlap, does it collide if going all the way?
2017-03-05 23:44:50 +08:00
if ( CollisionSolverSW : : solve_distance ( & mshape , p_xform , col_obj - > get_shape ( shape_idx ) , col_obj_xform , point_A , point_B , aabb , & sep_axis ) ) {
2015-10-09 02:00:40 +08:00
continue ;
}
2017-08-28 03:07:15 +08:00
//test initial overlap
2017-03-05 23:44:50 +08:00
sep_axis = p_motion . normalized ( ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
if ( ! CollisionSolverSW : : solve_distance ( shape , p_xform , col_obj - > get_shape ( shape_idx ) , col_obj_xform , point_A , point_B , aabb , & sep_axis ) ) {
2015-10-09 02:00:40 +08:00
return false ;
}
//just do kinematic solving
2017-03-05 23:44:50 +08:00
real_t low = 0 ;
real_t hi = 1 ;
Vector3 mnormal = p_motion . normalized ( ) ;
2015-10-09 02:00:40 +08:00
2019-02-13 04:10:08 +08:00
for ( int j = 0 ; j < 8 ; j + + ) { //steps should be customizable..
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
real_t ofs = ( low + hi ) * 0.5 ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
Vector3 sep = mnormal ; //important optimization for this to work fast enough
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
mshape . motion = xform_inv . basis . xform ( p_motion * ofs ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
Vector3 lA , lB ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
bool collided = ! CollisionSolverSW : : solve_distance ( & mshape , p_xform , col_obj - > get_shape ( shape_idx ) , col_obj_xform , lA , lB , aabb , & sep ) ;
2015-10-09 02:00:40 +08:00
if ( collided ) {
2017-03-05 23:44:50 +08:00
hi = ofs ;
2015-10-09 02:00:40 +08:00
} else {
2017-03-05 23:44:50 +08:00
point_A = lA ;
point_B = lB ;
low = ofs ;
2015-10-09 02:00:40 +08:00
}
}
2017-03-05 23:44:50 +08:00
if ( low < best_safe ) {
best_first = true ; //force reset
best_safe = low ;
best_unsafe = hi ;
2015-10-09 02:00:40 +08:00
}
2017-03-05 23:44:50 +08:00
if ( r_info & & ( best_first | | ( point_A . distance_squared_to ( point_B ) < closest_A . distance_squared_to ( closest_B ) & & low < = best_safe ) ) ) {
closest_A = point_A ;
closest_B = point_B ;
r_info - > collider_id = col_obj - > get_instance_id ( ) ;
r_info - > rid = col_obj - > get_self ( ) ;
r_info - > shape = shape_idx ;
r_info - > point = closest_B ;
r_info - > normal = ( closest_A - closest_B ) . normalized ( ) ;
best_first = false ;
if ( col_obj - > get_type ( ) = = CollisionObjectSW : : TYPE_BODY ) {
const BodySW * body = static_cast < const BodySW * > ( col_obj ) ;
r_info - > linear_velocity = body - > get_linear_velocity ( ) + ( body - > get_angular_velocity ( ) ) . cross ( body - > get_transform ( ) . origin - closest_B ) ;
2015-10-09 02:00:40 +08:00
}
}
}
2017-03-05 23:44:50 +08:00
p_closest_safe = best_safe ;
p_closest_unsafe = best_unsafe ;
2015-10-09 02:00:40 +08:00
return true ;
}
2018-08-21 23:37:51 +08:00
bool PhysicsDirectSpaceStateSW : : collide_shape ( RID p_shape , const Transform & p_shape_xform , real_t p_margin , Vector3 * 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 ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
if ( p_result_max < = 0 )
2015-10-09 02:00:40 +08:00
return 0 ;
2017-03-05 23:44:50 +08:00
ShapeSW * shape = static_cast < PhysicsServerSW * > ( PhysicsServer : : get_singleton ( ) ) - > shape_owner . get ( p_shape ) ;
ERR_FAIL_COND_V ( ! shape , 0 ) ;
2015-10-09 02:00:40 +08:00
2017-11-17 10:09:00 +08:00
AABB aabb = p_shape_xform . xform ( shape - > get_aabb ( ) ) ;
2017-03-05 23:44:50 +08:00
aabb = aabb . grow ( p_margin ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
int amount = space - > broadphase - > cull_aabb ( aabb , space - > intersection_query_results , SpaceSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
bool collided = false ;
r_result_count = 0 ;
2015-10-09 02:00:40 +08:00
PhysicsServerSW : : CollCbkData cbk ;
2017-03-05 23:44:50 +08:00
cbk . max = p_result_max ;
cbk . amount = 0 ;
cbk . ptr = r_results ;
2019-08-07 18:54:30 +08:00
CollisionSolverSW : : CallbackResult cbkres = PhysicsServerSW : : _shape_col_cbk ;
2017-03-05 23:44:50 +08:00
2019-08-07 18:54:30 +08:00
PhysicsServerSW : : CollCbkData * cbkptr = & cbk ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
for ( int i = 0 ; i < amount ; i + + ) {
2015-10-09 02:00:40 +08:00
2018-08-22 02:30:41 +08:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2015-10-09 02:00:40 +08:00
continue ;
2017-03-05 23:44:50 +08:00
const CollisionObjectSW * col_obj = space - > intersection_query_results [ i ] ;
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
if ( p_exclude . has ( col_obj - > get_self ( ) ) ) {
2015-10-09 02:00:40 +08:00
continue ;
}
2017-03-05 23:44:50 +08:00
if ( CollisionSolverSW : : solve_static ( shape , p_shape_xform , col_obj - > get_shape ( shape_idx ) , col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) , cbkres , cbkptr , NULL , p_margin ) ) {
collided = true ;
2015-10-09 02:00:40 +08:00
}
}
2017-03-05 23:44:50 +08:00
r_result_count = cbk . amount ;
2015-10-09 02:00:40 +08:00
return collided ;
}
struct _RestCallbackData {
const CollisionObjectSW * object ;
const CollisionObjectSW * best_object ;
int shape ;
int best_shape ;
Vector3 best_contact ;
Vector3 best_normal ;
2017-02-14 07:25:05 +08:00
real_t best_len ;
2019-02-17 00:45:01 +08:00
real_t min_allowed_depth ;
2015-10-09 02:00:40 +08:00
} ;
2017-03-05 23:44:50 +08:00
static void _rest_cbk_result ( const Vector3 & p_point_A , const Vector3 & p_point_B , void * p_userdata ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
_RestCallbackData * rd = ( _RestCallbackData * ) p_userdata ;
2015-10-09 02:00:40 +08:00
Vector3 contact_rel = p_point_B - p_point_A ;
2017-02-14 07:25:05 +08:00
real_t len = contact_rel . length ( ) ;
2019-02-17 00:45:01 +08:00
if ( len < rd - > min_allowed_depth )
return ;
2015-10-09 02:00:40 +08:00
if ( len < = rd - > best_len )
return ;
2017-03-05 23:44:50 +08: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 ;
2015-10-09 02:00:40 +08:00
}
2018-08-21 23:37:51 +08:00
bool PhysicsDirectSpaceStateSW : : rest_info ( RID p_shape , const Transform & p_shape_xform , 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 ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
ShapeSW * shape = static_cast < PhysicsServerSW * > ( PhysicsServer : : get_singleton ( ) ) - > shape_owner . get ( p_shape ) ;
ERR_FAIL_COND_V ( ! shape , 0 ) ;
2015-10-09 02:00:40 +08:00
2017-11-17 10:09:00 +08:00
AABB aabb = p_shape_xform . xform ( shape - > get_aabb ( ) ) ;
2017-03-05 23:44:50 +08:00
aabb = aabb . grow ( p_margin ) ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
int amount = space - > broadphase - > cull_aabb ( aabb , space - > intersection_query_results , SpaceSW : : INTERSECTION_QUERY_MAX , space - > intersection_query_subindex_results ) ;
2015-10-09 02:00:40 +08:00
_RestCallbackData rcd ;
2017-03-05 23:44:50 +08:00
rcd . best_len = 0 ;
rcd . best_object = NULL ;
rcd . best_shape = 0 ;
2019-02-17 00:45:01 +08:00
rcd . min_allowed_depth = space - > test_motion_min_contact_depth ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
for ( int i = 0 ; i < amount ; i + + ) {
2015-10-09 02:00:40 +08:00
2018-08-22 02:30:41 +08:00
if ( ! _can_collide_with ( space - > intersection_query_results [ i ] , p_collision_mask , p_collide_with_bodies , p_collide_with_areas ) )
2015-10-09 02:00:40 +08:00
continue ;
2017-03-05 23:44:50 +08:00
const CollisionObjectSW * col_obj = space - > intersection_query_results [ i ] ;
int shape_idx = space - > intersection_query_subindex_results [ i ] ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
if ( p_exclude . has ( col_obj - > get_self ( ) ) )
2015-10-09 02:00:40 +08:00
continue ;
2017-03-05 23:44:50 +08:00
rcd . object = col_obj ;
rcd . shape = shape_idx ;
bool sc = CollisionSolverSW : : solve_static ( shape , p_shape_xform , col_obj - > get_shape ( shape_idx ) , col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) , _rest_cbk_result , & rcd , NULL , p_margin ) ;
2015-10-09 02:00:40 +08:00
if ( ! sc )
continue ;
}
2019-06-26 21:08:25 +08:00
if ( rcd . best_len = = 0 | | ! rcd . best_object )
2015-10-09 02:00:40 +08:00
return false ;
2017-03-05 23:44:50 +08: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 ( ) ;
if ( rcd . best_object - > get_type ( ) = = CollisionObjectSW : : TYPE_BODY ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
const BodySW * body = static_cast < const BodySW * > ( rcd . best_object ) ;
2015-10-09 02:00:40 +08:00
r_info - > linear_velocity = body - > get_linear_velocity ( ) +
2017-03-05 23:44:50 +08:00
( body - > get_angular_velocity ( ) ) . cross ( body - > get_transform ( ) . origin - rcd . best_contact ) ; // * mPos);
2015-10-09 02:00:40 +08:00
} else {
2017-03-05 23:44:50 +08:00
r_info - > linear_velocity = Vector3 ( ) ;
2015-10-09 02:00:40 +08:00
}
return true ;
}
2017-07-15 12:23:10 +08:00
Vector3 PhysicsDirectSpaceStateSW : : get_closest_point_to_object_volume ( RID p_object , const Vector3 p_point ) const {
2017-08-22 03:15:36 +08:00
CollisionObjectSW * obj = PhysicsServerSW : : singleton - > area_owner . getornull ( p_object ) ;
2017-07-15 12:23:10 +08:00
if ( ! obj ) {
obj = PhysicsServerSW : : singleton - > body_owner . getornull ( p_object ) ;
}
ERR_FAIL_COND_V ( ! obj , Vector3 ( ) ) ;
ERR_FAIL_COND_V ( obj - > get_space ( ) ! = space , Vector3 ( ) ) ;
float min_distance = 1e20 ;
Vector3 min_point ;
bool shapes_found = false ;
for ( int i = 0 ; i < obj - > get_shape_count ( ) ; i + + ) {
if ( obj - > is_shape_set_as_disabled ( i ) )
continue ;
Transform shape_xform = obj - > get_transform ( ) * obj - > get_shape_transform ( i ) ;
ShapeSW * shape = obj - > get_shape ( i ) ;
Vector3 point = shape - > get_closest_point_to ( shape_xform . affine_inverse ( ) . xform ( p_point ) ) ;
point = shape_xform . xform ( point ) ;
float dist = point . distance_to ( p_point ) ;
if ( dist < min_distance ) {
min_distance = dist ;
min_point = point ;
}
shapes_found = true ;
}
if ( ! shapes_found ) {
return obj - > get_transform ( ) . origin ; //no shapes found, use distance to origin.
} else {
return min_point ;
}
}
2015-10-09 02:00:40 +08:00
PhysicsDirectSpaceStateSW : : PhysicsDirectSpaceStateSW ( ) {
2017-03-05 23:44:50 +08:00
space = NULL ;
2015-10-09 02:00:40 +08:00
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////
2017-11-17 10:09:00 +08:00
int SpaceSW : : _cull_aabb_for_body ( BodySW * p_body , const AABB & p_aabb ) {
2017-07-15 12:23:10 +08:00
int amount = broadphase - > cull_aabb ( p_aabb , intersection_query_results , INTERSECTION_QUERY_MAX , intersection_query_subindex_results ) ;
for ( int i = 0 ; i < amount ; i + + ) {
bool keep = true ;
if ( intersection_query_results [ i ] = = p_body )
keep = false ;
else if ( intersection_query_results [ i ] - > get_type ( ) = = CollisionObjectSW : : TYPE_AREA )
keep = false ;
else if ( ( static_cast < BodySW * > ( intersection_query_results [ i ] ) - > test_collision_mask ( p_body ) ) = = 0 )
keep = false ;
else if ( static_cast < BodySW * > ( intersection_query_results [ i ] ) - > has_exception ( p_body - > get_self ( ) ) | | p_body - > has_exception ( intersection_query_results [ i ] - > get_self ( ) ) )
keep = false ;
else if ( static_cast < BodySW * > ( intersection_query_results [ i ] ) - > is_shape_set_as_disabled ( intersection_query_subindex_results [ i ] ) )
keep = false ;
if ( ! keep ) {
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 ] ) ;
}
amount - - ;
i - - ;
}
}
return amount ;
}
2018-08-21 04:31:55 +08:00
int SpaceSW : : test_body_ray_separation ( BodySW * p_body , const Transform & p_transform , bool p_infinite_inertia , Vector3 & r_recover_motion , PhysicsServer : : SeparationResult * r_results , int p_result_max , real_t p_margin ) {
AABB body_aabb ;
2018-11-03 02:44:29 +08:00
bool shapes_found = false ;
2018-08-21 04:31:55 +08:00
for ( int i = 0 ; i < p_body - > get_shape_count ( ) ; i + + ) {
2018-11-03 02:44:29 +08:00
if ( p_body - > is_shape_set_as_disabled ( i ) )
continue ;
if ( ! shapes_found ) {
2018-08-21 04:31:55 +08:00
body_aabb = p_body - > get_shape_aabb ( i ) ;
2018-11-03 02:44:29 +08:00
shapes_found = true ;
} else {
2018-08-21 04:31:55 +08:00
body_aabb = body_aabb . merge ( p_body - > get_shape_aabb ( i ) ) ;
2018-11-03 02:44:29 +08:00
}
2018-08-21 04:31:55 +08:00
}
2018-11-03 02:44:29 +08:00
if ( ! shapes_found ) {
return 0 ;
}
2018-08-21 04:31:55 +08: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 ) ;
Transform 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 ;
Vector3 sr [ max_results * 2 ] ;
PhysicsServerSW : : CollCbkData cbk ;
cbk . max = max_results ;
PhysicsServerSW : : CollCbkData * cbkptr = & cbk ;
CollisionSolverSW : : CallbackResult cbkres = PhysicsServerSW : : _shape_col_cbk ;
do {
Vector3 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 ;
ShapeSW * body_shape = p_body - > get_shape ( j ) ;
if ( body_shape - > get_type ( ) ! = PhysicsServer : : SHAPE_RAY )
continue ;
Transform body_shape_xform = body_transform * p_body - > get_shape_transform ( j ) ;
for ( int i = 0 ; i < amount ; i + + ) {
const CollisionObjectSW * col_obj = intersection_query_results [ i ] ;
int shape_idx = intersection_query_subindex_results [ i ] ;
cbk . amount = 0 ;
cbk . ptr = sr ;
if ( CollisionObjectSW : : TYPE_BODY = = col_obj - > get_type ( ) ) {
const BodySW * b = static_cast < const BodySW * > ( col_obj ) ;
if ( p_infinite_inertia & & PhysicsServer : : BODY_MODE_STATIC ! = b - > get_mode ( ) & & PhysicsServer : : BODY_MODE_KINEMATIC ! = b - > get_mode ( ) ) {
continue ;
}
}
ShapeSW * against_shape = col_obj - > get_shape ( shape_idx ) ;
if ( CollisionSolverSW : : solve_static ( body_shape , body_shape_xform , against_shape , col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) , cbkres , cbkptr , NULL , p_margin ) ) {
if ( cbk . amount > 0 ) {
collided = true ;
}
2019-02-17 03:14:57 +08:00
int ray_index = - 1 ; //reuse shape
for ( int k = 0 ; k < rays_found ; k + + ) {
2019-03-26 05:46:26 +08:00
if ( r_results [ k ] . collision_local_shape = = j ) {
2019-02-17 03:14:57 +08:00
ray_index = k ;
}
}
if ( ray_index = = - 1 & & rays_found < p_result_max ) {
ray_index = rays_found ;
rays_found + + ;
}
if ( ray_index ! = - 1 ) {
2018-08-21 04:31:55 +08:00
PhysicsServer : : SeparationResult & result = r_results [ ray_index ] ;
for ( int k = 0 ; k < cbk . amount ; k + + ) {
Vector3 a = sr [ k * 2 + 0 ] ;
Vector3 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-17 03:14:57 +08:00
result . collision_local_shape = j ;
2018-08-21 04:31:55 +08:00
result . collider = col_obj - > get_self ( ) ;
result . collider_id = col_obj - > get_instance_id ( ) ;
2019-02-17 03:14:57 +08:00
result . collider_shape = shape_idx ;
2018-08-21 04:31:55 +08:00
//result.collider_metadata = col_obj->get_shape_metadata(shape_idx);
if ( col_obj - > get_type ( ) = = CollisionObjectSW : : TYPE_BODY ) {
BodySW * body = ( BodySW * ) col_obj ;
Vector3 rel_vec = b - body - > get_transform ( ) . get_origin ( ) ;
//result.collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
result . collider_velocity = body - > get_linear_velocity ( ) + ( body - > get_angular_velocity ( ) ) . cross ( body - > get_transform ( ) . origin - rel_vec ) ; // * mPos);
}
}
}
}
}
}
}
if ( ! collided | | recover_motion = = Vector3 ( ) ) {
break ;
}
body_transform . origin + = 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 . origin - p_transform . origin ;
return rays_found ;
}
bool SpaceSW : : test_body_motion ( BodySW * p_body , const Transform & p_from , const Vector3 & p_motion , bool p_infinite_inertia , real_t p_margin , PhysicsServer : : MotionResult * r_result , bool p_exclude_raycast_shapes ) {
2017-07-15 12:23:10 +08: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..
if ( r_result ) {
r_result - > collider_id = 0 ;
r_result - > collider_shape = 0 ;
}
2017-11-17 10:09:00 +08:00
AABB body_aabb ;
2018-11-03 02:44:29 +08:00
bool shapes_found = false ;
2017-07-15 12:23:10 +08:00
for ( int i = 0 ; i < p_body - > get_shape_count ( ) ; i + + ) {
2018-11-03 02:44:29 +08:00
if ( p_body - > is_shape_set_as_disabled ( i ) )
continue ;
if ( ! shapes_found ) {
2017-07-15 12:23:10 +08:00
body_aabb = p_body - > get_shape_aabb ( i ) ;
2018-11-03 02:44:29 +08:00
shapes_found = true ;
} else {
2017-07-15 12:23:10 +08:00
body_aabb = body_aabb . merge ( p_body - > get_shape_aabb ( i ) ) ;
2018-11-03 02:44:29 +08:00
}
}
if ( ! shapes_found ) {
2019-02-16 22:06:17 +08:00
if ( r_result ) {
* r_result = PhysicsServer : : MotionResult ( ) ;
r_result - > motion = p_motion ;
}
2018-11-03 02:44:29 +08:00
return false ;
2017-07-15 12:23:10 +08: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 ) ) ;
body_aabb = body_aabb . grow ( p_margin ) ;
Transform body_transform = p_from ;
{
//STEP 1, FREE BODY IF STUCK
const int max_results = 32 ;
int recover_attempts = 4 ;
Vector3 sr [ max_results * 2 ] ;
do {
PhysicsServerSW : : CollCbkData cbk ;
cbk . max = max_results ;
cbk . amount = 0 ;
cbk . ptr = sr ;
2017-08-22 03:15:36 +08:00
PhysicsServerSW : : CollCbkData * cbkptr = & cbk ;
CollisionSolverSW : : CallbackResult cbkres = PhysicsServerSW : : _shape_col_cbk ;
2017-07-15 12:23:10 +08:00
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 ;
Transform body_shape_xform = body_transform * p_body - > get_shape_transform ( j ) ;
ShapeSW * body_shape = p_body - > get_shape ( j ) ;
2018-08-21 04:31:55 +08:00
if ( p_exclude_raycast_shapes & & body_shape - > get_type ( ) = = PhysicsServer : : SHAPE_RAY ) {
continue ;
}
2017-07-15 12:23:10 +08:00
for ( int i = 0 ; i < amount ; i + + ) {
const CollisionObjectSW * col_obj = intersection_query_results [ i ] ;
int shape_idx = intersection_query_subindex_results [ i ] ;
if ( CollisionSolverSW : : solve_static ( body_shape , body_shape_xform , col_obj - > get_shape ( shape_idx ) , col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) , cbkres , cbkptr , NULL , p_margin ) ) {
collided = cbk . amount > 0 ;
}
}
}
if ( ! collided ) {
break ;
}
Vector3 recover_motion ;
for ( int i = 0 ; i < cbk . amount ; i + + ) {
Vector3 a = sr [ i * 2 + 0 ] ;
Vector3 b = sr [ i * 2 + 1 ] ;
recover_motion + = ( b - a ) * 0.4 ;
}
if ( recover_motion = = Vector3 ( ) ) {
collided = false ;
break ;
}
body_transform . origin + = recover_motion ;
body_aabb . position + = recover_motion ;
recover_attempts - - ;
} while ( recover_attempts ) ;
}
real_t safe = 1.0 ;
real_t unsafe = 1.0 ;
int best_shape = - 1 ;
{
// STEP 2 ATTEMPT MOTION
2017-11-17 10:09:00 +08:00
AABB motion_aabb = body_aabb ;
2017-07-15 12:23:10 +08:00
motion_aabb . position + = p_motion ;
motion_aabb = motion_aabb . merge ( body_aabb ) ;
int amount = _cull_aabb_for_body ( p_body , motion_aabb ) ;
for ( int j = 0 ; j < p_body - > get_shape_count ( ) ; j + + ) {
if ( p_body - > is_shape_set_as_disabled ( j ) )
continue ;
Transform body_shape_xform = body_transform * p_body - > get_shape_transform ( j ) ;
ShapeSW * body_shape = p_body - > get_shape ( j ) ;
2018-08-21 04:31:55 +08:00
if ( p_exclude_raycast_shapes & & body_shape - > get_type ( ) = = PhysicsServer : : SHAPE_RAY ) {
continue ;
}
2017-07-15 12:23:10 +08:00
Transform body_shape_xform_inv = body_shape_xform . affine_inverse ( ) ;
MotionShapeSW mshape ;
mshape . shape = body_shape ;
mshape . motion = body_shape_xform_inv . basis . xform ( p_motion ) ;
bool stuck = false ;
real_t best_safe = 1 ;
real_t best_unsafe = 1 ;
for ( int i = 0 ; i < amount ; i + + ) {
const CollisionObjectSW * col_obj = intersection_query_results [ i ] ;
int shape_idx = intersection_query_subindex_results [ i ] ;
//test initial overlap, does it collide if going all the way?
Vector3 point_A , point_B ;
Vector3 sep_axis = p_motion . normalized ( ) ;
Transform col_obj_xform = col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) ;
//test initial overlap, does it collide if going all the way?
if ( CollisionSolverSW : : solve_distance ( & mshape , body_shape_xform , col_obj - > get_shape ( shape_idx ) , col_obj_xform , point_A , point_B , motion_aabb , & sep_axis ) ) {
continue ;
}
sep_axis = p_motion . normalized ( ) ;
if ( ! CollisionSolverSW : : solve_distance ( body_shape , body_shape_xform , col_obj - > get_shape ( shape_idx ) , col_obj_xform , point_A , point_B , motion_aabb , & sep_axis ) ) {
stuck = true ;
break ;
}
//just do kinematic solving
real_t low = 0 ;
real_t hi = 1 ;
Vector3 mnormal = p_motion . normalized ( ) ;
2019-02-13 04:10:08 +08:00
for ( int k = 0 ; k < 8 ; k + + ) { //steps should be customizable..
2017-07-15 12:23:10 +08:00
real_t ofs = ( low + hi ) * 0.5 ;
Vector3 sep = mnormal ; //important optimization for this to work fast enough
mshape . motion = body_shape_xform_inv . basis . xform ( p_motion * ofs ) ;
Vector3 lA , lB ;
bool collided = ! CollisionSolverSW : : solve_distance ( & mshape , body_shape_xform , col_obj - > get_shape ( shape_idx ) , col_obj_xform , lA , lB , motion_aabb , & sep ) ;
if ( collided ) {
hi = ofs ;
} else {
point_A = lA ;
point_B = lB ;
low = ofs ;
}
}
if ( low < best_safe ) {
best_safe = low ;
best_unsafe = hi ;
}
}
if ( stuck ) {
safe = 0 ;
unsafe = 0 ;
best_shape = j ; //sadly it's the best
break ;
}
if ( best_safe = = 1.0 ) {
continue ;
}
if ( best_safe < safe ) {
safe = best_safe ;
unsafe = best_unsafe ;
best_shape = j ;
}
}
}
bool collided = false ;
if ( safe > = 1 ) {
//not collided
collided = false ;
if ( r_result ) {
r_result - > motion = p_motion ;
r_result - > remainder = Vector3 ( ) ;
r_result - > motion + = ( body_transform . get_origin ( ) - p_from . get_origin ( ) ) ;
}
} else {
//it collided, let's get the rest info in unsafe advance
Transform ugt = body_transform ;
ugt . origin + = p_motion * unsafe ;
_RestCallbackData rcd ;
rcd . best_len = 0 ;
rcd . best_object = NULL ;
rcd . best_shape = 0 ;
2019-02-17 00:45:01 +08:00
rcd . min_allowed_depth = test_motion_min_contact_depth ;
2017-07-15 12:23:10 +08:00
Transform body_shape_xform = ugt * p_body - > get_shape_transform ( best_shape ) ;
ShapeSW * body_shape = p_body - > get_shape ( best_shape ) ;
body_aabb . position + = p_motion * unsafe ;
int amount = _cull_aabb_for_body ( p_body , body_aabb ) ;
for ( int i = 0 ; i < amount ; i + + ) {
const CollisionObjectSW * col_obj = intersection_query_results [ i ] ;
int shape_idx = intersection_query_subindex_results [ i ] ;
rcd . object = col_obj ;
rcd . shape = shape_idx ;
bool sc = CollisionSolverSW : : solve_static ( body_shape , body_shape_xform , col_obj - > get_shape ( shape_idx ) , col_obj - > get_transform ( ) * col_obj - > get_shape_transform ( shape_idx ) , _rest_cbk_result , & rcd , NULL , p_margin ) ;
if ( ! sc )
continue ;
}
if ( rcd . best_len ! = 0 ) {
if ( r_result ) {
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 ;
r_result - > collision_local_shape = best_shape ;
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 BodySW * body = static_cast < const BodySW * > ( rcd . best_object ) ;
//Vector3 rel_vec = r_result->collision_point - body->get_transform().get_origin();
// r_result->collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
r_result - > collider_velocity = body - > get_linear_velocity ( ) + ( body - > get_angular_velocity ( ) ) . cross ( body - > get_transform ( ) . origin - rcd . best_contact ) ; // * mPos);
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 ( ) ) ;
}
collided = true ;
} else {
if ( r_result ) {
r_result - > motion = p_motion ;
r_result - > remainder = Vector3 ( ) ;
r_result - > motion + = ( body_transform . get_origin ( ) - p_from . get_origin ( ) ) ;
}
collided = false ;
}
}
return collided ;
}
2017-03-05 23:44:50 +08:00
void * SpaceSW : : _broadphase_pair ( CollisionObjectSW * A , int p_subindex_A , CollisionObjectSW * B , int p_subindex_B , void * p_self ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
CollisionObjectSW : : Type type_A = A - > get_type ( ) ;
CollisionObjectSW : : Type type_B = B - > get_type ( ) ;
if ( type_A > type_B ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
SWAP ( A , B ) ;
SWAP ( p_subindex_A , p_subindex_B ) ;
SWAP ( type_A , type_B ) ;
2015-10-09 02:00:40 +08:00
}
2017-03-05 23:44:50 +08:00
SpaceSW * self = ( SpaceSW * ) p_self ;
2015-10-09 02:00:40 +08:00
self - > collision_pairs + + ;
2017-03-05 23:44:50 +08:00
if ( type_A = = CollisionObjectSW : : TYPE_AREA ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
AreaSW * area = static_cast < AreaSW * > ( A ) ;
if ( type_B = = CollisionObjectSW : : TYPE_AREA ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
AreaSW * area_b = static_cast < AreaSW * > ( B ) ;
Area2PairSW * area2_pair = memnew ( Area2PairSW ( area_b , p_subindex_B , area , p_subindex_A ) ) ;
2015-10-09 02:00:40 +08:00
return area2_pair ;
} else {
2017-03-05 23:44:50 +08:00
BodySW * body = static_cast < BodySW * > ( B ) ;
AreaPairSW * area_pair = memnew ( AreaPairSW ( body , p_subindex_B , area , p_subindex_A ) ) ;
2015-10-09 02:00:40 +08:00
return area_pair ;
}
} else {
2017-03-05 23:44:50 +08:00
BodyPairSW * b = memnew ( BodyPairSW ( ( BodySW * ) A , p_subindex_A , ( BodySW * ) B , p_subindex_B ) ) ;
2015-10-09 02:00:40 +08:00
return b ;
}
return NULL ;
}
2017-03-05 23:44:50 +08:00
void SpaceSW : : _broadphase_unpair ( CollisionObjectSW * A , int p_subindex_A , CollisionObjectSW * B , int p_subindex_B , void * p_data , void * p_self ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
SpaceSW * self = ( SpaceSW * ) p_self ;
2015-10-09 02:00:40 +08:00
self - > collision_pairs - - ;
2017-03-05 23:44:50 +08:00
ConstraintSW * c = ( ConstraintSW * ) p_data ;
2015-10-09 02:00:40 +08:00
memdelete ( c ) ;
}
2017-03-05 23:44:50 +08:00
const SelfList < BodySW > : : List & SpaceSW : : get_active_body_list ( ) const {
2015-10-09 02:00:40 +08:00
return active_list ;
}
2017-03-05 23:44:50 +08:00
void SpaceSW : : body_add_to_active_list ( SelfList < BodySW > * p_body ) {
2015-10-09 02:00:40 +08:00
active_list . add ( p_body ) ;
}
2017-03-05 23:44:50 +08:00
void SpaceSW : : body_remove_from_active_list ( SelfList < BodySW > * p_body ) {
2015-10-09 02:00:40 +08:00
active_list . remove ( p_body ) ;
}
2017-03-05 23:44:50 +08:00
void SpaceSW : : body_add_to_inertia_update_list ( SelfList < BodySW > * p_body ) {
2015-10-09 02:00:40 +08:00
inertia_update_list . add ( p_body ) ;
}
2017-03-05 23:44:50 +08:00
void SpaceSW : : body_remove_from_inertia_update_list ( SelfList < BodySW > * p_body ) {
2015-10-09 02:00:40 +08:00
inertia_update_list . remove ( p_body ) ;
}
BroadPhaseSW * SpaceSW : : get_broadphase ( ) {
return broadphase ;
}
void SpaceSW : : add_object ( CollisionObjectSW * p_object ) {
2017-03-05 23:44:50 +08:00
ERR_FAIL_COND ( objects . has ( p_object ) ) ;
2015-10-09 02:00:40 +08:00
objects . insert ( p_object ) ;
}
void SpaceSW : : remove_object ( CollisionObjectSW * p_object ) {
2017-03-05 23:44:50 +08:00
ERR_FAIL_COND ( ! objects . has ( p_object ) ) ;
2015-10-09 02:00:40 +08:00
objects . erase ( p_object ) ;
}
2017-03-05 23:44:50 +08:00
const Set < CollisionObjectSW * > & SpaceSW : : get_objects ( ) const {
2015-10-09 02:00:40 +08:00
return objects ;
}
2017-03-05 23:44:50 +08:00
void SpaceSW : : body_add_to_state_query_list ( SelfList < BodySW > * p_body ) {
2015-10-09 02:00:40 +08:00
state_query_list . add ( p_body ) ;
}
2017-03-05 23:44:50 +08:00
void SpaceSW : : body_remove_from_state_query_list ( SelfList < BodySW > * p_body ) {
2015-10-09 02:00:40 +08:00
state_query_list . remove ( p_body ) ;
}
2017-03-05 23:44:50 +08:00
void SpaceSW : : area_add_to_monitor_query_list ( SelfList < AreaSW > * p_area ) {
2015-10-09 02:00:40 +08:00
monitor_query_list . add ( p_area ) ;
}
2017-03-05 23:44:50 +08:00
void SpaceSW : : area_remove_from_monitor_query_list ( SelfList < AreaSW > * p_area ) {
2015-10-09 02:00:40 +08:00
monitor_query_list . remove ( p_area ) ;
}
2017-03-05 23:44:50 +08:00
void SpaceSW : : area_add_to_moved_list ( SelfList < AreaSW > * p_area ) {
2015-10-09 02:00:40 +08:00
area_moved_list . add ( p_area ) ;
}
2017-03-05 23:44:50 +08:00
void SpaceSW : : area_remove_from_moved_list ( SelfList < AreaSW > * p_area ) {
2015-10-09 02:00:40 +08:00
area_moved_list . remove ( p_area ) ;
}
2017-03-05 23:44:50 +08:00
const SelfList < AreaSW > : : List & SpaceSW : : get_moved_area_list ( ) const {
2015-10-09 02:00:40 +08:00
return area_moved_list ;
}
void SpaceSW : : call_queries ( ) {
2017-03-05 23:44:50 +08:00
while ( state_query_list . first ( ) ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
BodySW * b = state_query_list . first ( ) - > self ( ) ;
2015-10-09 02:00:40 +08:00
state_query_list . remove ( state_query_list . first ( ) ) ;
2017-11-10 20:21:33 +08:00
b - > call_queries ( ) ;
2015-10-09 02:00:40 +08:00
}
2017-03-05 23:44:50 +08:00
while ( monitor_query_list . first ( ) ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
AreaSW * a = monitor_query_list . first ( ) - > self ( ) ;
2015-10-09 02:00:40 +08:00
monitor_query_list . remove ( monitor_query_list . first ( ) ) ;
2017-11-10 20:21:33 +08:00
a - > call_queries ( ) ;
2015-10-09 02:00:40 +08:00
}
}
void SpaceSW : : setup ( ) {
2017-03-05 23:44:50 +08:00
contact_debug_count = 0 ;
while ( inertia_update_list . first ( ) ) {
2015-10-09 02:00:40 +08:00
inertia_update_list . first ( ) - > self ( ) - > update_inertias ( ) ;
inertia_update_list . remove ( inertia_update_list . first ( ) ) ;
}
}
void SpaceSW : : update ( ) {
broadphase - > update ( ) ;
}
void SpaceSW : : set_param ( PhysicsServer : : SpaceParameter p_param , real_t p_value ) {
2017-03-05 23:44:50 +08:00
switch ( p_param ) {
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
case PhysicsServer : : SPACE_PARAM_CONTACT_RECYCLE_RADIUS : contact_recycle_radius = p_value ; break ;
case PhysicsServer : : SPACE_PARAM_CONTACT_MAX_SEPARATION : contact_max_separation = p_value ; break ;
case PhysicsServer : : SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION : contact_max_allowed_penetration = p_value ; break ;
2017-07-08 23:12:18 +08:00
case PhysicsServer : : SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD : body_linear_velocity_sleep_threshold = p_value ; break ;
case PhysicsServer : : SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD : body_angular_velocity_sleep_threshold = p_value ; break ;
2017-03-05 23:44:50 +08:00
case PhysicsServer : : SPACE_PARAM_BODY_TIME_TO_SLEEP : body_time_to_sleep = p_value ; break ;
case PhysicsServer : : SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO : body_angular_velocity_damp_ratio = p_value ; break ;
case PhysicsServer : : SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS : constraint_bias = p_value ; break ;
2019-02-17 00:45:01 +08:00
case PhysicsServer : : SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH : test_motion_min_contact_depth = p_value ; break ;
2015-10-09 02:00:40 +08:00
}
}
real_t SpaceSW : : get_param ( PhysicsServer : : SpaceParameter p_param ) const {
2017-03-05 23:44:50 +08:00
switch ( p_param ) {
2015-10-09 02:00:40 +08:00
case PhysicsServer : : SPACE_PARAM_CONTACT_RECYCLE_RADIUS : return contact_recycle_radius ;
case PhysicsServer : : SPACE_PARAM_CONTACT_MAX_SEPARATION : return contact_max_separation ;
case PhysicsServer : : SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION : return contact_max_allowed_penetration ;
2017-07-08 23:12:18 +08:00
case PhysicsServer : : SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD : return body_linear_velocity_sleep_threshold ;
case PhysicsServer : : SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD : return body_angular_velocity_sleep_threshold ;
2015-10-09 02:00:40 +08:00
case PhysicsServer : : SPACE_PARAM_BODY_TIME_TO_SLEEP : return body_time_to_sleep ;
case PhysicsServer : : SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO : return body_angular_velocity_damp_ratio ;
case PhysicsServer : : SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS : return constraint_bias ;
2019-02-17 00:45:01 +08:00
case PhysicsServer : : SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH : return test_motion_min_contact_depth ;
2015-10-09 02:00:40 +08:00
}
return 0 ;
}
void SpaceSW : : lock ( ) {
2017-03-05 23:44:50 +08:00
locked = true ;
2015-10-09 02:00:40 +08:00
}
void SpaceSW : : unlock ( ) {
2017-03-05 23:44:50 +08:00
locked = false ;
2015-10-09 02:00:40 +08:00
}
bool SpaceSW : : is_locked ( ) const {
return locked ;
}
PhysicsDirectSpaceStateSW * SpaceSW : : get_direct_state ( ) {
return direct_access ;
}
SpaceSW : : SpaceSW ( ) {
2017-03-05 23:44:50 +08:00
collision_pairs = 0 ;
active_objects = 0 ;
island_count = 0 ;
contact_debug_count = 0 ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
locked = false ;
contact_recycle_radius = 0.01 ;
contact_max_separation = 0.05 ;
contact_max_allowed_penetration = 0.01 ;
2019-02-17 00:45:01 +08:00
test_motion_min_contact_depth = 0.00001 ;
2015-10-09 02:00:40 +08:00
constraint_bias = 0.01 ;
2017-03-05 23:44:50 +08:00
body_linear_velocity_sleep_threshold = GLOBAL_DEF ( " physics/3d/sleep_threshold_linear " , 0.1 ) ;
body_angular_velocity_sleep_threshold = GLOBAL_DEF ( " physics/3d/sleep_threshold_angular " , ( 8.0 / 180.0 * Math_PI ) ) ;
body_time_to_sleep = GLOBAL_DEF ( " physics/3d/time_before_sleep " , 0.5 ) ;
2018-10-06 00:43:53 +08:00
ProjectSettings : : get_singleton ( ) - > set_custom_property_info ( " physics/3d/time_before_sleep " , PropertyInfo ( Variant : : REAL , " physics/3d/time_before_sleep " , PROPERTY_HINT_RANGE , " 0,5,0.01,or_greater " ) ) ;
2017-03-05 23:44:50 +08:00
body_angular_velocity_damp_ratio = 10 ;
2015-10-09 02:00:40 +08:00
broadphase = BroadPhaseSW : : create_func ( ) ;
2017-03-05 23:44:50 +08:00
broadphase - > set_pair_callback ( _broadphase_pair , this ) ;
broadphase - > set_unpair_callback ( _broadphase_unpair , this ) ;
area = NULL ;
2015-10-09 02:00:40 +08:00
2017-03-05 23:44:50 +08:00
direct_access = memnew ( PhysicsDirectSpaceStateSW ) ;
direct_access - > space = this ;
2016-05-22 08:18:16 +08:00
2017-03-05 23:44:50 +08:00
for ( int i = 0 ; i < ELAPSED_TIME_MAX ; i + + )
elapsed_time [ i ] = 0 ;
2015-10-09 02:00:40 +08:00
}
SpaceSW : : ~ SpaceSW ( ) {
memdelete ( broadphase ) ;
2017-03-05 23:44:50 +08:00
memdelete ( direct_access ) ;
2015-10-09 02:00:40 +08:00
}