2017-11-04 19:52:59 +00:00
/*************************************************************************/
/* space_bullet.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
2018-01-04 23:50:27 +00:00
/* https://godotengine.org */
2017-11-04 19:52:59 +00:00
/*************************************************************************/
2021-01-01 19:13:46 +00:00
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
2017-11-04 19:52:59 +00: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. */
/*************************************************************************/
# include "space_bullet.h"
2018-01-04 23:50:27 +00:00
2017-11-04 19:52:59 +00:00
# include "bullet_physics_server.h"
# include "bullet_types_converter.h"
# include "bullet_utilities.h"
# include "constraint_bullet.h"
2020-11-07 22:33:38 +00:00
# include "core/config/project_settings.h"
# include "core/string/ustring.h"
2017-11-04 19:52:59 +00:00
# include "godot_collision_configuration.h"
# include "godot_collision_dispatcher.h"
# include "rigid_body_bullet.h"
2020-03-27 18:21:27 +00:00
# include "servers/physics_server_3d.h"
2017-11-04 19:52:59 +00:00
# include "soft_body_bullet.h"
2018-01-04 23:50:27 +00:00
2019-10-08 09:35:35 +00:00
# include <BulletCollision/BroadphaseCollision/btBroadphaseProxy.h>
2018-01-04 23:50:27 +00:00
# include <BulletCollision/CollisionDispatch/btCollisionObject.h>
# include <BulletCollision/CollisionDispatch/btGhostObject.h>
# include <BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h>
# include <BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h>
# include <BulletCollision/NarrowPhaseCollision/btPointCollector.h>
# include <BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h>
# include <BulletSoftBody/btSoftRigidDynamicsWorld.h>
# include <btBulletDynamicsCommon.h>
2017-11-04 19:52:59 +00:00
# include <assert.h>
2018-01-04 23:50:27 +00:00
/**
@ author AndreaCatania
*/
2017-12-06 20:36:34 +00:00
BulletPhysicsDirectSpaceState : : BulletPhysicsDirectSpaceState ( SpaceBullet * p_space ) :
2020-03-27 18:21:27 +00:00
PhysicsDirectSpaceState3D ( ) ,
2017-12-06 20:36:34 +00:00
space ( p_space ) { }
2017-11-04 19:52:59 +00:00
2018-08-21 15:37:51 +00:00
int BulletPhysicsDirectSpaceState : : 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 ) {
2020-05-14 14:41:43 +00:00
if ( p_result_max < = 0 ) {
2017-11-04 19:52:59 +00:00
return 0 ;
2020-05-14 14:41:43 +00:00
}
2017-11-04 19:52:59 +00:00
btVector3 bt_point ;
G_TO_B ( p_point , bt_point ) ;
2018-08-21 15:37:51 +00:00
btSphereShape sphere_point ( 0.001f ) ;
2017-11-04 19:52:59 +00:00
btCollisionObject collision_object_point ;
collision_object_point . setCollisionShape ( & sphere_point ) ;
collision_object_point . setWorldTransform ( btTransform ( btQuaternion : : getIdentity ( ) , bt_point ) ) ;
// Setup query
2018-08-21 15:37:51 +00:00
GodotAllContactResultCallback btResult ( & collision_object_point , r_results , p_result_max , & p_exclude , p_collide_with_bodies , p_collide_with_areas ) ;
2017-11-21 21:56:40 +00:00
btResult . m_collisionFilterGroup = 0 ;
btResult . m_collisionFilterMask = p_collision_mask ;
2017-11-04 19:52:59 +00:00
space - > dynamicsWorld - > contactTest ( & collision_object_point , btResult ) ;
2021-03-12 13:35:16 +00:00
// The results are already populated by GodotAllConvexResultCallback
2017-11-04 19:52:59 +00:00
return btResult . m_count ;
}
2018-08-21 15:37:51 +00:00
bool BulletPhysicsDirectSpaceState : : 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 ) {
2017-11-04 19:52:59 +00:00
btVector3 btVec_from ;
btVector3 btVec_to ;
G_TO_B ( p_from , btVec_from ) ;
G_TO_B ( p_to , btVec_to ) ;
// setup query
2018-08-21 15:37:51 +00:00
GodotClosestRayResultCallback btResult ( btVec_from , btVec_to , & p_exclude , p_collide_with_bodies , p_collide_with_areas ) ;
2017-11-21 21:56:40 +00:00
btResult . m_collisionFilterGroup = 0 ;
btResult . m_collisionFilterMask = p_collision_mask ;
2017-11-04 19:52:59 +00:00
btResult . m_pickRay = p_pick_ray ;
space - > dynamicsWorld - > rayTest ( btVec_from , btVec_to , btResult ) ;
if ( btResult . hasHit ( ) ) {
B_TO_G ( btResult . m_hitPointWorld , r_result . position ) ;
B_TO_G ( btResult . m_hitNormalWorld . normalize ( ) , r_result . normal ) ;
CollisionObjectBullet * gObj = static_cast < CollisionObjectBullet * > ( btResult . m_collisionObject - > getUserPointer ( ) ) ;
if ( gObj ) {
2017-12-10 11:20:36 +00:00
r_result . shape = btResult . m_shapeId ;
2017-11-04 19:52:59 +00:00
r_result . rid = gObj - > get_self ( ) ;
r_result . collider_id = gObj - > get_instance_id ( ) ;
2020-04-01 23:20:12 +00:00
r_result . collider = r_result . collider_id . is_null ( ) ? nullptr : ObjectDB : : get_instance ( r_result . collider_id ) ;
2017-11-04 19:52:59 +00:00
} else {
2019-11-07 08:44:15 +00:00
WARN_PRINT ( " The raycast performed has hit a collision object that is not part of Godot scene, please check it. " ) ;
2017-11-04 19:52:59 +00:00
}
return true ;
} else {
return false ;
}
}
2020-10-17 05:08:21 +00:00
int BulletPhysicsDirectSpaceState : : intersect_shape ( const RID & p_shape , const Transform3D & 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 ) {
2020-05-14 14:41:43 +00:00
if ( p_result_max < = 0 ) {
2017-11-04 19:52:59 +00:00
return 0 ;
2020-05-14 14:41:43 +00:00
}
2017-11-04 19:52:59 +00:00
2021-09-29 17:08:41 +00:00
ShapeBullet * shape = space - > get_physics_server ( ) - > get_shape_owner ( ) - > get_or_null ( p_shape ) ;
2020-06-22 08:22:11 +00:00
ERR_FAIL_COND_V ( ! shape , 0 ) ;
2017-11-04 19:52:59 +00:00
2018-04-14 19:53:25 +00:00
btCollisionShape * btShape = shape - > create_bt_shape ( p_xform . basis . get_scale_abs ( ) , p_margin ) ;
2017-11-19 16:11:47 +00:00
if ( ! btShape - > isConvex ( ) ) {
2020-10-08 10:22:59 +00:00
bulletdelete ( btShape ) ;
2019-11-06 16:03:04 +00:00
ERR_PRINT ( " The shape is not a convex shape, then is not supported: shape type: " + itos ( shape - > get_type ( ) ) ) ;
2017-11-04 19:52:59 +00:00
return 0 ;
}
2017-11-19 16:11:47 +00:00
btConvexShape * btConvex = static_cast < btConvexShape * > ( btShape ) ;
2017-11-04 19:52:59 +00:00
btTransform bt_xform ;
G_TO_B ( p_xform , bt_xform ) ;
2017-12-23 17:23:12 +00:00
UNSCALE_BT_BASIS ( bt_xform ) ;
2017-11-04 19:52:59 +00:00
btCollisionObject collision_object ;
collision_object . setCollisionShape ( btConvex ) ;
collision_object . setWorldTransform ( bt_xform ) ;
2018-08-21 15:37:51 +00:00
GodotAllContactResultCallback btQuery ( & collision_object , r_results , p_result_max , & p_exclude , p_collide_with_bodies , p_collide_with_areas ) ;
2017-11-21 21:56:40 +00:00
btQuery . m_collisionFilterGroup = 0 ;
btQuery . m_collisionFilterMask = p_collision_mask ;
2017-12-23 17:23:12 +00:00
btQuery . m_closestDistanceThreshold = 0 ;
2017-11-04 19:52:59 +00:00
space - > dynamicsWorld - > contactTest ( & collision_object , btQuery ) ;
2020-10-08 10:22:59 +00:00
bulletdelete ( btConvex ) ;
2017-11-04 19:52:59 +00:00
return btQuery . m_count ;
}
2020-10-17 05:08:21 +00:00
bool BulletPhysicsDirectSpaceState : : cast_motion ( const RID & p_shape , const Transform3D & p_xform , const Vector3 & p_motion , real_t p_margin , real_t & r_closest_safe , real_t & r_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 ) {
2019-12-09 10:11:06 +00:00
r_closest_safe = 0.0f ;
r_closest_unsafe = 0.0f ;
btVector3 bt_motion ;
G_TO_B ( p_motion , bt_motion ) ;
2021-09-29 17:08:41 +00:00
ShapeBullet * shape = space - > get_physics_server ( ) - > get_shape_owner ( ) - > get_or_null ( p_shape ) ;
2020-06-22 08:22:11 +00:00
ERR_FAIL_COND_V ( ! shape , false ) ;
2017-11-04 19:52:59 +00:00
2017-12-23 17:23:12 +00:00
btCollisionShape * btShape = shape - > create_bt_shape ( p_xform . basis . get_scale ( ) , p_margin ) ;
2017-11-19 16:11:47 +00:00
if ( ! btShape - > isConvex ( ) ) {
2020-10-08 10:22:59 +00:00
bulletdelete ( btShape ) ;
2019-11-06 16:03:04 +00:00
ERR_PRINT ( " The shape is not a convex shape, then is not supported: shape type: " + itos ( shape - > get_type ( ) ) ) ;
2018-09-22 09:17:31 +00:00
return false ;
2017-11-04 19:52:59 +00:00
}
2017-11-19 16:11:47 +00:00
btConvexShape * bt_convex_shape = static_cast < btConvexShape * > ( btShape ) ;
2017-11-04 19:52:59 +00:00
btTransform bt_xform_from ;
G_TO_B ( p_xform , bt_xform_from ) ;
2017-12-23 17:23:12 +00:00
UNSCALE_BT_BASIS ( bt_xform_from ) ;
2017-11-04 19:52:59 +00:00
btTransform bt_xform_to ( bt_xform_from ) ;
bt_xform_to . getOrigin ( ) + = bt_motion ;
2020-07-18 12:10:27 +00:00
if ( ( bt_xform_to . getOrigin ( ) - bt_xform_from . getOrigin ( ) ) . fuzzyZero ( ) ) {
2020-11-27 14:33:10 +00:00
r_closest_safe = 1.0f ;
r_closest_unsafe = 1.0f ;
2020-10-08 11:10:48 +00:00
bulletdelete ( btShape ) ;
2020-11-27 14:33:10 +00:00
return true ;
2020-07-18 12:10:27 +00:00
}
2018-08-21 15:37:51 +00:00
GodotClosestConvexResultCallback btResult ( bt_xform_from . getOrigin ( ) , bt_xform_to . getOrigin ( ) , & p_exclude , p_collide_with_bodies , p_collide_with_areas ) ;
2017-11-21 21:56:40 +00:00
btResult . m_collisionFilterGroup = 0 ;
btResult . m_collisionFilterMask = p_collision_mask ;
2017-11-04 19:52:59 +00:00
2018-10-05 08:43:48 +00:00
space - > dynamicsWorld - > convexSweepTest ( bt_convex_shape , bt_xform_from , bt_xform_to , btResult , space - > dynamicsWorld - > getDispatchInfo ( ) . m_allowedCcdPenetration ) ;
2017-11-04 19:52:59 +00:00
2017-11-20 18:00:47 +00:00
if ( btResult . hasHit ( ) ) {
2018-08-26 10:09:52 +00:00
const btScalar l = bt_motion . length ( ) ;
2018-09-22 09:17:31 +00:00
r_closest_unsafe = btResult . m_closestHitFraction ;
r_closest_safe = MAX ( r_closest_unsafe - ( 1 - ( ( l - 0.01 ) / l ) ) , 0 ) ;
2017-11-20 18:00:47 +00:00
if ( r_info ) {
2017-11-19 17:21:36 +00:00
if ( btCollisionObject : : CO_RIGID_BODY = = btResult . m_hitCollisionObject - > getInternalType ( ) ) {
B_TO_G ( static_cast < const btRigidBody * > ( btResult . m_hitCollisionObject ) - > getVelocityInLocalPoint ( btResult . m_hitPointWorld ) , r_info - > linear_velocity ) ;
}
CollisionObjectBullet * collision_object = static_cast < CollisionObjectBullet * > ( btResult . m_hitCollisionObject - > getUserPointer ( ) ) ;
B_TO_G ( btResult . m_hitPointWorld , r_info - > point ) ;
B_TO_G ( btResult . m_hitNormalWorld , r_info - > normal ) ;
r_info - > rid = collision_object - > get_self ( ) ;
r_info - > collider_id = collision_object - > get_instance_id ( ) ;
r_info - > shape = btResult . m_shapeId ;
2017-11-04 19:52:59 +00:00
}
2019-12-09 10:11:06 +00:00
} else {
r_closest_safe = 1.0f ;
r_closest_unsafe = 1.0f ;
2017-11-04 19:52:59 +00:00
}
2020-10-08 10:22:59 +00:00
bulletdelete ( bt_convex_shape ) ;
2018-09-22 09:17:31 +00:00
return true ; // Mean success
2017-11-04 19:52:59 +00:00
}
/// Returns the list of contacts pairs in this order: Local contact, other body contact
2020-10-17 05:08:21 +00:00
bool BulletPhysicsDirectSpaceState : : collide_shape ( RID p_shape , const Transform3D & 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 ) {
2020-05-14 14:41:43 +00:00
if ( p_result_max < = 0 ) {
2020-05-14 09:00:19 +00:00
return false ;
2020-05-14 14:41:43 +00:00
}
2017-11-04 19:52:59 +00:00
2021-09-29 17:08:41 +00:00
ShapeBullet * shape = space - > get_physics_server ( ) - > get_shape_owner ( ) - > get_or_null ( p_shape ) ;
2020-06-22 08:22:11 +00:00
ERR_FAIL_COND_V ( ! shape , false ) ;
2017-11-04 19:52:59 +00:00
2018-04-14 19:53:25 +00:00
btCollisionShape * btShape = shape - > create_bt_shape ( p_shape_xform . basis . get_scale_abs ( ) , p_margin ) ;
2017-11-19 16:11:47 +00:00
if ( ! btShape - > isConvex ( ) ) {
2020-10-08 10:22:59 +00:00
bulletdelete ( btShape ) ;
2019-11-06 16:03:04 +00:00
ERR_PRINT ( " The shape is not a convex shape, then is not supported: shape type: " + itos ( shape - > get_type ( ) ) ) ;
2020-05-14 09:00:19 +00:00
return false ;
2017-11-04 19:52:59 +00:00
}
2017-11-19 16:11:47 +00:00
btConvexShape * btConvex = static_cast < btConvexShape * > ( btShape ) ;
2017-11-04 19:52:59 +00:00
btTransform bt_xform ;
G_TO_B ( p_shape_xform , bt_xform ) ;
2017-12-23 17:23:12 +00:00
UNSCALE_BT_BASIS ( bt_xform ) ;
2017-11-04 19:52:59 +00:00
btCollisionObject collision_object ;
collision_object . setCollisionShape ( btConvex ) ;
collision_object . setWorldTransform ( bt_xform ) ;
2018-08-21 15:37:51 +00:00
GodotContactPairContactResultCallback btQuery ( & collision_object , r_results , p_result_max , & p_exclude , p_collide_with_bodies , p_collide_with_areas ) ;
2017-11-21 21:56:40 +00:00
btQuery . m_collisionFilterGroup = 0 ;
btQuery . m_collisionFilterMask = p_collision_mask ;
2017-12-23 17:23:12 +00:00
btQuery . m_closestDistanceThreshold = 0 ;
2017-11-04 19:52:59 +00:00
space - > dynamicsWorld - > contactTest ( & collision_object , btQuery ) ;
r_result_count = btQuery . m_count ;
2020-10-08 10:22:59 +00:00
bulletdelete ( btConvex ) ;
2017-11-04 19:52:59 +00:00
return btQuery . m_count ;
}
2020-10-17 05:08:21 +00:00
bool BulletPhysicsDirectSpaceState : : rest_info ( RID p_shape , const Transform3D & 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 ) {
2021-09-29 17:08:41 +00:00
ShapeBullet * shape = space - > get_physics_server ( ) - > get_shape_owner ( ) - > get_or_null ( p_shape ) ;
2020-06-22 08:22:11 +00:00
ERR_FAIL_COND_V ( ! shape , false ) ;
2017-11-04 19:52:59 +00:00
2018-04-14 19:53:25 +00:00
btCollisionShape * btShape = shape - > create_bt_shape ( p_shape_xform . basis . get_scale_abs ( ) , p_margin ) ;
2017-11-19 16:11:47 +00:00
if ( ! btShape - > isConvex ( ) ) {
2020-10-08 10:22:59 +00:00
bulletdelete ( btShape ) ;
2019-11-06 16:03:04 +00:00
ERR_PRINT ( " The shape is not a convex shape, then is not supported: shape type: " + itos ( shape - > get_type ( ) ) ) ;
2020-05-14 09:00:19 +00:00
return false ;
2017-11-04 19:52:59 +00:00
}
2017-11-19 16:11:47 +00:00
btConvexShape * btConvex = static_cast < btConvexShape * > ( btShape ) ;
2017-11-04 19:52:59 +00:00
btTransform bt_xform ;
G_TO_B ( p_shape_xform , bt_xform ) ;
2017-12-23 17:23:12 +00:00
UNSCALE_BT_BASIS ( bt_xform ) ;
2017-11-04 19:52:59 +00:00
btCollisionObject collision_object ;
collision_object . setCollisionShape ( btConvex ) ;
collision_object . setWorldTransform ( bt_xform ) ;
2018-08-21 15:37:51 +00:00
GodotRestInfoContactResultCallback btQuery ( & collision_object , r_info , & p_exclude , p_collide_with_bodies , p_collide_with_areas ) ;
2017-11-21 21:56:40 +00:00
btQuery . m_collisionFilterGroup = 0 ;
btQuery . m_collisionFilterMask = p_collision_mask ;
2017-12-23 17:23:12 +00:00
btQuery . m_closestDistanceThreshold = 0 ;
2017-11-04 19:52:59 +00:00
space - > dynamicsWorld - > contactTest ( & collision_object , btQuery ) ;
2020-10-08 10:22:59 +00:00
bulletdelete ( btConvex ) ;
2017-11-04 19:52:59 +00:00
if ( btQuery . m_collided ) {
if ( btCollisionObject : : CO_RIGID_BODY = = btQuery . m_rest_info_collision_object - > getInternalType ( ) ) {
B_TO_G ( static_cast < const btRigidBody * > ( btQuery . m_rest_info_collision_object ) - > getVelocityInLocalPoint ( btQuery . m_rest_info_bt_point ) , r_info - > linear_velocity ) ;
}
B_TO_G ( btQuery . m_rest_info_bt_point , r_info - > point ) ;
}
return btQuery . m_collided ;
}
Vector3 BulletPhysicsDirectSpaceState : : get_closest_point_to_object_volume ( RID p_object , const Vector3 p_point ) const {
2020-06-21 16:29:45 +00:00
RigidCollisionObjectBullet * rigid_object = space - > get_physics_server ( ) - > get_rigid_collision_object ( p_object ) ;
2017-11-04 19:52:59 +00:00
ERR_FAIL_COND_V ( ! rigid_object , Vector3 ( ) ) ;
btVector3 out_closest_point ( 0 , 0 , 0 ) ;
btScalar out_distance = 1e20 ;
btVector3 bt_point ;
G_TO_B ( p_point , bt_point ) ;
btSphereShape point_shape ( 0. ) ;
btCollisionShape * shape ;
btConvexShape * convex_shape ;
btTransform child_transform ;
btTransform body_transform ( rigid_object - > get_bt_collision_object ( ) - > getWorldTransform ( ) ) ;
btGjkPairDetector : : ClosestPointInput input ;
input . m_transformA . getBasis ( ) . setIdentity ( ) ;
input . m_transformA . setOrigin ( bt_point ) ;
bool shapes_found = false ;
2018-09-06 16:19:05 +00:00
for ( int i = rigid_object - > get_shape_count ( ) - 1 ; 0 < = i ; - - i ) {
shape = rigid_object - > get_bt_shape ( i ) ;
2017-11-04 19:52:59 +00:00
if ( shape - > isConvex ( ) ) {
2018-09-06 16:19:05 +00:00
child_transform = rigid_object - > get_bt_shape_transform ( i ) ;
2017-11-04 19:52:59 +00:00
convex_shape = static_cast < btConvexShape * > ( shape ) ;
input . m_transformB = body_transform * child_transform ;
btPointCollector result ;
2017-11-07 14:22:09 +00:00
btGjkPairDetector gjk_pair_detector ( & point_shape , convex_shape , space - > gjk_simplex_solver , space - > gjk_epa_pen_solver ) ;
2020-04-01 23:20:12 +00:00
gjk_pair_detector . getClosestPoints ( input , result , nullptr ) ;
2017-11-04 19:52:59 +00:00
if ( out_distance > result . m_distance ) {
out_distance = result . m_distance ;
out_closest_point = result . m_pointInWorld ;
}
}
shapes_found = true ;
}
if ( shapes_found ) {
Vector3 out ;
B_TO_G ( out_closest_point , out ) ;
return out ;
} else {
// no shapes found, use distance to origin.
return rigid_object - > get_transform ( ) . get_origin ( ) ;
}
}
2020-05-12 15:01:17 +00:00
SpaceBullet : : SpaceBullet ( ) {
2017-11-21 00:36:32 +00:00
create_empty_world ( GLOBAL_DEF ( " physics/3d/active_soft_world " , true ) ) ;
2017-11-04 19:52:59 +00:00
direct_access = memnew ( BulletPhysicsDirectSpaceState ( this ) ) ;
}
SpaceBullet : : ~ SpaceBullet ( ) {
memdelete ( direct_access ) ;
destroy_world ( ) ;
}
void SpaceBullet : : flush_queries ( ) {
2020-10-08 10:22:59 +00:00
const btCollisionObjectArray & colObjArray = dynamicsWorld - > getCollisionObjectArray ( ) ;
for ( int i = colObjArray . size ( ) - 1 ; 0 < = i ; - - i ) {
static_cast < CollisionObjectBullet * > ( colObjArray [ i ] - > getUserPointer ( ) ) - > dispatch_callbacks ( ) ;
2017-11-04 19:52:59 +00:00
}
}
void SpaceBullet : : step ( real_t p_delta_time ) {
2017-11-21 00:36:32 +00:00
delta_time = p_delta_time ;
2017-11-04 19:52:59 +00:00
dynamicsWorld - > stepSimulation ( p_delta_time , 0 , 0 ) ;
}
2020-03-27 18:21:27 +00:00
void SpaceBullet : : set_param ( PhysicsServer3D : : AreaParameter p_param , const Variant & p_value ) {
2017-11-04 19:52:59 +00:00
assert ( dynamicsWorld ) ;
switch ( p_param ) {
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_GRAVITY :
2017-11-04 19:52:59 +00:00
gravityMagnitude = p_value ;
update_gravity ( ) ;
break ;
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_GRAVITY_VECTOR :
2017-11-04 19:52:59 +00:00
gravityDirection = p_value ;
update_gravity ( ) ;
break ;
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_LINEAR_DAMP :
2020-03-26 11:32:20 +00:00
linear_damp = p_value ;
break ;
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_ANGULAR_DAMP :
2020-03-26 11:32:20 +00:00
angular_damp = p_value ;
break ;
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_PRIORITY :
2017-11-04 19:52:59 +00:00
// Priority is always 0, the lower
break ;
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_GRAVITY_IS_POINT :
case PhysicsServer3D : : AREA_PARAM_GRAVITY_DISTANCE_SCALE :
case PhysicsServer3D : : AREA_PARAM_GRAVITY_POINT_ATTENUATION :
2017-11-04 19:52:59 +00:00
break ;
default :
2019-11-07 08:44:15 +00:00
WARN_PRINT ( " This set parameter ( " + itos ( p_param ) + " ) is ignored, the SpaceBullet doesn't support it. " ) ;
2017-11-04 19:52:59 +00:00
break ;
}
}
2020-03-27 18:21:27 +00:00
Variant SpaceBullet : : get_param ( PhysicsServer3D : : AreaParameter p_param ) {
2017-11-04 19:52:59 +00:00
switch ( p_param ) {
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_GRAVITY :
2017-11-04 19:52:59 +00:00
return gravityMagnitude ;
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_GRAVITY_VECTOR :
2017-11-04 19:52:59 +00:00
return gravityDirection ;
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_LINEAR_DAMP :
2020-03-26 11:32:20 +00:00
return linear_damp ;
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_ANGULAR_DAMP :
2020-03-26 11:32:20 +00:00
return angular_damp ;
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_PRIORITY :
2017-11-04 19:52:59 +00:00
return 0 ; // Priority is always 0, the lower
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_GRAVITY_IS_POINT :
2017-11-04 19:52:59 +00:00
return false ;
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_GRAVITY_DISTANCE_SCALE :
2017-11-04 19:52:59 +00:00
return 0 ;
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : AREA_PARAM_GRAVITY_POINT_ATTENUATION :
2017-11-04 19:52:59 +00:00
return 0 ;
default :
2019-11-07 08:44:15 +00:00
WARN_PRINT ( " This get parameter ( " + itos ( p_param ) + " ) is ignored, the SpaceBullet doesn't support it. " ) ;
2017-11-04 19:52:59 +00:00
return Variant ( ) ;
}
}
2020-03-27 18:21:27 +00:00
void SpaceBullet : : set_param ( PhysicsServer3D : : SpaceParameter p_param , real_t p_value ) {
2017-11-04 19:52:59 +00:00
switch ( p_param ) {
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : SPACE_PARAM_CONTACT_RECYCLE_RADIUS :
case PhysicsServer3D : : SPACE_PARAM_CONTACT_MAX_SEPARATION :
case PhysicsServer3D : : SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION :
case PhysicsServer3D : : SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD :
case PhysicsServer3D : : SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD :
case PhysicsServer3D : : SPACE_PARAM_BODY_TIME_TO_SLEEP :
case PhysicsServer3D : : SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO :
case PhysicsServer3D : : SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS :
2017-11-04 19:52:59 +00:00
default :
2019-11-07 08:44:15 +00:00
WARN_PRINT ( " This set parameter ( " + itos ( p_param ) + " ) is ignored, the SpaceBullet doesn't support it. " ) ;
2017-11-04 19:52:59 +00:00
break ;
}
}
2020-03-27 18:21:27 +00:00
real_t SpaceBullet : : get_param ( PhysicsServer3D : : SpaceParameter p_param ) {
2017-11-04 19:52:59 +00:00
switch ( p_param ) {
2020-03-27 18:21:27 +00:00
case PhysicsServer3D : : SPACE_PARAM_CONTACT_RECYCLE_RADIUS :
case PhysicsServer3D : : SPACE_PARAM_CONTACT_MAX_SEPARATION :
case PhysicsServer3D : : SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION :
case PhysicsServer3D : : SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD :
case PhysicsServer3D : : SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD :
case PhysicsServer3D : : SPACE_PARAM_BODY_TIME_TO_SLEEP :
case PhysicsServer3D : : SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO :
case PhysicsServer3D : : SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS :
2017-11-04 19:52:59 +00:00
default :
2021-06-07 08:17:32 +00:00
WARN_PRINT ( " The SpaceBullet doesn't support this get parameter ( " + itos ( p_param ) + " ), 0 is returned. " ) ;
2017-11-04 19:52:59 +00:00
return 0.f ;
}
}
void SpaceBullet : : add_area ( AreaBullet * p_area ) {
areas . push_back ( p_area ) ;
dynamicsWorld - > addCollisionObject ( p_area - > get_bt_ghost ( ) , p_area - > get_collision_layer ( ) , p_area - > get_collision_mask ( ) ) ;
}
void SpaceBullet : : remove_area ( AreaBullet * p_area ) {
2020-10-08 10:22:59 +00:00
areas . erase ( p_area ) ;
dynamicsWorld - > removeCollisionObject ( p_area - > get_bt_ghost ( ) ) ;
2017-11-04 19:52:59 +00:00
}
void SpaceBullet : : reload_collision_filters ( AreaBullet * p_area ) {
2019-10-08 09:35:35 +00:00
btGhostObject * ghost_object = p_area - > get_bt_ghost ( ) ;
btBroadphaseProxy * ghost_proxy = ghost_object - > getBroadphaseHandle ( ) ;
ghost_proxy - > m_collisionFilterGroup = p_area - > get_collision_layer ( ) ;
ghost_proxy - > m_collisionFilterMask = p_area - > get_collision_mask ( ) ;
dynamicsWorld - > refreshBroadphaseProxy ( ghost_object ) ;
2017-11-04 19:52:59 +00:00
}
void SpaceBullet : : add_rigid_body ( RigidBodyBullet * p_body ) {
if ( p_body - > is_static ( ) ) {
dynamicsWorld - > addCollisionObject ( p_body - > get_bt_rigid_body ( ) , p_body - > get_collision_layer ( ) , p_body - > get_collision_mask ( ) ) ;
} else {
dynamicsWorld - > addRigidBody ( p_body - > get_bt_rigid_body ( ) , p_body - > get_collision_layer ( ) , p_body - > get_collision_mask ( ) ) ;
2017-11-23 00:25:29 +00:00
p_body - > scratch_space_override_modificator ( ) ;
2017-11-04 19:52:59 +00:00
}
}
2021-01-13 13:30:42 +00:00
void SpaceBullet : : remove_rigid_body_constraints ( RigidBodyBullet * p_body ) {
2020-11-26 16:55:42 +00:00
btRigidBody * btBody = p_body - > get_bt_rigid_body ( ) ;
int constraints = btBody - > getNumConstraintRefs ( ) ;
if ( constraints > 0 ) {
2020-12-26 14:23:30 +00:00
ERR_PRINT ( " A body connected to joints was removed. " ) ;
2020-11-26 16:55:42 +00:00
for ( int i = 0 ; i < constraints ; i + + ) {
dynamicsWorld - > removeConstraint ( btBody - > getConstraintRef ( i ) ) ;
}
}
2021-01-13 13:30:42 +00:00
}
void SpaceBullet : : remove_rigid_body ( RigidBodyBullet * p_body ) {
btRigidBody * btBody = p_body - > get_bt_rigid_body ( ) ;
2020-11-26 16:55:42 +00:00
2020-10-08 10:22:59 +00:00
if ( p_body - > is_static ( ) ) {
2020-11-26 16:55:42 +00:00
dynamicsWorld - > removeCollisionObject ( btBody ) ;
2020-10-08 10:22:59 +00:00
} else {
2020-11-26 16:55:42 +00:00
dynamicsWorld - > removeRigidBody ( btBody ) ;
2017-11-04 19:52:59 +00:00
}
}
void SpaceBullet : : reload_collision_filters ( RigidBodyBullet * p_body ) {
2019-10-08 09:35:35 +00:00
btRigidBody * rigid_body = p_body - > get_bt_rigid_body ( ) ;
btBroadphaseProxy * body_proxy = rigid_body - > getBroadphaseProxy ( ) ;
body_proxy - > m_collisionFilterGroup = p_body - > get_collision_layer ( ) ;
body_proxy - > m_collisionFilterMask = p_body - > get_collision_mask ( ) ;
dynamicsWorld - > refreshBroadphaseProxy ( rigid_body ) ;
2017-11-04 19:52:59 +00:00
}
void SpaceBullet : : add_soft_body ( SoftBodyBullet * p_body ) {
if ( is_using_soft_world ( ) ) {
if ( p_body - > get_bt_soft_body ( ) ) {
2017-11-21 00:36:32 +00:00
p_body - > get_bt_soft_body ( ) - > m_worldInfo = get_soft_body_world_info ( ) ;
2017-11-04 19:52:59 +00:00
static_cast < btSoftRigidDynamicsWorld * > ( dynamicsWorld ) - > addSoftBody ( p_body - > get_bt_soft_body ( ) , p_body - > get_collision_layer ( ) , p_body - > get_collision_mask ( ) ) ;
}
} else {
ERR_PRINT ( " This soft body can't be added to non soft world " ) ;
}
}
void SpaceBullet : : remove_soft_body ( SoftBodyBullet * p_body ) {
if ( is_using_soft_world ( ) ) {
if ( p_body - > get_bt_soft_body ( ) ) {
static_cast < btSoftRigidDynamicsWorld * > ( dynamicsWorld ) - > removeSoftBody ( p_body - > get_bt_soft_body ( ) ) ;
2020-04-01 23:20:12 +00:00
p_body - > get_bt_soft_body ( ) - > m_worldInfo = nullptr ;
2017-11-04 19:52:59 +00:00
}
}
}
void SpaceBullet : : reload_collision_filters ( SoftBodyBullet * p_body ) {
// This is necessary to change collision filter
remove_soft_body ( p_body ) ;
add_soft_body ( p_body ) ;
}
void SpaceBullet : : add_constraint ( ConstraintBullet * p_constraint , bool disableCollisionsBetweenLinkedBodies ) {
p_constraint - > set_space ( this ) ;
dynamicsWorld - > addConstraint ( p_constraint - > get_bt_constraint ( ) , disableCollisionsBetweenLinkedBodies ) ;
}
void SpaceBullet : : remove_constraint ( ConstraintBullet * p_constraint ) {
dynamicsWorld - > removeConstraint ( p_constraint - > get_bt_constraint ( ) ) ;
}
int SpaceBullet : : get_num_collision_objects ( ) const {
return dynamicsWorld - > getNumCollisionObjects ( ) ;
}
void SpaceBullet : : remove_all_collision_objects ( ) {
for ( int i = dynamicsWorld - > getNumCollisionObjects ( ) - 1 ; 0 < = i ; - - i ) {
btCollisionObject * btObj = dynamicsWorld - > getCollisionObjectArray ( ) [ i ] ;
CollisionObjectBullet * colObj = static_cast < CollisionObjectBullet * > ( btObj - > getUserPointer ( ) ) ;
2020-04-01 23:20:12 +00:00
colObj - > set_space ( nullptr ) ;
2017-11-04 19:52:59 +00:00
}
}
void onBulletTickCallback ( btDynamicsWorld * p_dynamicsWorld , btScalar timeStep ) {
const btCollisionObjectArray & colObjArray = p_dynamicsWorld - > getCollisionObjectArray ( ) ;
2018-10-07 05:14:38 +00:00
// Notify all Collision objects the collision checker is started
2017-11-04 19:52:59 +00:00
for ( int i = colObjArray . size ( ) - 1 ; 0 < = i ; - - i ) {
2018-10-07 05:14:38 +00:00
static_cast < CollisionObjectBullet * > ( colObjArray [ i ] - > getUserPointer ( ) ) - > on_collision_checker_start ( ) ;
2017-11-04 19:52:59 +00:00
}
SpaceBullet * sb = static_cast < SpaceBullet * > ( p_dynamicsWorld - > getWorldUserInfo ( ) ) ;
sb - > check_ghost_overlaps ( ) ;
sb - > check_body_collision ( ) ;
2018-10-07 05:14:38 +00:00
for ( int i = colObjArray . size ( ) - 1 ; 0 < = i ; - - i ) {
static_cast < CollisionObjectBullet * > ( colObjArray [ i ] - > getUserPointer ( ) ) - > on_collision_checker_end ( ) ;
}
2017-11-04 19:52:59 +00:00
}
BulletPhysicsDirectSpaceState * SpaceBullet : : get_direct_state ( ) {
return direct_access ;
}
btScalar calculateGodotCombinedRestitution ( const btCollisionObject * body0 , const btCollisionObject * body1 ) {
2018-07-23 14:37:07 +00:00
return CLAMP ( body0 - > getRestitution ( ) + body1 - > getRestitution ( ) , 0 , 1 ) ;
2017-10-24 16:10:30 +00:00
}
btScalar calculateGodotCombinedFriction ( const btCollisionObject * body0 , const btCollisionObject * body1 ) {
2018-07-23 14:37:07 +00:00
return ABS ( MIN ( body0 - > getFriction ( ) , body1 - > getFriction ( ) ) ) ;
2017-11-04 19:52:59 +00:00
}
void SpaceBullet : : create_empty_world ( bool p_create_soft_world ) {
2017-11-07 14:22:09 +00:00
gjk_epa_pen_solver = bulletnew ( btGjkEpaPenetrationDepthSolver ) ;
gjk_simplex_solver = bulletnew ( btVoronoiSimplexSolver ) ;
2017-11-04 19:52:59 +00:00
void * world_mem ;
if ( p_create_soft_world ) {
world_mem = malloc ( sizeof ( btSoftRigidDynamicsWorld ) ) ;
} else {
world_mem = malloc ( sizeof ( btDiscreteDynamicsWorld ) ) ;
}
2019-08-11 08:49:53 +00:00
ERR_FAIL_COND_MSG ( ! world_mem , " Out of memory. " ) ;
2017-11-04 19:52:59 +00:00
if ( p_create_soft_world ) {
2018-09-22 07:42:19 +00:00
collisionConfiguration = bulletnew ( GodotSoftCollisionConfiguration ( static_cast < btDiscreteDynamicsWorld * > ( world_mem ) ) ) ;
2017-11-04 19:52:59 +00:00
} else {
collisionConfiguration = bulletnew ( GodotCollisionConfiguration ( static_cast < btDiscreteDynamicsWorld * > ( world_mem ) ) ) ;
}
dispatcher = bulletnew ( GodotCollisionDispatcher ( collisionConfiguration ) ) ;
broadphase = bulletnew ( btDbvtBroadphase ) ;
solver = bulletnew ( btSequentialImpulseConstraintSolver ) ;
if ( p_create_soft_world ) {
dynamicsWorld = new ( world_mem ) btSoftRigidDynamicsWorld ( dispatcher , broadphase , solver , collisionConfiguration ) ;
soft_body_world_info = bulletnew ( btSoftBodyWorldInfo ) ;
} else {
dynamicsWorld = new ( world_mem ) btDiscreteDynamicsWorld ( dispatcher , broadphase , solver , collisionConfiguration ) ;
}
ghostPairCallback = bulletnew ( btGhostPairCallback ) ;
godotFilterCallback = bulletnew ( GodotFilterCallback ) ;
gCalculateCombinedRestitutionCallback = & calculateGodotCombinedRestitution ;
2017-10-24 16:10:30 +00:00
gCalculateCombinedFrictionCallback = & calculateGodotCombinedFriction ;
2018-08-31 07:40:50 +00:00
gContactAddedCallback = & godotContactAddedCallback ;
2017-11-04 19:52:59 +00:00
dynamicsWorld - > setWorldUserInfo ( this ) ;
dynamicsWorld - > setInternalTickCallback ( onBulletTickCallback , this , false ) ;
dynamicsWorld - > getBroadphase ( ) - > getOverlappingPairCache ( ) - > setInternalGhostPairCallback ( ghostPairCallback ) ; // Setup ghost check
dynamicsWorld - > getPairCache ( ) - > setOverlapFilterCallback ( godotFilterCallback ) ;
if ( soft_body_world_info ) {
soft_body_world_info - > m_broadphase = broadphase ;
soft_body_world_info - > m_dispatcher = dispatcher ;
soft_body_world_info - > m_sparsesdf . Initialize ( ) ;
}
update_gravity ( ) ;
}
void SpaceBullet : : destroy_world ( ) {
/// The world elements (like: Collision Objects, Constraints, Shapes) are managed by godot
2020-04-01 23:20:12 +00:00
dynamicsWorld - > getBroadphase ( ) - > getOverlappingPairCache ( ) - > setInternalGhostPairCallback ( nullptr ) ;
dynamicsWorld - > getPairCache ( ) - > setOverlapFilterCallback ( nullptr ) ;
2017-11-04 19:52:59 +00:00
bulletdelete ( ghostPairCallback ) ;
bulletdelete ( godotFilterCallback ) ;
// Deallocate world
dynamicsWorld - > ~ btDiscreteDynamicsWorld ( ) ;
free ( dynamicsWorld ) ;
2020-04-01 23:20:12 +00:00
dynamicsWorld = nullptr ;
2017-11-04 19:52:59 +00:00
bulletdelete ( solver ) ;
bulletdelete ( broadphase ) ;
bulletdelete ( dispatcher ) ;
bulletdelete ( collisionConfiguration ) ;
bulletdelete ( soft_body_world_info ) ;
2017-11-07 14:22:09 +00:00
bulletdelete ( gjk_simplex_solver ) ;
bulletdelete ( gjk_epa_pen_solver ) ;
2017-11-04 19:52:59 +00:00
}
void SpaceBullet : : check_ghost_overlaps ( ) {
2018-02-21 16:30:55 +00:00
/// Algorithm support variables
2018-09-05 11:43:02 +00:00
btCollisionShape * other_body_shape ;
2017-11-04 19:52:59 +00:00
btConvexShape * area_shape ;
2019-02-22 11:46:54 +00:00
btGjkPairDetector : : ClosestPointInput gjk_input ;
2017-11-04 19:52:59 +00:00
AreaBullet * area ;
int x ( - 1 ) , i ( - 1 ) , y ( - 1 ) , z ( - 1 ) , indexOverlap ( - 1 ) ;
/// For each areas
for ( x = areas . size ( ) - 1 ; 0 < = x ; - - x ) {
area = areas [ x ] ;
2019-02-18 11:57:55 +00:00
btVector3 area_scale ( area - > get_bt_body_scale ( ) ) ;
2020-05-14 14:41:43 +00:00
if ( ! area - > is_monitoring ( ) ) {
2017-11-04 19:52:59 +00:00
continue ;
2020-05-14 14:41:43 +00:00
}
2017-11-04 19:52:59 +00:00
/// 1. Reset all states
for ( i = area - > overlappingObjects . size ( ) - 1 ; 0 < = i ; - - i ) {
2020-10-08 10:16:12 +00:00
AreaBullet : : OverlappingObjectData & otherObj = area - > overlappingObjects . write [ i ] ;
2017-11-04 19:52:59 +00:00
// This check prevent the overwrite of ENTER state
// if this function is called more times before dispatchCallbacks
if ( otherObj . state ! = AreaBullet : : OVERLAP_STATE_ENTER ) {
otherObj . state = AreaBullet : : OVERLAP_STATE_DIRTY ;
}
}
/// 2. Check all overlapping objects using GJK
const btAlignedObjectArray < btCollisionObject * > ghostOverlaps = area - > get_bt_ghost ( ) - > getOverlappingPairs ( ) ;
// For each overlapping
for ( i = ghostOverlaps . size ( ) - 1 ; 0 < = i ; - - i ) {
2019-01-21 07:30:49 +00:00
bool hasOverlap = false ;
2018-10-07 05:14:38 +00:00
btCollisionObject * overlapped_bt_co = ghostOverlaps [ i ] ;
RigidCollisionObjectBullet * otherObject = static_cast < RigidCollisionObjectBullet * > ( overlapped_bt_co - > getUserPointer ( ) ) ;
2019-02-18 11:57:55 +00:00
btVector3 other_body_scale ( otherObject - > get_bt_body_scale ( ) ) ;
2018-10-07 05:14:38 +00:00
2019-01-21 07:30:49 +00:00
if ( ! area - > is_transform_changed ( ) & & ! otherObject - > is_transform_changed ( ) ) {
2019-02-18 13:15:27 +00:00
hasOverlap = - 1 ! = area - > find_overlapping_object ( otherObject ) ;
2019-01-21 07:30:49 +00:00
goto collision_found ;
}
2017-11-04 19:52:59 +00:00
2018-10-07 05:14:38 +00:00
if ( overlapped_bt_co - > getUserIndex ( ) = = CollisionObjectBullet : : TYPE_AREA ) {
2020-05-14 14:41:43 +00:00
if ( ! static_cast < AreaBullet * > ( overlapped_bt_co - > getUserPointer ( ) ) - > is_monitorable ( ) ) {
2018-10-07 05:14:38 +00:00
continue ;
2020-05-14 14:41:43 +00:00
}
} else if ( overlapped_bt_co - > getUserIndex ( ) ! = CollisionObjectBullet : : TYPE_RIGID_BODY ) {
2018-10-07 05:14:38 +00:00
continue ;
2020-05-14 14:41:43 +00:00
}
2017-11-04 19:52:59 +00:00
// For each area shape
2018-09-06 16:19:05 +00:00
for ( y = area - > get_shape_count ( ) - 1 ; 0 < = y ; - - y ) {
2020-05-14 14:41:43 +00:00
if ( ! area - > get_bt_shape ( y ) - > isConvex ( ) ) {
2017-11-04 19:52:59 +00:00
continue ;
2020-05-14 14:41:43 +00:00
}
2017-11-04 19:52:59 +00:00
2019-02-18 11:57:55 +00:00
btTransform area_shape_treansform ( area - > get_bt_shape_transform ( y ) ) ;
area_shape_treansform . getOrigin ( ) * = area_scale ;
2019-02-22 11:46:54 +00:00
gjk_input . m_transformA =
area - > get_transform__bullet ( ) *
area_shape_treansform ;
2018-09-06 16:19:05 +00:00
area_shape = static_cast < btConvexShape * > ( area - > get_bt_shape ( y ) ) ;
2017-11-04 19:52:59 +00:00
// For each other object shape
2018-09-06 16:19:05 +00:00
for ( z = otherObject - > get_shape_count ( ) - 1 ; 0 < = z ; - - z ) {
other_body_shape = static_cast < btCollisionShape * > ( otherObject - > get_bt_shape ( z ) ) ;
2019-02-18 11:57:55 +00:00
btTransform other_shape_transform ( otherObject - > get_bt_shape_transform ( z ) ) ;
other_shape_transform . getOrigin ( ) * = other_body_scale ;
2019-02-22 11:46:54 +00:00
gjk_input . m_transformB =
otherObject - > get_transform__bullet ( ) *
other_shape_transform ;
if ( other_body_shape - > isConvex ( ) ) {
btPointCollector result ;
btGjkPairDetector gjk_pair_detector (
area_shape ,
static_cast < btConvexShape * > ( other_body_shape ) ,
gjk_simplex_solver ,
gjk_epa_pen_solver ) ;
2020-04-01 23:20:12 +00:00
gjk_pair_detector . getClosestPoints ( gjk_input , result , nullptr ) ;
2019-02-22 11:46:54 +00:00
if ( 0 > = result . m_distance ) {
hasOverlap = true ;
goto collision_found ;
}
} else {
2020-04-01 23:20:12 +00:00
btCollisionObjectWrapper obA ( nullptr , area_shape , area - > get_bt_ghost ( ) , gjk_input . m_transformA , - 1 , y ) ;
btCollisionObjectWrapper obB ( nullptr , other_body_shape , otherObject - > get_bt_collision_object ( ) , gjk_input . m_transformB , - 1 , z ) ;
2019-02-22 11:46:54 +00:00
2020-04-01 23:20:12 +00:00
btCollisionAlgorithm * algorithm = dispatcher - > findAlgorithm ( & obA , & obB , nullptr , BT_CONTACT_POINT_ALGORITHMS ) ;
2019-02-22 11:46:54 +00:00
2020-05-14 14:41:43 +00:00
if ( ! algorithm ) {
2019-02-22 11:46:54 +00:00
continue ;
2020-05-14 14:41:43 +00:00
}
2018-09-05 11:43:02 +00:00
2019-02-22 11:46:54 +00:00
GodotDeepPenetrationContactResultCallback contactPointResult ( & obA , & obB ) ;
algorithm - > processCollision ( & obA , & obB , dynamicsWorld - > getDispatchInfo ( ) , & contactPointResult ) ;
2018-09-05 11:43:02 +00:00
2019-02-22 11:46:54 +00:00
algorithm - > ~ btCollisionAlgorithm ( ) ;
dispatcher - > freeCollisionAlgorithm ( algorithm ) ;
2018-09-05 11:43:02 +00:00
2019-02-22 11:46:54 +00:00
if ( contactPointResult . hasHit ( ) ) {
hasOverlap = true ;
goto collision_found ;
}
2017-11-04 19:52:59 +00:00
}
2018-09-05 11:43:02 +00:00
2017-11-04 19:52:59 +00:00
} // ~For each other object shape
} // ~For each area shape
collision_found :
2020-05-14 14:41:43 +00:00
if ( ! hasOverlap ) {
2017-11-04 19:52:59 +00:00
continue ;
2020-05-14 14:41:43 +00:00
}
2017-11-04 19:52:59 +00:00
indexOverlap = area - > find_overlapping_object ( otherObject ) ;
if ( - 1 = = indexOverlap ) {
// Not found
area - > add_overlap ( otherObject ) ;
} else {
// Found
area - > put_overlap_as_inside ( indexOverlap ) ;
}
}
/// 3. Remove not overlapping
for ( i = area - > overlappingObjects . size ( ) - 1 ; 0 < = i ; - - i ) {
// If the overlap has DIRTY state it means that it's no more overlapping
if ( area - > overlappingObjects [ i ] . state = = AreaBullet : : OVERLAP_STATE_DIRTY ) {
area - > put_overlap_as_exit ( i ) ;
}
}
}
}
void SpaceBullet : : check_body_collision ( ) {
# ifdef DEBUG_ENABLED
reset_debug_contact_count ( ) ;
# endif
const int numManifolds = dynamicsWorld - > getDispatcher ( ) - > getNumManifolds ( ) ;
for ( int i = 0 ; i < numManifolds ; + + i ) {
btPersistentManifold * contactManifold = dynamicsWorld - > getDispatcher ( ) - > getManifoldByIndexInternal ( i ) ;
// I know this static cast is a bit risky. But I'm checking its type just after it.
// This allow me to avoid a lot of other cast and checks
2017-11-07 14:22:09 +00:00
RigidBodyBullet * bodyA = static_cast < RigidBodyBullet * > ( contactManifold - > getBody0 ( ) - > getUserPointer ( ) ) ;
RigidBodyBullet * bodyB = static_cast < RigidBodyBullet * > ( contactManifold - > getBody1 ( ) - > getUserPointer ( ) ) ;
2017-11-04 19:52:59 +00:00
if ( CollisionObjectBullet : : TYPE_RIGID_BODY = = bodyA - > getType ( ) & & CollisionObjectBullet : : TYPE_RIGID_BODY = = bodyB - > getType ( ) ) {
if ( ! bodyA - > can_add_collision ( ) & & ! bodyB - > can_add_collision ( ) ) {
continue ;
}
const int numContacts = contactManifold - > getNumContacts ( ) ;
2018-11-22 08:03:21 +00:00
/// Since I don't need report all contacts for these objects,
/// So report only the first
2017-11-04 19:52:59 +00:00
# define REPORT_ALL_CONTACTS 0
# if REPORT_ALL_CONTACTS
for ( int j = 0 ; j < numContacts ; j + + ) {
btManifoldPoint & pt = contactManifold - > getContactPoint ( j ) ;
# else
if ( numContacts ) {
btManifoldPoint & pt = contactManifold - > getContactPoint ( 0 ) ;
# endif
2018-11-22 08:03:21 +00:00
if (
pt . getDistance ( ) < = 0.0 | |
bodyA - > was_colliding ( bodyB ) | |
bodyB - > was_colliding ( bodyA ) ) {
2018-09-19 17:01:59 +00:00
Vector3 collisionWorldPosition ;
Vector3 collisionLocalPosition ;
Vector3 normalOnB ;
2021-01-28 06:34:26 +00:00
real_t appliedImpulse = pt . m_appliedImpulse ;
2018-09-19 17:01:59 +00:00
B_TO_G ( pt . m_normalWorldOnB , normalOnB ) ;
2020-10-17 17:09:10 +00:00
// The pt.m_index only contains the shape index when more than one collision shape is used
// and only if the collision shape is not a concave collision shape.
// A value of -1 in pt.m_partId indicates the pt.m_index is a shape index.
int shape_index_a = 0 ;
if ( bodyA - > get_shape_count ( ) > 1 & & pt . m_partId0 = = - 1 ) {
shape_index_a = pt . m_index0 ;
}
int shape_index_b = 0 ;
if ( bodyB - > get_shape_count ( ) > 1 & & pt . m_partId1 = = - 1 ) {
shape_index_b = pt . m_index1 ;
}
2018-09-19 17:01:59 +00:00
if ( bodyA - > can_add_collision ( ) ) {
B_TO_G ( pt . getPositionWorldOnB ( ) , collisionWorldPosition ) ;
/// pt.m_localPointB Doesn't report the exact point in local space
B_TO_G ( pt . getPositionWorldOnB ( ) - contactManifold - > getBody1 ( ) - > getWorldTransform ( ) . getOrigin ( ) , collisionLocalPosition ) ;
2020-10-17 17:09:10 +00:00
bodyA - > add_collision_object ( bodyB , collisionWorldPosition , collisionLocalPosition , normalOnB , appliedImpulse , shape_index_b , shape_index_a ) ;
2018-09-19 17:01:59 +00:00
}
if ( bodyB - > can_add_collision ( ) ) {
B_TO_G ( pt . getPositionWorldOnA ( ) , collisionWorldPosition ) ;
/// pt.m_localPointA Doesn't report the exact point in local space
B_TO_G ( pt . getPositionWorldOnA ( ) - contactManifold - > getBody0 ( ) - > getWorldTransform ( ) . getOrigin ( ) , collisionLocalPosition ) ;
2020-10-17 17:09:10 +00:00
bodyB - > add_collision_object ( bodyA , collisionWorldPosition , collisionLocalPosition , normalOnB * - 1 , appliedImpulse * - 1 , shape_index_a , shape_index_b ) ;
2018-09-19 17:01:59 +00:00
}
2017-11-04 19:52:59 +00:00
# ifdef DEBUG_ENABLED
2018-09-19 17:01:59 +00:00
if ( is_debugging_contacts ( ) ) {
add_debug_contact ( collisionWorldPosition ) ;
}
2017-11-04 19:52:59 +00:00
# endif
2018-09-19 17:01:59 +00:00
}
2017-11-04 19:52:59 +00:00
}
}
}
}
void SpaceBullet : : update_gravity ( ) {
btVector3 btGravity ;
G_TO_B ( gravityDirection * gravityMagnitude , btGravity ) ;
2017-11-19 16:52:45 +00:00
//dynamicsWorld->setGravity(btGravity);
dynamicsWorld - > setGravity ( btVector3 ( 0 , 0 , 0 ) ) ;
2017-11-04 19:52:59 +00:00
if ( soft_body_world_info ) {
soft_body_world_info - > m_gravity = btGravity ;
}
}
/// IMPORTANT: Please don't turn it ON this is not managed correctly!!
/// I'm leaving this here just for future tests.
/// Debug motion and normal vector drawing
# define debug_test_motion 0
2018-02-25 20:05:14 +00:00
2017-12-23 17:23:12 +00:00
# define RECOVERING_MOVEMENT_SCALE 0.4
# define RECOVERING_MOVEMENT_CYCLES 4
2017-11-07 14:22:09 +00:00
2017-11-04 19:52:59 +00:00
# if debug_test_motion
2017-11-07 14:22:09 +00:00
# include "scene/3d/immediate_geometry.h"
2020-04-01 23:20:12 +00:00
static ImmediateGeometry3D * motionVec ( nullptr ) ;
static ImmediateGeometry3D * normalLine ( nullptr ) ;
2019-09-15 04:01:52 +00:00
static Ref < StandardMaterial3D > red_mat ;
static Ref < StandardMaterial3D > blue_mat ;
2017-11-04 19:52:59 +00:00
# endif
2021-08-10 01:16:45 +00:00
bool SpaceBullet : : test_body_motion ( RigidBodyBullet * p_body , const Transform3D & p_from , const Vector3 & p_motion , bool p_infinite_inertia , PhysicsServer3D : : MotionResult * r_result , bool p_exclude_raycast_shapes , const Set < RID > & p_exclude ) {
2017-11-04 19:52:59 +00:00
# if debug_test_motion
2017-11-07 14:22:09 +00:00
/// Yes I know this is not good, but I've used it as fast debugging hack.
2017-11-04 19:52:59 +00:00
/// I'm leaving it here just for speedup the other eventual debugs
if ( ! normalLine ) {
2020-03-30 16:22:57 +00:00
motionVec = memnew ( ImmediateGeometry3D ) ;
normalLine = memnew ( ImmediateGeometry3D ) ;
2017-11-04 19:52:59 +00:00
SceneTree : : get_singleton ( ) - > get_current_scene ( ) - > add_child ( motionVec ) ;
SceneTree : : get_singleton ( ) - > get_current_scene ( ) - > add_child ( normalLine ) ;
2020-10-02 21:03:52 +00:00
motionVec - > set_as_top_level ( true ) ;
normalLine - > set_as_top_level ( true ) ;
2017-12-23 17:23:12 +00:00
2019-09-15 04:01:52 +00:00
red_mat = Ref < StandardMaterial3D > ( memnew ( StandardMaterial3D ) ) ;
red_mat - > set_shading_mode ( StandardMaterial3D : : SHADING_MODE_UNSHADED ) ;
2017-11-04 19:52:59 +00:00
red_mat - > set_line_width ( 20.0 ) ;
2019-09-15 04:01:52 +00:00
red_mat - > set_transparency ( StandardMaterial3D : : TRANSPARENCY_ALPHA ) ;
red_mat - > set_flag ( StandardMaterial3D : : FLAG_ALBEDO_FROM_VERTEX_COLOR , true ) ;
red_mat - > set_flag ( StandardMaterial3D : : FLAG_SRGB_VERTEX_COLOR , true ) ;
2017-11-04 19:52:59 +00:00
red_mat - > set_albedo ( Color ( 1 , 0 , 0 , 1 ) ) ;
motionVec - > set_material_override ( red_mat ) ;
2019-09-15 04:01:52 +00:00
blue_mat = Ref < StandardMaterial3D > ( memnew ( StandardMaterial3D ) ) ;
blue_mat - > set_shading_mode ( StandardMaterial3D : : SHADING_MODE_UNSHADED ) ;
2017-11-04 19:52:59 +00:00
blue_mat - > set_line_width ( 20.0 ) ;
2019-09-15 04:01:52 +00:00
blue_mat - > set_transparency ( StandardMaterial3D : : TRANSPARENCY_ALPHA ) ;
blue_mat - > set_flag ( StandardMaterial3D : : FLAG_ALBEDO_FROM_VERTEX_COLOR , true ) ;
blue_mat - > set_flag ( StandardMaterial3D : : FLAG_SRGB_VERTEX_COLOR , true ) ;
2017-11-04 19:52:59 +00:00
blue_mat - > set_albedo ( Color ( 0 , 0 , 1 , 1 ) ) ;
normalLine - > set_material_override ( blue_mat ) ;
}
# endif
2018-05-27 12:55:52 +00:00
btTransform body_transform ;
G_TO_B ( p_from , body_transform ) ;
UNSCALE_BT_BASIS ( body_transform ) ;
2017-11-04 19:52:59 +00:00
2021-08-18 20:18:19 +00:00
if ( ! p_body - > get_kinematic_utilities ( ) ) {
p_body - > init_kinematic_utilities ( ) ;
}
2018-05-27 12:55:52 +00:00
btVector3 initial_recover_motion ( 0 , 0 , 0 ) ;
2017-12-23 17:23:12 +00:00
{ /// Phase one - multi shapes depenetration using margin
for ( int t ( RECOVERING_MOVEMENT_CYCLES ) ; 0 < t ; - - t ) {
2021-08-10 01:16:45 +00:00
if ( ! recover_from_penetration ( p_body , body_transform , RECOVERING_MOVEMENT_SCALE , p_infinite_inertia , initial_recover_motion , nullptr , p_exclude ) ) {
2017-12-23 17:23:12 +00:00
break ;
}
2017-11-07 14:22:09 +00:00
}
2018-01-08 00:22:54 +00:00
// Add recover movement in order to make it safe
2018-05-27 12:55:52 +00:00
body_transform . getOrigin ( ) + = initial_recover_motion ;
2017-11-07 14:22:09 +00:00
}
2017-11-04 19:52:59 +00:00
2018-01-08 00:22:54 +00:00
btVector3 motion ;
G_TO_B ( p_motion , motion ) ;
2021-08-18 20:18:19 +00:00
real_t total_length = motion . length ( ) ;
real_t unsafe_fraction = 1.0 ;
real_t safe_fraction = 1.0 ;
2019-12-09 14:06:25 +00:00
{
2019-12-08 18:59:24 +00:00
// Phase two - sweep test, from a secure position without margin
2017-11-04 19:52:59 +00:00
2018-01-08 00:22:54 +00:00
const int shape_count ( p_body - > get_shape_count ( ) ) ;
2017-11-04 19:52:59 +00:00
# if debug_test_motion
2018-01-08 00:22:54 +00:00
Vector3 sup_line ;
B_TO_G ( body_safe_position . getOrigin ( ) , sup_line ) ;
motionVec - > clear ( ) ;
2020-04-01 23:20:12 +00:00
motionVec - > begin ( Mesh : : PRIMITIVE_LINES , nullptr ) ;
2018-01-08 00:22:54 +00:00
motionVec - > add_vertex ( sup_line ) ;
motionVec - > add_vertex ( sup_line + p_motion * 10 ) ;
motionVec - > end ( ) ;
2017-11-04 19:52:59 +00:00
# endif
2020-07-18 12:10:27 +00:00
for ( int shIndex = 0 ; shIndex < shape_count ; + + shIndex ) {
2017-11-04 19:52:59 +00:00
if ( p_body - > is_shape_disabled ( shIndex ) ) {
continue ;
}
2017-11-19 16:11:47 +00:00
if ( ! p_body - > get_bt_shape ( shIndex ) - > isConvex ( ) ) {
2017-11-04 19:52:59 +00:00
// Skip no convex shape
continue ;
}
2018-08-14 17:20:48 +00:00
2018-08-20 20:31:55 +00:00
if ( p_exclude_raycast_shapes & & p_body - > get_bt_shape ( shIndex ) - > getShapeType ( ) = = CUSTOM_CONVEX_SHAPE_TYPE ) {
2018-08-14 17:20:48 +00:00
// Skip rayshape in order to implement custom separation process
continue ;
}
2017-11-19 16:11:47 +00:00
btConvexShape * convex_shape_test ( static_cast < btConvexShape * > ( p_body - > get_bt_shape ( shIndex ) ) ) ;
2017-11-04 19:52:59 +00:00
2018-05-27 12:55:52 +00:00
btTransform shape_world_from = body_transform * p_body - > get_kinematic_utilities ( ) - > shapes [ shIndex ] . transform ;
2017-11-04 19:52:59 +00:00
2017-11-07 14:22:09 +00:00
btTransform shape_world_to ( shape_world_from ) ;
2018-01-08 00:22:54 +00:00
shape_world_to . getOrigin ( ) + = motion ;
2017-11-04 19:52:59 +00:00
2020-07-18 12:10:27 +00:00
if ( ( shape_world_to . getOrigin ( ) - shape_world_from . getOrigin ( ) ) . fuzzyZero ( ) ) {
motion = btVector3 ( 0 , 0 , 0 ) ;
break ;
}
2021-08-10 01:16:45 +00:00
GodotKinClosestConvexResultCallback btResult ( shape_world_from . getOrigin ( ) , shape_world_to . getOrigin ( ) , p_body , p_infinite_inertia , & p_exclude ) ;
2017-11-04 19:52:59 +00:00
btResult . m_collisionFilterGroup = p_body - > get_collision_layer ( ) ;
btResult . m_collisionFilterMask = p_body - > get_collision_mask ( ) ;
2018-02-25 20:05:14 +00:00
dynamicsWorld - > convexSweepTest ( convex_shape_test , shape_world_from , shape_world_to , btResult , dynamicsWorld - > getDispatchInfo ( ) . m_allowedCcdPenetration ) ;
2017-11-04 19:52:59 +00:00
if ( btResult . hasHit ( ) ) {
2021-08-18 20:18:19 +00:00
if ( total_length > CMP_EPSILON ) {
real_t hit_fraction = btResult . m_closestHitFraction * motion . length ( ) / total_length ;
if ( hit_fraction < unsafe_fraction ) {
unsafe_fraction = hit_fraction ;
real_t margin = p_body - > get_kinematic_utilities ( ) - > safe_margin ;
safe_fraction = MAX ( hit_fraction - ( 1 - ( ( total_length - margin ) / total_length ) ) , 0 ) ;
}
}
2017-11-04 19:52:59 +00:00
/// Since for each sweep test I fix the motion of new shapes in base the recover result,
2017-11-07 14:22:09 +00:00
/// if another shape will hit something it means that has a deepest penetration respect the previous shape
2018-01-08 00:22:54 +00:00
motion * = btResult . m_closestHitFraction ;
2017-11-04 19:52:59 +00:00
}
}
2018-01-08 00:22:54 +00:00
2018-05-27 12:55:52 +00:00
body_transform . getOrigin ( ) + = motion ;
2017-11-04 19:52:59 +00:00
}
2017-12-23 17:23:12 +00:00
bool has_penetration = false ;
2017-11-04 19:52:59 +00:00
2018-05-27 12:55:52 +00:00
{ /// Phase three - contact test with margin
2017-11-04 19:52:59 +00:00
2018-05-27 12:55:52 +00:00
btVector3 __rec ( 0 , 0 , 0 ) ;
2017-11-19 01:04:49 +00:00
RecoverResult r_recover_result ;
2017-11-04 19:52:59 +00:00
2021-08-10 01:16:45 +00:00
has_penetration = recover_from_penetration ( p_body , body_transform , 1 , p_infinite_inertia , __rec , & r_recover_result , p_exclude ) ;
2017-11-04 19:52:59 +00:00
2018-05-27 12:55:52 +00:00
// Parse results
if ( r_result ) {
2018-08-11 08:25:56 +00:00
B_TO_G ( motion + initial_recover_motion + __rec , r_result - > motion ) ;
2018-02-25 20:05:14 +00:00
2018-05-27 12:55:52 +00:00
if ( has_penetration ) {
const btRigidBody * btRigid = static_cast < const btRigidBody * > ( r_recover_result . other_collision_object ) ;
CollisionObjectBullet * collisionObject = static_cast < CollisionObjectBullet * > ( btRigid - > getUserPointer ( ) ) ;
2017-12-23 17:23:12 +00:00
2018-05-27 12:55:52 +00:00
B_TO_G ( motion , r_result - > remainder ) ; // is the remaining movements
r_result - > remainder = p_motion - r_result - > remainder ;
2017-12-23 17:23:12 +00:00
2018-05-27 12:55:52 +00:00
B_TO_G ( r_recover_result . pointWorld , r_result - > collision_point ) ;
B_TO_G ( r_recover_result . normal , r_result - > collision_normal ) ;
B_TO_G ( btRigid - > getVelocityInLocalPoint ( r_recover_result . pointWorld - btRigid - > getWorldTransform ( ) . getOrigin ( ) ) , r_result - > collider_velocity ) ; // It calculates velocity at point and assign it using special function Bullet_to_Godot
r_result - > collider = collisionObject - > get_self ( ) ;
r_result - > collider_id = collisionObject - > get_instance_id ( ) ;
r_result - > collider_shape = r_recover_result . other_compound_shape_index ;
r_result - > collision_local_shape = r_recover_result . local_shape_most_recovered ;
2021-08-18 20:18:19 +00:00
r_result - > collision_depth = Math : : abs ( r_recover_result . penetration_distance ) ;
r_result - > collision_safe_fraction = safe_fraction ;
r_result - > collision_unsafe_fraction = unsafe_fraction ;
2017-12-23 17:23:12 +00:00
2017-11-04 19:52:59 +00:00
# if debug_test_motion
2018-05-27 12:55:52 +00:00
Vector3 sup_line2 ;
B_TO_G ( motion , sup_line2 ) ;
normalLine - > clear ( ) ;
2020-04-01 23:20:12 +00:00
normalLine - > begin ( Mesh : : PRIMITIVE_LINES , nullptr ) ;
2018-05-27 12:55:52 +00:00
normalLine - > add_vertex ( r_result - > collision_point ) ;
normalLine - > add_vertex ( r_result - > collision_point + r_result - > collision_normal * 10 ) ;
normalLine - > end ( ) ;
2017-11-04 19:52:59 +00:00
# endif
} else {
2018-05-27 12:55:52 +00:00
r_result - > remainder = Vector3 ( ) ;
2017-11-04 19:52:59 +00:00
}
}
}
2017-12-23 17:23:12 +00:00
return has_penetration ;
2017-11-04 19:52:59 +00:00
}
2020-10-17 05:08:21 +00:00
int SpaceBullet : : test_ray_separation ( RigidBodyBullet * p_body , const Transform3D & p_transform , bool p_infinite_inertia , Vector3 & r_recover_motion , PhysicsServer3D : : SeparationResult * r_results , int p_result_max , real_t p_margin ) {
2018-08-14 17:20:48 +00:00
btTransform body_transform ;
G_TO_B ( p_transform , body_transform ) ;
UNSCALE_BT_BASIS ( body_transform ) ;
2021-08-18 20:18:19 +00:00
if ( ! p_body - > get_kinematic_utilities ( ) ) {
p_body - > init_kinematic_utilities ( ) ;
}
2018-08-14 17:20:48 +00:00
btVector3 recover_motion ( 0 , 0 , 0 ) ;
2018-08-29 12:23:14 +00:00
int rays_found = 0 ;
2019-03-25 21:46:26 +00:00
int rays_found_this_round = 0 ;
2018-08-14 17:20:48 +00:00
for ( int t ( RECOVERING_MOVEMENT_CYCLES ) ; 0 < t ; - - t ) {
2020-03-27 18:21:27 +00:00
PhysicsServer3D : : SeparationResult * next_results = & r_results [ rays_found ] ;
2019-03-25 21:46:26 +00:00
rays_found_this_round = recover_from_penetration_ray ( p_body , body_transform , RECOVERING_MOVEMENT_SCALE , p_infinite_inertia , p_result_max - rays_found , recover_motion , next_results ) ;
2018-08-14 17:20:48 +00:00
2019-03-25 21:46:26 +00:00
rays_found + = rays_found_this_round ;
if ( rays_found_this_round = = 0 ) {
2018-08-14 17:20:48 +00:00
body_transform . getOrigin ( ) + = recover_motion ;
2019-03-25 21:46:26 +00:00
break ;
2018-08-14 17:20:48 +00:00
}
}
B_TO_G ( recover_motion , r_recover_motion ) ;
return rays_found ;
}
2017-11-07 14:22:09 +00:00
struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback {
private :
2019-03-25 21:46:26 +00:00
btDbvtVolume bounds ;
2017-11-07 14:22:09 +00:00
const btCollisionObject * self_collision_object ;
2021-02-08 09:57:18 +00:00
uint32_t collision_layer = 0 ;
2017-11-04 19:52:59 +00:00
2019-03-25 21:46:26 +00:00
struct CompoundLeafCallback : btDbvt : : ICollide {
private :
2021-02-08 09:57:18 +00:00
RecoverPenetrationBroadPhaseCallback * parent_callback = nullptr ;
btCollisionObject * collision_object = nullptr ;
2019-03-25 21:46:26 +00:00
public :
CompoundLeafCallback ( RecoverPenetrationBroadPhaseCallback * p_parent_callback , btCollisionObject * p_collision_object ) :
parent_callback ( p_parent_callback ) ,
collision_object ( p_collision_object ) {
}
void Process ( const btDbvtNode * leaf ) {
BroadphaseResult result ;
result . collision_object = collision_object ;
result . compound_child_index = leaf - > dataAsInt ;
parent_callback - > results . push_back ( result ) ;
}
} ;
2017-11-07 14:22:09 +00:00
public :
2019-03-25 21:46:26 +00:00
struct BroadphaseResult {
2021-02-08 09:57:18 +00:00
btCollisionObject * collision_object = nullptr ;
int compound_child_index = 0 ;
2019-03-25 21:46:26 +00:00
} ;
Vector < BroadphaseResult > results ;
2017-11-04 19:52:59 +00:00
2017-11-07 14:22:09 +00:00
public :
2020-10-08 11:45:03 +00:00
RecoverPenetrationBroadPhaseCallback ( const btCollisionObject * p_self_collision_object , uint32_t p_collision_layer , btVector3 p_aabb_min , btVector3 p_aabb_max ) :
2017-12-06 20:36:34 +00:00
self_collision_object ( p_self_collision_object ) ,
2020-10-08 11:45:03 +00:00
collision_layer ( p_collision_layer ) {
2019-03-25 21:46:26 +00:00
bounds = btDbvtVolume : : FromMM ( p_aabb_min , p_aabb_max ) ;
}
2017-11-07 14:22:09 +00:00
virtual ~ RecoverPenetrationBroadPhaseCallback ( ) { }
virtual bool process ( const btBroadphaseProxy * proxy ) {
btCollisionObject * co = static_cast < btCollisionObject * > ( proxy - > m_clientObject ) ;
if ( co - > getInternalType ( ) < = btCollisionObject : : CO_RIGID_BODY ) {
2021-07-15 19:38:48 +00:00
if ( self_collision_object ! = proxy - > m_clientObject & & ( proxy - > collision_layer & m_collisionFilterMask ) ) {
2019-03-25 21:46:26 +00:00
if ( co - > getCollisionShape ( ) - > isCompound ( ) ) {
const btCompoundShape * cs = static_cast < btCompoundShape * > ( co - > getCollisionShape ( ) ) ;
if ( cs - > getNumChildShapes ( ) > 1 ) {
const btDbvt * tree = cs - > getDynamicAabbTree ( ) ;
2020-04-01 23:20:12 +00:00
ERR_FAIL_COND_V ( tree = = nullptr , true ) ;
2019-03-25 21:46:26 +00:00
// Transform bounds into compound shape local space
const btTransform other_in_compound_space = co - > getWorldTransform ( ) . inverse ( ) ;
const btMatrix3x3 abs_b = other_in_compound_space . getBasis ( ) . absolute ( ) ;
const btVector3 local_center = other_in_compound_space ( bounds . Center ( ) ) ;
const btVector3 local_extent = bounds . Extents ( ) . dot3 ( abs_b [ 0 ] , abs_b [ 1 ] , abs_b [ 2 ] ) ;
const btVector3 local_aabb_min = local_center - local_extent ;
const btVector3 local_aabb_max = local_center + local_extent ;
const btDbvtVolume local_bounds = btDbvtVolume : : FromMM ( local_aabb_min , local_aabb_max ) ;
// Test collision against compound child shapes using its AABB tree
CompoundLeafCallback compound_leaf_callback ( this , co ) ;
tree - > collideTV ( tree - > m_root , local_bounds , compound_leaf_callback ) ;
} else {
// If there's only a single child shape then there's no need to search any more, we know which child overlaps
BroadphaseResult result ;
result . collision_object = co ;
result . compound_child_index = 0 ;
results . push_back ( result ) ;
}
} else {
BroadphaseResult result ;
result . collision_object = co ;
result . compound_child_index = - 1 ;
results . push_back ( result ) ;
}
2017-11-07 14:22:09 +00:00
return true ;
}
2017-11-04 19:52:59 +00:00
}
2017-11-07 14:22:09 +00:00
return false ;
}
} ;
2021-08-10 01:16:45 +00:00
bool SpaceBullet : : recover_from_penetration ( RigidBodyBullet * p_body , const btTransform & p_body_position , btScalar p_recover_movement_scale , bool p_infinite_inertia , btVector3 & r_delta_recover_movement , RecoverResult * r_recover_result , const Set < RID > & p_exclude ) {
2019-05-19 10:34:40 +00:00
// Calculate the cumulative AABB of all shapes of the kinematic body
2019-03-25 21:46:26 +00:00
btVector3 aabb_min , aabb_max ;
bool shapes_found = false ;
for ( int kinIndex = p_body - > get_kinematic_utilities ( ) - > shapes . size ( ) - 1 ; 0 < = kinIndex ; - - kinIndex ) {
const RigidBodyBullet : : KinematicShape & kin_shape ( p_body - > get_kinematic_utilities ( ) - > shapes [ kinIndex ] ) ;
if ( ! kin_shape . is_active ( ) ) {
continue ;
}
if ( kin_shape . shape - > getShapeType ( ) = = CUSTOM_CONVEX_SHAPE_TYPE ) {
// Skip rayshape in order to implement custom separation process
continue ;
}
btTransform shape_transform = p_body_position * kin_shape . transform ;
shape_transform . getOrigin ( ) + = r_delta_recover_movement ;
btVector3 shape_aabb_min , shape_aabb_max ;
kin_shape . shape - > getAabb ( shape_transform , shape_aabb_min , shape_aabb_max ) ;
if ( ! shapes_found ) {
aabb_min = shape_aabb_min ;
aabb_max = shape_aabb_max ;
shapes_found = true ;
} else {
aabb_min . setX ( ( aabb_min . x ( ) < shape_aabb_min . x ( ) ) ? aabb_min . x ( ) : shape_aabb_min . x ( ) ) ;
aabb_min . setY ( ( aabb_min . y ( ) < shape_aabb_min . y ( ) ) ? aabb_min . y ( ) : shape_aabb_min . y ( ) ) ;
aabb_min . setZ ( ( aabb_min . z ( ) < shape_aabb_min . z ( ) ) ? aabb_min . z ( ) : shape_aabb_min . z ( ) ) ;
aabb_max . setX ( ( aabb_max . x ( ) > shape_aabb_max . x ( ) ) ? aabb_max . x ( ) : shape_aabb_max . x ( ) ) ;
aabb_max . setY ( ( aabb_max . y ( ) > shape_aabb_max . y ( ) ) ? aabb_max . y ( ) : shape_aabb_max . y ( ) ) ;
aabb_max . setZ ( ( aabb_max . z ( ) > shape_aabb_max . z ( ) ) ? aabb_max . z ( ) : shape_aabb_max . z ( ) ) ;
}
}
2017-11-04 19:52:59 +00:00
2019-03-25 21:46:26 +00:00
// If there are no shapes then there is no penetration either
if ( ! shapes_found ) {
return false ;
}
2017-11-04 19:52:59 +00:00
2019-03-25 21:46:26 +00:00
// Perform broadphase test
2020-10-08 11:45:03 +00:00
RecoverPenetrationBroadPhaseCallback recover_broad_result ( p_body - > get_bt_collision_object ( ) , p_body - > get_collision_layer ( ) , aabb_min , aabb_max ) ;
2019-03-25 21:46:26 +00:00
dynamicsWorld - > getBroadphase ( ) - > aabbTest ( aabb_min , aabb_max , recover_broad_result ) ;
2017-11-07 14:22:09 +00:00
bool penetration = false ;
2017-11-04 19:52:59 +00:00
2019-03-25 21:46:26 +00:00
// Perform narrowphase per shape
2017-11-07 14:22:09 +00:00
for ( int kinIndex = p_body - > get_kinematic_utilities ( ) - > shapes . size ( ) - 1 ; 0 < = kinIndex ; - - kinIndex ) {
const RigidBodyBullet : : KinematicShape & kin_shape ( p_body - > get_kinematic_utilities ( ) - > shapes [ kinIndex ] ) ;
if ( ! kin_shape . is_active ( ) ) {
continue ;
}
2017-11-04 19:52:59 +00:00
2018-08-14 17:20:48 +00:00
if ( kin_shape . shape - > getShapeType ( ) = = CUSTOM_CONVEX_SHAPE_TYPE ) {
// Skip rayshape in order to implement custom separation process
continue ;
}
2021-04-06 17:33:30 +00:00
if ( kin_shape . shape - > getShapeType ( ) = = EMPTY_SHAPE_PROXYTYPE ) {
continue ;
}
2019-03-25 21:46:26 +00:00
btTransform shape_transform = p_body_position * kin_shape . transform ;
shape_transform . getOrigin ( ) + = r_delta_recover_movement ;
2017-11-04 19:52:59 +00:00
2019-03-25 21:46:26 +00:00
for ( int i = recover_broad_result . results . size ( ) - 1 ; 0 < = i ; - - i ) {
btCollisionObject * otherObject = recover_broad_result . results [ i ] . collision_object ;
2021-08-10 01:16:45 +00:00
CollisionObjectBullet * gObj = static_cast < CollisionObjectBullet * > ( otherObject - > getUserPointer ( ) ) ;
if ( p_exclude . has ( gObj - > get_self ( ) ) ) {
continue ;
}
2018-04-12 05:59:12 +00:00
if ( p_infinite_inertia & & ! otherObject - > isStaticOrKinematicObject ( ) ) {
otherObject - > activate ( ) ; // Force activation of hitten rigid, soft body
continue ;
2020-05-14 14:41:43 +00:00
} else if ( ! p_body - > get_bt_collision_object ( ) - > checkCollideWith ( otherObject ) | | ! otherObject - > checkCollideWith ( p_body - > get_bt_collision_object ( ) ) ) {
2017-11-04 19:52:59 +00:00
continue ;
2020-05-14 14:41:43 +00:00
}
2017-11-04 19:52:59 +00:00
2017-11-19 01:04:49 +00:00
if ( otherObject - > getCollisionShape ( ) - > isCompound ( ) ) {
2019-03-25 21:46:26 +00:00
const btCompoundShape * cs = static_cast < const btCompoundShape * > ( otherObject - > getCollisionShape ( ) ) ;
int shape_idx = recover_broad_result . results [ i ] . compound_child_index ;
ERR_FAIL_COND_V ( shape_idx < 0 | | shape_idx > = cs - > getNumChildShapes ( ) , false ) ;
2017-11-04 19:52:59 +00:00
2019-03-25 21:46:26 +00:00
if ( cs - > getChildShape ( shape_idx ) - > isConvex ( ) ) {
2019-08-07 10:32:32 +00:00
if ( RFP_convex_convex_test ( kin_shape . shape , static_cast < const btConvexShape * > ( cs - > getChildShape ( shape_idx ) ) , otherObject , kinIndex , shape_idx , shape_transform , otherObject - > getWorldTransform ( ) * cs - > getChildTransform ( shape_idx ) , p_recover_movement_scale , r_delta_recover_movement , r_recover_result ) ) {
2019-03-25 21:46:26 +00:00
penetration = true ;
}
} else {
if ( RFP_convex_world_test ( kin_shape . shape , cs - > getChildShape ( shape_idx ) , p_body - > get_bt_collision_object ( ) , otherObject , kinIndex , shape_idx , shape_transform , otherObject - > getWorldTransform ( ) * cs - > getChildTransform ( shape_idx ) , p_recover_movement_scale , r_delta_recover_movement , r_recover_result ) ) {
penetration = true ;
2017-11-07 14:22:09 +00:00
}
}
} else if ( otherObject - > getCollisionShape ( ) - > isConvex ( ) ) { /// Execute GJK test against object shape
2019-08-07 10:32:32 +00:00
if ( RFP_convex_convex_test ( kin_shape . shape , static_cast < const btConvexShape * > ( otherObject - > getCollisionShape ( ) ) , otherObject , kinIndex , 0 , shape_transform , otherObject - > getWorldTransform ( ) , p_recover_movement_scale , r_delta_recover_movement , r_recover_result ) ) {
2017-11-07 14:22:09 +00:00
penetration = true ;
2017-11-19 01:04:49 +00:00
}
} else {
2019-03-25 21:46:26 +00:00
if ( RFP_convex_world_test ( kin_shape . shape , otherObject - > getCollisionShape ( ) , p_body - > get_bt_collision_object ( ) , otherObject , kinIndex , 0 , shape_transform , otherObject - > getWorldTransform ( ) , p_recover_movement_scale , r_delta_recover_movement , r_recover_result ) ) {
2017-11-19 01:04:49 +00:00
penetration = true ;
2017-11-04 19:52:59 +00:00
}
}
}
}
return penetration ;
}
2017-11-19 01:04:49 +00:00
2019-08-07 10:32:32 +00:00
bool SpaceBullet : : RFP_convex_convex_test ( const btConvexShape * p_shapeA , const btConvexShape * p_shapeB , btCollisionObject * p_objectB , int p_shapeId_A , int p_shapeId_B , const btTransform & p_transformA , const btTransform & p_transformB , btScalar p_recover_movement_scale , btVector3 & r_delta_recover_movement , RecoverResult * r_recover_result ) {
2017-11-19 01:04:49 +00:00
// Initialize GJK input
btGjkPairDetector : : ClosestPointInput gjk_input ;
gjk_input . m_transformA = p_transformA ;
gjk_input . m_transformB = p_transformB ;
// Perform GJK test
btPointCollector result ;
btGjkPairDetector gjk_pair_detector ( p_shapeA , p_shapeB , gjk_simplex_solver , gjk_epa_pen_solver ) ;
2020-04-01 23:20:12 +00:00
gjk_pair_detector . getClosestPoints ( gjk_input , result , nullptr ) ;
2017-11-19 01:04:49 +00:00
if ( 0 > result . m_distance ) {
// Has penetration
2018-01-08 00:22:54 +00:00
r_delta_recover_movement + = result . m_normalOnBInWorld * ( result . m_distance * - 1 * p_recover_movement_scale ) ;
2017-11-19 01:04:49 +00:00
if ( r_recover_result ) {
2017-12-23 17:23:12 +00:00
if ( result . m_distance < r_recover_result - > penetration_distance ) {
r_recover_result - > hasPenetration = true ;
2019-08-07 10:32:32 +00:00
r_recover_result - > local_shape_most_recovered = p_shapeId_A ;
2017-12-23 17:23:12 +00:00
r_recover_result - > other_collision_object = p_objectB ;
r_recover_result - > other_compound_shape_index = p_shapeId_B ;
r_recover_result - > penetration_distance = result . m_distance ;
r_recover_result - > pointWorld = result . m_pointInWorld ;
r_recover_result - > normal = result . m_normalOnBInWorld ;
}
2017-11-19 01:04:49 +00:00
}
return true ;
}
return false ;
}
2018-01-08 00:22:54 +00:00
bool SpaceBullet : : RFP_convex_world_test ( const btConvexShape * p_shapeA , const btCollisionShape * p_shapeB , btCollisionObject * p_objectA , btCollisionObject * p_objectB , int p_shapeId_A , int p_shapeId_B , const btTransform & p_transformA , const btTransform & p_transformB , btScalar p_recover_movement_scale , btVector3 & r_delta_recover_movement , RecoverResult * r_recover_result ) {
2017-11-19 01:04:49 +00:00
/// Contact test
2018-01-08 00:22:54 +00:00
btTransform tA ( p_transformA ) ;
2020-04-01 23:20:12 +00:00
btCollisionObjectWrapper obA ( nullptr , p_shapeA , p_objectA , tA , - 1 , p_shapeId_A ) ;
btCollisionObjectWrapper obB ( nullptr , p_shapeB , p_objectB , p_transformB , - 1 , p_shapeId_B ) ;
2017-11-19 01:04:49 +00:00
2020-04-01 23:20:12 +00:00
btCollisionAlgorithm * algorithm = dispatcher - > findAlgorithm ( & obA , & obB , nullptr , BT_CONTACT_POINT_ALGORITHMS ) ;
2017-11-19 01:04:49 +00:00
if ( algorithm ) {
GodotDeepPenetrationContactResultCallback contactPointResult ( & obA , & obB ) ;
//discrete collision detection query
algorithm - > processCollision ( & obA , & obB , dynamicsWorld - > getDispatchInfo ( ) , & contactPointResult ) ;
algorithm - > ~ btCollisionAlgorithm ( ) ;
dispatcher - > freeCollisionAlgorithm ( algorithm ) ;
if ( contactPointResult . hasHit ( ) ) {
2018-04-04 08:49:10 +00:00
r_delta_recover_movement + = contactPointResult . m_pointNormalWorld * ( contactPointResult . m_penetration_distance * - 1 * p_recover_movement_scale ) ;
2017-11-19 01:04:49 +00:00
if ( r_recover_result ) {
2017-12-23 17:23:12 +00:00
if ( contactPointResult . m_penetration_distance < r_recover_result - > penetration_distance ) {
r_recover_result - > hasPenetration = true ;
2019-08-07 10:32:32 +00:00
r_recover_result - > local_shape_most_recovered = p_shapeId_A ;
2017-12-23 17:23:12 +00:00
r_recover_result - > other_collision_object = p_objectB ;
r_recover_result - > other_compound_shape_index = p_shapeId_B ;
r_recover_result - > penetration_distance = contactPointResult . m_penetration_distance ;
r_recover_result - > pointWorld = contactPointResult . m_pointWorld ;
r_recover_result - > normal = contactPointResult . m_pointNormalWorld ;
}
2017-11-19 01:04:49 +00:00
}
return true ;
}
}
return false ;
}
2018-08-14 17:20:48 +00:00
2020-03-27 18:21:27 +00:00
int SpaceBullet : : add_separation_result ( PhysicsServer3D : : SeparationResult * r_result , const SpaceBullet : : RecoverResult & p_recover_result , int p_shape_id , const btCollisionObject * p_other_object ) const {
2019-03-25 21:46:26 +00:00
// optimize results (ignore non-colliding)
if ( p_recover_result . penetration_distance < 0.0 ) {
const btRigidBody * btRigid = static_cast < const btRigidBody * > ( p_other_object ) ;
CollisionObjectBullet * collisionObject = static_cast < CollisionObjectBullet * > ( p_other_object - > getUserPointer ( ) ) ;
2019-02-11 00:48:33 +00:00
2019-03-25 21:46:26 +00:00
r_result - > collision_depth = p_recover_result . penetration_distance ;
B_TO_G ( p_recover_result . pointWorld , r_result - > collision_point ) ;
B_TO_G ( p_recover_result . normal , r_result - > collision_normal ) ;
B_TO_G ( btRigid - > getVelocityInLocalPoint ( p_recover_result . pointWorld - btRigid - > getWorldTransform ( ) . getOrigin ( ) ) , r_result - > collider_velocity ) ;
r_result - > collision_local_shape = p_shape_id ;
r_result - > collider_id = collisionObject - > get_instance_id ( ) ;
r_result - > collider = collisionObject - > get_self ( ) ;
r_result - > collider_shape = p_recover_result . other_compound_shape_index ;
2019-02-11 00:48:33 +00:00
2019-03-25 21:46:26 +00:00
return 1 ;
} else {
return 0 ;
}
2019-02-11 00:48:33 +00:00
}
2020-03-27 18:21:27 +00:00
int SpaceBullet : : recover_from_penetration_ray ( RigidBodyBullet * p_body , const btTransform & p_body_position , btScalar p_recover_movement_scale , bool p_infinite_inertia , int p_result_max , btVector3 & r_delta_recover_movement , PhysicsServer3D : : SeparationResult * r_results ) {
2019-05-19 10:34:40 +00:00
// Calculate the cumulative AABB of all shapes of the kinematic body
2019-03-25 21:46:26 +00:00
btVector3 aabb_min , aabb_max ;
bool shapes_found = false ;
for ( int kinIndex = p_body - > get_kinematic_utilities ( ) - > shapes . size ( ) - 1 ; 0 < = kinIndex ; - - kinIndex ) {
const RigidBodyBullet : : KinematicShape & kin_shape ( p_body - > get_kinematic_utilities ( ) - > shapes [ kinIndex ] ) ;
if ( ! kin_shape . is_active ( ) ) {
continue ;
}
if ( kin_shape . shape - > getShapeType ( ) ! = CUSTOM_CONVEX_SHAPE_TYPE ) {
continue ;
}
btTransform shape_transform = p_body_position * kin_shape . transform ;
shape_transform . getOrigin ( ) + = r_delta_recover_movement ;
2018-08-14 17:20:48 +00:00
2019-03-25 21:46:26 +00:00
btVector3 shape_aabb_min , shape_aabb_max ;
kin_shape . shape - > getAabb ( shape_transform , shape_aabb_min , shape_aabb_max ) ;
2018-08-14 17:20:48 +00:00
2019-03-25 21:46:26 +00:00
if ( ! shapes_found ) {
aabb_min = shape_aabb_min ;
aabb_max = shape_aabb_max ;
shapes_found = true ;
} else {
aabb_min . setX ( ( aabb_min . x ( ) < shape_aabb_min . x ( ) ) ? aabb_min . x ( ) : shape_aabb_min . x ( ) ) ;
aabb_min . setY ( ( aabb_min . y ( ) < shape_aabb_min . y ( ) ) ? aabb_min . y ( ) : shape_aabb_min . y ( ) ) ;
aabb_min . setZ ( ( aabb_min . z ( ) < shape_aabb_min . z ( ) ) ? aabb_min . z ( ) : shape_aabb_min . z ( ) ) ;
2018-08-14 17:20:48 +00:00
2019-03-25 21:46:26 +00:00
aabb_max . setX ( ( aabb_max . x ( ) > shape_aabb_max . x ( ) ) ? aabb_max . x ( ) : shape_aabb_max . x ( ) ) ;
aabb_max . setY ( ( aabb_max . y ( ) > shape_aabb_max . y ( ) ) ? aabb_max . y ( ) : shape_aabb_max . y ( ) ) ;
aabb_max . setZ ( ( aabb_max . z ( ) > shape_aabb_max . z ( ) ) ? aabb_max . z ( ) : shape_aabb_max . z ( ) ) ;
}
}
2018-08-14 17:20:48 +00:00
2019-03-25 21:46:26 +00:00
// If there are no shapes then there is no penetration either
if ( ! shapes_found ) {
return 0 ;
}
// Perform broadphase test
2020-10-08 11:45:03 +00:00
RecoverPenetrationBroadPhaseCallback recover_broad_result ( p_body - > get_bt_collision_object ( ) , p_body - > get_collision_layer ( ) , aabb_min , aabb_max ) ;
2019-03-25 21:46:26 +00:00
dynamicsWorld - > getBroadphase ( ) - > aabbTest ( aabb_min , aabb_max , recover_broad_result ) ;
2018-08-14 17:20:48 +00:00
2019-03-25 21:46:26 +00:00
int ray_count = 0 ;
// Perform narrowphase per shape
for ( int kinIndex = p_body - > get_kinematic_utilities ( ) - > shapes . size ( ) - 1 ; 0 < = kinIndex ; - - kinIndex ) {
if ( ray_count > = p_result_max ) {
2018-08-14 17:20:48 +00:00
break ;
}
const RigidBodyBullet : : KinematicShape & kin_shape ( p_body - > get_kinematic_utilities ( ) - > shapes [ kinIndex ] ) ;
if ( ! kin_shape . is_active ( ) ) {
continue ;
}
if ( kin_shape . shape - > getShapeType ( ) ! = CUSTOM_CONVEX_SHAPE_TYPE ) {
continue ;
}
2019-03-25 21:46:26 +00:00
btTransform shape_transform = p_body_position * kin_shape . transform ;
shape_transform . getOrigin ( ) + = r_delta_recover_movement ;
2018-08-14 17:20:48 +00:00
2019-03-25 21:46:26 +00:00
for ( int i = recover_broad_result . results . size ( ) - 1 ; 0 < = i ; - - i ) {
btCollisionObject * otherObject = recover_broad_result . results [ i ] . collision_object ;
2018-08-14 17:20:48 +00:00
if ( p_infinite_inertia & & ! otherObject - > isStaticOrKinematicObject ( ) ) {
otherObject - > activate ( ) ; // Force activation of hitten rigid, soft body
continue ;
2020-05-14 14:41:43 +00:00
} else if ( ! p_body - > get_bt_collision_object ( ) - > checkCollideWith ( otherObject ) | | ! otherObject - > checkCollideWith ( p_body - > get_bt_collision_object ( ) ) ) {
2018-08-14 17:20:48 +00:00
continue ;
2020-05-14 14:41:43 +00:00
}
2018-08-14 17:20:48 +00:00
if ( otherObject - > getCollisionShape ( ) - > isCompound ( ) ) {
2019-03-25 21:46:26 +00:00
const btCompoundShape * cs = static_cast < const btCompoundShape * > ( otherObject - > getCollisionShape ( ) ) ;
int shape_idx = recover_broad_result . results [ i ] . compound_child_index ;
ERR_FAIL_COND_V ( shape_idx < 0 | | shape_idx > = cs - > getNumChildShapes ( ) , false ) ;
2018-08-14 17:20:48 +00:00
2019-03-25 21:46:26 +00:00
RecoverResult recover_result ;
if ( RFP_convex_world_test ( kin_shape . shape , cs - > getChildShape ( shape_idx ) , p_body - > get_bt_collision_object ( ) , otherObject , kinIndex , shape_idx , shape_transform , otherObject - > getWorldTransform ( ) * cs - > getChildTransform ( shape_idx ) , p_recover_movement_scale , r_delta_recover_movement , & recover_result ) ) {
ray_count = add_separation_result ( & r_results [ ray_count ] , recover_result , kinIndex , otherObject ) ;
2018-08-14 17:20:48 +00:00
}
2019-02-11 00:48:33 +00:00
} else {
RecoverResult recover_result ;
2019-03-25 21:46:26 +00:00
if ( RFP_convex_world_test ( kin_shape . shape , otherObject - > getCollisionShape ( ) , p_body - > get_bt_collision_object ( ) , otherObject , kinIndex , 0 , shape_transform , otherObject - > getWorldTransform ( ) , p_recover_movement_scale , r_delta_recover_movement , & recover_result ) ) {
ray_count = add_separation_result ( & r_results [ ray_count ] , recover_result , kinIndex , otherObject ) ;
2019-02-11 00:48:33 +00:00
}
2018-08-14 17:20:48 +00:00
}
}
}
2019-03-25 21:46:26 +00:00
return ray_count ;
2018-08-14 17:20:48 +00:00
}