2023-01-05 13:25:55 +01:00
/**************************************************************************/
/* godot_body_pair_3d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
2018-01-05 00:50:27 +01:00
2021-10-18 21:24:30 +02:00
# include "godot_body_pair_3d.h"
# include "godot_collision_solver_3d.h"
# include "godot_space_3d.h"
2017-08-27 21:07:15 +02:00
2018-09-11 18:13:45 +02:00
# include "core/os/os.h"
2014-02-10 02:10:30 +01:00
# define MIN_VELOCITY 0.0001
2017-09-18 01:01:19 +02:00
# define MAX_BIAS_ROTATION (Math_PI / 8)
2014-02-10 02:10:30 +01:00
2023-01-15 03:39:23 +01:00
void GodotBodyPair3D : : _contact_added_callback ( const Vector3 & p_point_A , int p_index_A , const Vector3 & p_point_B , int p_index_B , const Vector3 & normal , void * p_userdata ) {
2022-04-05 12:40:26 +02:00
GodotBodyPair3D * pair = static_cast < GodotBodyPair3D * > ( p_userdata ) ;
2023-01-15 03:39:23 +01:00
pair - > contact_added_callback ( p_point_A , p_index_A , p_point_B , p_index_B , normal ) ;
2014-02-10 02:10:30 +01:00
}
2023-01-15 03:39:23 +01:00
void GodotBodyPair3D : : contact_added_callback ( const Vector3 & p_point_A , int p_index_A , const Vector3 & p_point_B , int p_index_B , const Vector3 & normal ) {
2014-02-10 02:10:30 +01:00
Vector3 local_A = A - > get_inv_transform ( ) . basis . xform ( p_point_A ) ;
Vector3 local_B = B - > get_inv_transform ( ) . basis . xform ( p_point_B - offset_B ) ;
int new_index = contact_count ;
ERR_FAIL_COND ( new_index > = ( MAX_CONTACTS + 1 ) ) ;
Contact contact ;
2021-03-12 04:33:46 +01:00
contact . index_A = p_index_A ;
contact . index_B = p_index_B ;
2014-02-10 02:10:30 +01:00
contact . local_A = local_A ;
contact . local_B = local_B ;
contact . normal = ( p_point_A - p_point_B ) . normalized ( ) ;
2021-12-03 18:38:40 +01:00
contact . used = true ;
2014-02-10 02:10:30 +01:00
2021-12-03 18:38:40 +01:00
// Attempt to determine if the contact will be reused.
2014-02-10 02:10:30 +01:00
real_t contact_recycle_radius = space - > get_contact_recycle_radius ( ) ;
for ( int i = 0 ; i < contact_count ; i + + ) {
Contact & c = contacts [ i ] ;
2017-09-18 01:01:19 +02:00
if ( c . local_A . distance_squared_to ( local_A ) < ( contact_recycle_radius * contact_recycle_radius ) & &
2014-02-10 02:10:30 +01:00
c . local_B . distance_squared_to ( local_B ) < ( contact_recycle_radius * contact_recycle_radius ) ) {
contact . acc_normal_impulse = c . acc_normal_impulse ;
contact . acc_bias_impulse = c . acc_bias_impulse ;
2017-09-18 01:01:19 +02:00
contact . acc_bias_impulse_center_of_mass = c . acc_bias_impulse_center_of_mass ;
2014-02-10 02:10:30 +01:00
contact . acc_tangent_impulse = c . acc_tangent_impulse ;
2021-12-03 18:38:40 +01:00
c = contact ;
return ;
2014-02-10 02:10:30 +01:00
}
}
2021-12-03 18:38:40 +01:00
// Figure out if the contact amount must be reduced to fit the new contact.
2014-02-10 02:10:30 +01:00
if ( new_index = = MAX_CONTACTS ) {
2021-12-03 18:38:40 +01:00
// Remove the contact with the minimum depth.
const Basis & basis_A = A - > get_transform ( ) . basis ;
const Basis & basis_B = B - > get_transform ( ) . basis ;
2014-02-10 02:10:30 +01:00
int least_deep = - 1 ;
2021-12-03 18:38:40 +01:00
real_t min_depth ;
// Start with depth for new contact.
{
Vector3 global_A = basis_A . xform ( contact . local_A ) ;
Vector3 global_B = basis_B . xform ( contact . local_B ) + offset_B ;
Vector3 axis = global_A - global_B ;
min_depth = axis . dot ( contact . normal ) ;
}
2014-02-10 02:10:30 +01:00
2021-12-03 18:38:40 +01:00
for ( int i = 0 ; i < contact_count ; i + + ) {
const Contact & c = contacts [ i ] ;
Vector3 global_A = basis_A . xform ( c . local_A ) ;
Vector3 global_B = basis_B . xform ( c . local_B ) + offset_B ;
2014-02-10 02:10:30 +01:00
Vector3 axis = global_A - global_B ;
2017-02-14 00:25:05 +01:00
real_t depth = axis . dot ( c . normal ) ;
2014-02-10 02:10:30 +01:00
if ( depth < min_depth ) {
min_depth = depth ;
least_deep = i ;
}
}
2021-12-03 18:38:40 +01:00
if ( least_deep > - 1 ) {
// Replace the least deep contact by the new one.
2014-02-10 02:10:30 +01:00
contacts [ least_deep ] = contact ;
}
return ;
}
contacts [ new_index ] = contact ;
2021-12-03 18:38:40 +01:00
contact_count + + ;
2014-02-10 02:10:30 +01:00
}
2021-10-18 21:24:30 +02:00
void GodotBodyPair3D : : validate_contacts ( ) {
2021-12-03 18:38:40 +01:00
// Make sure to erase contacts that are no longer valid.
real_t max_separation = space - > get_contact_max_separation ( ) ;
real_t max_separation2 = max_separation * max_separation ;
const Basis & basis_A = A - > get_transform ( ) . basis ;
const Basis & basis_B = B - > get_transform ( ) . basis ;
2014-02-10 02:10:30 +01:00
for ( int i = 0 ; i < contact_count ; i + + ) {
Contact & c = contacts [ i ] ;
2021-12-03 18:38:40 +01:00
bool erase = false ;
if ( ! c . used ) {
// Was left behind in previous frame.
erase = true ;
} else {
c . used = false ;
Vector3 global_A = basis_A . xform ( c . local_A ) ;
Vector3 global_B = basis_B . xform ( c . local_B ) + offset_B ;
Vector3 axis = global_A - global_B ;
real_t depth = axis . dot ( c . normal ) ;
2014-02-10 02:10:30 +01:00
2021-12-03 18:38:40 +01:00
if ( depth < - max_separation | | ( global_B + c . normal * depth - global_A ) . length_squared ( ) > max_separation2 ) {
erase = true ;
}
}
2014-02-10 02:10:30 +01:00
2021-12-03 18:38:40 +01:00
if ( erase ) {
// Contact no longer needed, remove.
2014-02-10 02:10:30 +01:00
if ( ( i + 1 ) < contact_count ) {
2021-12-03 18:38:40 +01:00
// Swap with the last one.
2014-02-10 02:10:30 +01:00
SWAP ( contacts [ i ] , contacts [ contact_count - 1 ] ) ;
}
i - - ;
contact_count - - ;
}
}
}
2022-12-11 21:01:10 +01:00
// _test_ccd prevents tunneling by slowing down a high velocity body that is about to collide so that next frame it will be at an appropriate location to collide (i.e. slight overlap)
// Warning: the way velocity is adjusted down to cause a collision means the momentum will be weaker than it should for a bounce!
// Process: only proceed if body A's motion is high relative to its size.
// cast forward along motion vector to see if A is going to enter/pass B's collider next frame, only proceed if it does.
// adjust the velocity of A down so that it will just slightly intersect the collider instead of blowing right past it.
2021-10-18 21:24:30 +02:00
bool GodotBodyPair3D : : _test_ccd ( real_t p_step , GodotBody3D * p_A , int p_shape_A , const Transform3D & p_xform_A , GodotBody3D * p_B , int p_shape_B , const Transform3D & p_xform_B ) {
2023-02-08 21:00:36 +01:00
GodotShape3D * shape_A_ptr = p_A - > get_shape ( p_shape_A ) ;
GodotShape3D * shape_B_ptr = p_B - > get_shape ( p_shape_B ) ;
2015-01-06 03:00:35 +01:00
Vector3 motion = p_A - > get_linear_velocity ( ) * p_step ;
real_t mlen = motion . length ( ) ;
2020-05-14 16:41:43 +02:00
if ( mlen < CMP_EPSILON ) {
2015-01-06 03:00:35 +01:00
return false ;
2020-05-14 16:41:43 +02:00
}
2015-01-06 03:00:35 +01:00
Vector3 mnormal = motion / mlen ;
2022-10-01 21:09:22 +02:00
real_t min = 0.0 , max = 0.0 ;
2023-02-08 21:00:36 +01:00
shape_A_ptr - > project_range ( mnormal , p_xform_A , min , max ) ;
2015-01-06 03:00:35 +01:00
2021-12-10 00:36:39 +01:00
// Did it move enough in this direction to even attempt raycast?
// Let's say it should move more than 1/3 the size of the object in that axis.
bool fast_object = mlen > ( max - min ) * 0.3 ;
if ( ! fast_object ) {
2022-12-11 21:01:10 +01:00
return false ; // moving slow enough that there's no chance of tunneling.
2015-01-06 03:00:35 +01:00
}
2022-12-11 21:01:10 +01:00
// A is moving fast enough that tunneling might occur. See if it's really about to collide.
2021-12-10 00:36:39 +01:00
2023-02-08 21:00:36 +01:00
// Support points are the farthest forward points on A in the direction of the motion vector.
// i.e. the candidate points of which one should hit B first if any collision does occur.
static const int max_supports = 16 ;
Vector3 supports_A [ max_supports ] ;
int support_count_A ;
GodotShape3D : : FeatureType support_type_A ;
// Convert mnormal into body A's local xform because get_supports requires (and returns) local coordinates.
shape_A_ptr - > get_supports ( p_xform_A . basis . xform_inv ( mnormal ) . normalized ( ) , max_supports , supports_A , support_count_A , support_type_A ) ;
// Cast a segment from each support point of A in the motion direction.
int segment_support_idx = - 1 ;
float segment_hit_length = FLT_MAX ;
Vector3 segment_hit_local ;
for ( int i = 0 ; i < support_count_A ; i + + ) {
supports_A [ i ] = p_xform_A . xform ( supports_A [ i ] ) ;
Vector3 from = supports_A [ i ] ;
Vector3 to = from + motion ;
Transform3D from_inv = p_xform_B . affine_inverse ( ) ;
// Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
// At high speeds, this may mean we're actually casting from well behind the body instead of inside it, which is odd.
// But it still works out.
Vector3 local_from = from_inv . xform ( from - motion * 0.1 ) ;
Vector3 local_to = from_inv . xform ( to ) ;
Vector3 rpos , rnorm ;
if ( shape_B_ptr - > intersect_segment ( local_from , local_to , rpos , rnorm , true ) ) {
float hit_length = local_from . distance_to ( rpos ) ;
if ( hit_length < segment_hit_length ) {
segment_support_idx = i ;
segment_hit_length = hit_length ;
segment_hit_local = rpos ;
}
}
}
2015-01-06 03:00:35 +01:00
2023-02-08 21:00:36 +01:00
if ( segment_support_idx = = - 1 ) {
// There was no hit. Since the segment is the length of per-frame motion, this means the bodies will not
2022-12-11 21:01:10 +01:00
// actually collide yet on next frame. We'll probably check again next frame once they're closer.
2015-01-06 03:00:35 +01:00
return false ;
}
2023-02-08 21:00:36 +01:00
Vector3 hitpos = p_xform_B . xform ( segment_hit_local ) ;
real_t newlen = hitpos . distance_to ( supports_A [ segment_support_idx ] ) ;
if ( shape_B_ptr - > is_concave ( ) ) {
// Subtracting 5% of body length from the distance between collision and support point
// should cause body A's support point to arrive just before a face of B next frame.
newlen = MAX ( newlen - ( max - min ) * 0.05 , 0.0 ) ;
// NOTE: This may stop body A completely, without a proper collision response.
// We consider this preferable to tunneling.
} else {
// Adding 1% of body length to the distance between collision and support point
// should cause body A's support point to arrive just within B's collider next frame.
newlen + = ( max - min ) * 0.01 ;
}
2022-12-16 19:39:19 +01:00
2015-01-06 03:00:35 +01:00
p_A - > set_linear_velocity ( ( mnormal * newlen ) / p_step ) ;
return true ;
}
2021-10-18 21:24:30 +02:00
real_t combine_bounce ( GodotBody3D * A , GodotBody3D * B ) {
2018-07-23 16:37:07 +02:00
return CLAMP ( A - > get_bounce ( ) + B - > get_bounce ( ) , 0 , 1 ) ;
2017-10-24 18:10:30 +02:00
}
2021-10-18 21:24:30 +02:00
real_t combine_friction ( GodotBody3D * A , GodotBody3D * B ) {
2018-07-23 16:37:07 +02:00
return ABS ( MIN ( A - > get_friction ( ) , B - > get_friction ( ) ) ) ;
2017-10-24 18:10:30 +02:00
}
2021-10-18 21:24:30 +02:00
bool GodotBodyPair3D : : setup ( real_t p_step ) {
2021-12-10 00:36:39 +01:00
check_ccd = false ;
2020-10-08 13:45:03 +02:00
if ( ! A - > interacts_with ( B ) | | A - > has_exception ( B - > get_self ( ) ) | | B - > has_exception ( A - > get_self ( ) ) ) {
2014-09-03 04:13:40 +02:00
collided = false ;
return false ;
}
2014-02-10 02:10:30 +01:00
2021-07-15 21:38:48 +02:00
collide_A = ( A - > get_mode ( ) > PhysicsServer3D : : BODY_MODE_KINEMATIC ) & & A - > collides_with ( B ) ;
collide_B = ( B - > get_mode ( ) > PhysicsServer3D : : BODY_MODE_KINEMATIC ) & & B - > collides_with ( A ) ;
2021-04-20 03:38:11 +02:00
report_contacts_only = false ;
2021-07-15 21:38:48 +02:00
if ( ! collide_A & & ! collide_B ) {
2021-03-12 02:06:00 +01:00
if ( ( A - > get_max_contacts_reported ( ) > 0 ) | | ( B - > get_max_contacts_reported ( ) > 0 ) ) {
report_contacts_only = true ;
} else {
collided = false ;
return false ;
}
}
2014-02-10 02:10:30 +01:00
offset_B = B - > get_transform ( ) . get_origin ( ) - A - > get_transform ( ) . get_origin ( ) ;
validate_contacts ( ) ;
2021-04-20 03:38:11 +02:00
const Vector3 & offset_A = A - > get_transform ( ) . get_origin ( ) ;
2020-10-17 07:08:21 +02:00
Transform3D xform_Au = Transform3D ( A - > get_transform ( ) . basis , Vector3 ( ) ) ;
Transform3D xform_A = xform_Au * A - > get_shape_transform ( shape_A ) ;
2014-02-10 02:10:30 +01:00
2020-10-17 07:08:21 +02:00
Transform3D xform_Bu = B - > get_transform ( ) ;
2014-02-10 02:10:30 +01:00
xform_Bu . origin - = offset_A ;
2020-10-17 07:08:21 +02:00
Transform3D xform_B = xform_Bu * B - > get_shape_transform ( shape_B ) ;
2014-02-10 02:10:30 +01:00
2021-10-18 21:24:30 +02:00
GodotShape3D * shape_A_ptr = A - > get_shape ( shape_A ) ;
GodotShape3D * shape_B_ptr = B - > get_shape ( shape_B ) ;
2014-02-10 02:10:30 +01:00
2021-10-18 21:24:30 +02:00
collided = GodotCollisionSolver3D : : solve_static ( shape_A_ptr , xform_A , shape_B_ptr , xform_B , _contact_added_callback , this , & sep_axis ) ;
2015-01-06 03:00:35 +01:00
if ( ! collided ) {
2021-07-15 21:38:48 +02:00
if ( A - > is_continuous_collision_detection_enabled ( ) & & collide_A ) {
2021-12-10 00:36:39 +01:00
check_ccd = true ;
return true ;
2015-01-06 03:00:35 +01:00
}
2021-07-15 21:38:48 +02:00
if ( B - > is_continuous_collision_detection_enabled ( ) & & collide_B ) {
2021-12-10 00:36:39 +01:00
check_ccd = true ;
return true ;
2015-01-06 03:00:35 +01:00
}
2014-02-10 02:10:30 +01:00
return false ;
2015-01-06 03:00:35 +01:00
}
2014-02-10 02:10:30 +01:00
2021-04-20 03:38:11 +02:00
return true ;
}
2021-10-18 21:24:30 +02:00
bool GodotBodyPair3D : : pre_solve ( real_t p_step ) {
2021-04-20 03:38:11 +02:00
if ( ! collided ) {
2021-12-10 00:36:39 +01:00
if ( check_ccd ) {
const Vector3 & offset_A = A - > get_transform ( ) . get_origin ( ) ;
Transform3D xform_Au = Transform3D ( A - > get_transform ( ) . basis , Vector3 ( ) ) ;
Transform3D xform_A = xform_Au * A - > get_shape_transform ( shape_A ) ;
Transform3D xform_Bu = B - > get_transform ( ) ;
xform_Bu . origin - = offset_A ;
Transform3D xform_B = xform_Bu * B - > get_shape_transform ( shape_B ) ;
if ( A - > is_continuous_collision_detection_enabled ( ) & & collide_A ) {
_test_ccd ( p_step , A , shape_A , xform_A , B , shape_B , xform_B ) ;
}
if ( B - > is_continuous_collision_detection_enabled ( ) & & collide_B ) {
_test_ccd ( p_step , B , shape_B , xform_B , A , shape_A , xform_A ) ;
}
}
2021-04-20 03:38:11 +02:00
return false ;
}
2014-02-10 02:10:30 +01:00
real_t max_penetration = space - > get_contact_max_allowed_penetration ( ) ;
2021-12-03 18:38:40 +01:00
real_t bias = 0.8 ;
2014-02-10 02:10:30 +01:00
2021-10-18 21:24:30 +02:00
GodotShape3D * shape_A_ptr = A - > get_shape ( shape_A ) ;
GodotShape3D * shape_B_ptr = B - > get_shape ( shape_B ) ;
2021-04-20 03:38:11 +02:00
2014-02-10 02:10:30 +01:00
if ( shape_A_ptr - > get_custom_bias ( ) | | shape_B_ptr - > get_custom_bias ( ) ) {
2020-05-14 16:41:43 +02:00
if ( shape_A_ptr - > get_custom_bias ( ) = = 0 ) {
2014-02-10 02:10:30 +01:00
bias = shape_B_ptr - > get_custom_bias ( ) ;
2020-05-14 16:41:43 +02:00
} else if ( shape_B_ptr - > get_custom_bias ( ) = = 0 ) {
2014-02-10 02:10:30 +01:00
bias = shape_A_ptr - > get_custom_bias ( ) ;
2020-05-14 16:41:43 +02:00
} else {
2014-02-10 02:10:30 +01:00
bias = ( shape_B_ptr - > get_custom_bias ( ) + shape_A_ptr - > get_custom_bias ( ) ) * 0.5 ;
2020-05-14 16:41:43 +02:00
}
2014-02-10 02:10:30 +01:00
}
real_t inv_dt = 1.0 / p_step ;
2021-04-13 01:22:06 +02:00
bool do_process = false ;
2021-04-20 03:38:11 +02:00
const Basis & basis_A = A - > get_transform ( ) . basis ;
const Basis & basis_B = B - > get_transform ( ) . basis ;
2021-07-15 21:38:48 +02:00
Basis zero_basis ;
zero_basis . set_zero ( ) ;
const Basis & inv_inertia_tensor_A = collide_A ? A - > get_inv_inertia_tensor ( ) : zero_basis ;
const Basis & inv_inertia_tensor_B = collide_B ? B - > get_inv_inertia_tensor ( ) : zero_basis ;
real_t inv_mass_A = collide_A ? A - > get_inv_mass ( ) : 0.0 ;
real_t inv_mass_B = collide_B ? B - > get_inv_mass ( ) : 0.0 ;
2014-02-10 02:10:30 +01:00
for ( int i = 0 ; i < contact_count ; i + + ) {
Contact & c = contacts [ i ] ;
c . active = false ;
2021-04-20 03:38:11 +02:00
Vector3 global_A = basis_A . xform ( c . local_A ) ;
Vector3 global_B = basis_B . xform ( c . local_B ) + offset_B ;
2014-02-10 02:10:30 +01:00
2021-04-20 03:38:11 +02:00
Vector3 axis = global_A - global_B ;
real_t depth = axis . dot ( c . normal ) ;
2014-02-10 02:10:30 +01:00
2021-07-15 21:38:48 +02:00
if ( depth < = 0.0 ) {
2014-02-10 02:10:30 +01:00
continue ;
}
2015-09-20 18:03:46 +02:00
# ifdef DEBUG_ENABLED
if ( space - > is_debugging_contacts ( ) ) {
2021-04-20 03:38:11 +02:00
const Vector3 & offset_A = A - > get_transform ( ) . get_origin ( ) ;
2015-09-20 18:03:46 +02:00
space - > add_debug_contact ( global_A + offset_A ) ;
space - > add_debug_contact ( global_B + offset_A ) ;
}
# endif
2016-12-31 15:39:25 +01:00
c . rA = global_A - A - > get_center_of_mass ( ) ;
c . rB = global_B - B - > get_center_of_mass ( ) - offset_B ;
2014-02-10 02:10:30 +01:00
2023-01-08 04:34:28 +01:00
// Precompute normal mass, tangent mass, and bias.
Vector3 inertia_A = inv_inertia_tensor_A . xform ( c . rA . cross ( c . normal ) ) ;
Vector3 inertia_B = inv_inertia_tensor_B . xform ( c . rB . cross ( c . normal ) ) ;
real_t kNormal = inv_mass_A + inv_mass_B ;
kNormal + = c . normal . dot ( inertia_A . cross ( c . rA ) ) + c . normal . dot ( inertia_B . cross ( c . rB ) ) ;
c . mass_normal = 1.0f / kNormal ;
c . bias = - bias * inv_dt * MIN ( 0.0f , - depth + max_penetration ) ;
c . depth = depth ;
Vector3 j_vec = c . normal * c . acc_normal_impulse + c . acc_tangent_impulse ;
c . acc_impulse - = j_vec ;
2017-08-27 21:07:15 +02:00
// contact query reporting...
2014-02-10 02:10:30 +01:00
if ( A - > can_report_contacts ( ) ) {
2016-07-24 12:53:07 +02:00
Vector3 crA = A - > get_angular_velocity ( ) . cross ( c . rA ) + A - > get_linear_velocity ( ) ;
2023-01-08 04:34:28 +01:00
A - > add_contact ( global_A , - c . normal , depth , shape_A , global_B , shape_B , B - > get_instance_id ( ) , B - > get_self ( ) , crA , c . acc_impulse ) ;
2014-02-10 02:10:30 +01:00
}
if ( B - > can_report_contacts ( ) ) {
2016-07-24 12:53:07 +02:00
Vector3 crB = B - > get_angular_velocity ( ) . cross ( c . rB ) + B - > get_linear_velocity ( ) ;
2023-01-08 04:34:28 +01:00
B - > add_contact ( global_B , c . normal , depth , shape_B , global_A , shape_A , A - > get_instance_id ( ) , A - > get_self ( ) , crB , - c . acc_impulse ) ;
2014-02-10 02:10:30 +01:00
}
2021-03-12 02:06:00 +01:00
if ( report_contacts_only ) {
collided = false ;
continue ;
}
2014-02-10 02:10:30 +01:00
c . active = true ;
2021-04-13 01:22:06 +02:00
do_process = true ;
2014-02-10 02:10:30 +01:00
2021-07-15 21:38:48 +02:00
if ( collide_A ) {
2021-04-20 03:38:11 +02:00
A - > apply_impulse ( - j_vec , c . rA + A - > get_center_of_mass ( ) ) ;
}
2021-07-15 21:38:48 +02:00
if ( collide_B ) {
2021-04-20 03:38:11 +02:00
B - > apply_impulse ( j_vec , c . rB + B - > get_center_of_mass ( ) ) ;
}
2014-02-10 02:10:30 +01:00
2017-10-24 18:10:30 +02:00
c . bounce = combine_bounce ( A , B ) ;
2014-09-15 16:33:30 +02:00
if ( c . bounce ) {
2021-11-25 16:26:38 +01:00
Vector3 crA = A - > get_prev_angular_velocity ( ) . cross ( c . rA ) ;
Vector3 crB = B - > get_prev_angular_velocity ( ) . cross ( c . rB ) ;
Vector3 dv = B - > get_prev_linear_velocity ( ) + crB - A - > get_prev_linear_velocity ( ) - crA ;
2014-09-15 16:33:30 +02:00
c . bounce = c . bounce * dv . dot ( c . normal ) ;
}
2014-02-10 02:10:30 +01:00
}
2021-04-13 01:22:06 +02:00
return do_process ;
2014-02-10 02:10:30 +01:00
}
2021-10-18 21:24:30 +02:00
void GodotBodyPair3D : : solve ( real_t p_step ) {
2020-05-14 16:41:43 +02:00
if ( ! collided ) {
2014-02-10 02:10:30 +01:00
return ;
2020-05-14 16:41:43 +02:00
}
2014-02-10 02:10:30 +01:00
2021-04-20 03:38:11 +02:00
const real_t max_bias_av = MAX_BIAS_ROTATION / p_step ;
2021-07-15 21:38:48 +02:00
Basis zero_basis ;
zero_basis . set_zero ( ) ;
const Basis & inv_inertia_tensor_A = collide_A ? A - > get_inv_inertia_tensor ( ) : zero_basis ;
const Basis & inv_inertia_tensor_B = collide_B ? B - > get_inv_inertia_tensor ( ) : zero_basis ;
real_t inv_mass_A = collide_A ? A - > get_inv_mass ( ) : 0.0 ;
real_t inv_mass_B = collide_B ? B - > get_inv_mass ( ) : 0.0 ;
2014-02-10 02:10:30 +01:00
for ( int i = 0 ; i < contact_count ; i + + ) {
Contact & c = contacts [ i ] ;
2020-05-14 16:41:43 +02:00
if ( ! c . active ) {
2014-02-10 02:10:30 +01:00
continue ;
2020-05-14 16:41:43 +02:00
}
2014-02-10 02:10:30 +01:00
c . active = false ; //try to deactivate, will activate itself if still needed
2017-09-18 01:01:19 +02:00
//bias impulse
2014-02-10 02:10:30 +01:00
Vector3 crbA = A - > get_biased_angular_velocity ( ) . cross ( c . rA ) ;
Vector3 crbB = B - > get_biased_angular_velocity ( ) . cross ( c . rB ) ;
Vector3 dbv = B - > get_biased_linear_velocity ( ) + crbB - A - > get_biased_linear_velocity ( ) - crbA ;
real_t vbn = dbv . dot ( c . normal ) ;
if ( Math : : abs ( - vbn + c . bias ) > MIN_VELOCITY ) {
real_t jbn = ( - vbn + c . bias ) * c . mass_normal ;
real_t jbnOld = c . acc_bias_impulse ;
c . acc_bias_impulse = MAX ( jbnOld + jbn , 0.0f ) ;
Vector3 jb = c . normal * ( c . acc_bias_impulse - jbnOld ) ;
2021-07-15 21:38:48 +02:00
if ( collide_A ) {
2021-04-20 03:38:11 +02:00
A - > apply_bias_impulse ( - jb , c . rA + A - > get_center_of_mass ( ) , max_bias_av ) ;
}
2021-07-15 21:38:48 +02:00
if ( collide_B ) {
2021-04-20 03:38:11 +02:00
B - > apply_bias_impulse ( jb , c . rB + B - > get_center_of_mass ( ) , max_bias_av ) ;
}
2017-09-18 01:01:19 +02:00
crbA = A - > get_biased_angular_velocity ( ) . cross ( c . rA ) ;
crbB = B - > get_biased_angular_velocity ( ) . cross ( c . rB ) ;
dbv = B - > get_biased_linear_velocity ( ) + crbB - A - > get_biased_linear_velocity ( ) - crbA ;
vbn = dbv . dot ( c . normal ) ;
if ( Math : : abs ( - vbn + c . bias ) > MIN_VELOCITY ) {
2021-07-15 21:38:48 +02:00
real_t jbn_com = ( - vbn + c . bias ) / ( inv_mass_A + inv_mass_B ) ;
2017-09-18 01:01:19 +02:00
real_t jbnOld_com = c . acc_bias_impulse_center_of_mass ;
c . acc_bias_impulse_center_of_mass = MAX ( jbnOld_com + jbn_com , 0.0f ) ;
Vector3 jb_com = c . normal * ( c . acc_bias_impulse_center_of_mass - jbnOld_com ) ;
2021-07-15 21:38:48 +02:00
if ( collide_A ) {
2021-04-20 03:38:11 +02:00
A - > apply_bias_impulse ( - jb_com , A - > get_center_of_mass ( ) , 0.0f ) ;
}
2021-07-15 21:38:48 +02:00
if ( collide_B ) {
2021-04-20 03:38:11 +02:00
B - > apply_bias_impulse ( jb_com , B - > get_center_of_mass ( ) , 0.0f ) ;
}
2017-09-18 01:01:19 +02:00
}
2014-02-10 02:10:30 +01:00
c . active = true ;
}
Vector3 crA = A - > get_angular_velocity ( ) . cross ( c . rA ) ;
Vector3 crB = B - > get_angular_velocity ( ) . cross ( c . rB ) ;
Vector3 dv = B - > get_linear_velocity ( ) + crB - A - > get_linear_velocity ( ) - crA ;
2017-09-18 01:01:19 +02:00
//normal impulse
2014-02-10 02:10:30 +01:00
real_t vn = dv . dot ( c . normal ) ;
if ( Math : : abs ( vn ) > MIN_VELOCITY ) {
2014-09-15 16:33:30 +02:00
real_t jn = - ( c . bounce + vn ) * c . mass_normal ;
2014-02-10 02:10:30 +01:00
real_t jnOld = c . acc_normal_impulse ;
c . acc_normal_impulse = MAX ( jnOld + jn , 0.0f ) ;
Vector3 j = c . normal * ( c . acc_normal_impulse - jnOld ) ;
2021-07-15 21:38:48 +02:00
if ( collide_A ) {
2021-04-20 03:38:11 +02:00
A - > apply_impulse ( - j , c . rA + A - > get_center_of_mass ( ) ) ;
}
2021-07-15 21:38:48 +02:00
if ( collide_B ) {
2021-04-20 03:38:11 +02:00
B - > apply_impulse ( j , c . rB + B - > get_center_of_mass ( ) ) ;
}
2023-01-08 04:34:28 +01:00
c . acc_impulse - = j ;
2014-02-10 02:10:30 +01:00
c . active = true ;
}
2017-09-18 01:01:19 +02:00
//friction impulse
2014-02-10 02:10:30 +01:00
2017-10-24 18:10:30 +02:00
real_t friction = combine_friction ( A , B ) ;
2014-02-10 02:10:30 +01:00
Vector3 lvA = A - > get_linear_velocity ( ) + A - > get_angular_velocity ( ) . cross ( c . rA ) ;
Vector3 lvB = B - > get_linear_velocity ( ) + B - > get_angular_velocity ( ) . cross ( c . rB ) ;
Vector3 dtv = lvB - lvA ;
real_t tn = c . normal . dot ( dtv ) ;
// tangential velocity
Vector3 tv = dtv - c . normal * tn ;
real_t tvl = tv . length ( ) ;
if ( tvl > MIN_VELOCITY ) {
tv / = tvl ;
2021-07-15 21:38:48 +02:00
Vector3 temp1 = inv_inertia_tensor_A . xform ( c . rA . cross ( tv ) ) ;
Vector3 temp2 = inv_inertia_tensor_B . xform ( c . rB . cross ( tv ) ) ;
2014-02-10 02:10:30 +01:00
2021-10-28 15:19:35 +02:00
real_t t = - tvl / ( inv_mass_A + inv_mass_B + tv . dot ( temp1 . cross ( c . rA ) + temp2 . cross ( c . rB ) ) ) ;
2014-02-10 02:10:30 +01:00
Vector3 jt = t * tv ;
Vector3 jtOld = c . acc_tangent_impulse ;
c . acc_tangent_impulse + = jt ;
real_t fi_len = c . acc_tangent_impulse . length ( ) ;
real_t jtMax = c . acc_normal_impulse * friction ;
if ( fi_len > CMP_EPSILON & & fi_len > jtMax ) {
c . acc_tangent_impulse * = jtMax / fi_len ;
}
jt = c . acc_tangent_impulse - jtOld ;
2021-07-15 21:38:48 +02:00
if ( collide_A ) {
2021-04-20 03:38:11 +02:00
A - > apply_impulse ( - jt , c . rA + A - > get_center_of_mass ( ) ) ;
}
2021-07-15 21:38:48 +02:00
if ( collide_B ) {
2021-04-20 03:38:11 +02:00
B - > apply_impulse ( jt , c . rB + B - > get_center_of_mass ( ) ) ;
}
2023-01-08 04:34:28 +01:00
c . acc_impulse - = jt ;
2014-02-10 02:10:30 +01:00
c . active = true ;
}
}
}
2021-10-18 21:24:30 +02:00
GodotBodyPair3D : : GodotBodyPair3D ( GodotBody3D * p_A , int p_shape_A , GodotBody3D * p_B , int p_shape_B ) :
GodotBodyContact3D ( _arr , 2 ) {
2014-02-10 02:10:30 +01:00
A = p_A ;
B = p_B ;
shape_A = p_shape_A ;
shape_B = p_shape_B ;
space = A - > get_space ( ) ;
A - > add_constraint ( this , 0 ) ;
B - > add_constraint ( this , 1 ) ;
}
2021-10-18 21:24:30 +02:00
GodotBodyPair3D : : ~ GodotBodyPair3D ( ) {
2014-02-10 02:10:30 +01:00
A - > remove_constraint ( this ) ;
B - > remove_constraint ( this ) ;
}
2021-03-12 04:33:46 +01:00
2023-01-15 03:39:23 +01:00
void GodotBodySoftBodyPair3D : : _contact_added_callback ( const Vector3 & p_point_A , int p_index_A , const Vector3 & p_point_B , int p_index_B , const Vector3 & normal , void * p_userdata ) {
2022-04-05 12:40:26 +02:00
GodotBodySoftBodyPair3D * pair = static_cast < GodotBodySoftBodyPair3D * > ( p_userdata ) ;
2023-01-15 03:39:23 +01:00
pair - > contact_added_callback ( p_point_A , p_index_A , p_point_B , p_index_B , normal ) ;
2021-03-12 04:33:46 +01:00
}
2023-01-15 03:39:23 +01:00
void GodotBodySoftBodyPair3D : : contact_added_callback ( const Vector3 & p_point_A , int p_index_A , const Vector3 & p_point_B , int p_index_B , const Vector3 & normal ) {
2021-03-12 04:33:46 +01:00
Vector3 local_A = body - > get_inv_transform ( ) . xform ( p_point_A ) ;
Vector3 local_B = p_point_B - soft_body - > get_node_position ( p_index_B ) ;
Contact contact ;
contact . index_A = p_index_A ;
contact . index_B = p_index_B ;
contact . local_A = local_A ;
contact . local_B = local_B ;
2023-01-15 03:39:23 +01:00
contact . normal = ( normal . dot ( ( p_point_A - p_point_B ) ) < 0 ? - normal : normal ) ;
2021-12-03 18:38:40 +01:00
contact . used = true ;
2021-03-12 04:33:46 +01:00
// Attempt to determine if the contact will be reused.
real_t contact_recycle_radius = space - > get_contact_recycle_radius ( ) ;
uint32_t contact_count = contacts . size ( ) ;
for ( uint32_t contact_index = 0 ; contact_index < contact_count ; + + contact_index ) {
Contact & c = contacts [ contact_index ] ;
if ( c . index_B = = p_index_B ) {
if ( c . local_A . distance_squared_to ( local_A ) < ( contact_recycle_radius * contact_recycle_radius ) & &
c . local_B . distance_squared_to ( local_B ) < ( contact_recycle_radius * contact_recycle_radius ) ) {
contact . acc_normal_impulse = c . acc_normal_impulse ;
contact . acc_bias_impulse = c . acc_bias_impulse ;
contact . acc_bias_impulse_center_of_mass = c . acc_bias_impulse_center_of_mass ;
contact . acc_tangent_impulse = c . acc_tangent_impulse ;
}
c = contact ;
return ;
}
}
contacts . push_back ( contact ) ;
}
2021-10-18 21:24:30 +02:00
void GodotBodySoftBodyPair3D : : validate_contacts ( ) {
2021-03-12 04:33:46 +01:00
// Make sure to erase contacts that are no longer valid.
2021-12-03 18:38:40 +01:00
real_t max_separation = space - > get_contact_max_separation ( ) ;
real_t max_separation2 = max_separation * max_separation ;
2021-03-12 04:33:46 +01:00
2021-12-03 18:38:40 +01:00
const Transform3D & transform_A = body - > get_transform ( ) ;
2021-03-12 04:33:46 +01:00
uint32_t contact_count = contacts . size ( ) ;
for ( uint32_t contact_index = 0 ; contact_index < contact_count ; + + contact_index ) {
Contact & c = contacts [ contact_index ] ;
2021-12-03 18:38:40 +01:00
bool erase = false ;
if ( ! c . used ) {
// Was left behind in previous frame.
erase = true ;
} else {
c . used = false ;
Vector3 global_A = transform_A . xform ( c . local_A ) ;
Vector3 global_B = soft_body - > get_node_position ( c . index_B ) + c . local_B ;
Vector3 axis = global_A - global_B ;
real_t depth = axis . dot ( c . normal ) ;
if ( depth < - max_separation | | ( global_B + c . normal * depth - global_A ) . length_squared ( ) > max_separation2 ) {
erase = true ;
}
}
2021-03-12 04:33:46 +01:00
2021-12-03 18:38:40 +01:00
if ( erase ) {
2021-03-12 04:33:46 +01:00
// Contact no longer needed, remove.
if ( ( contact_index + 1 ) < contact_count ) {
// Swap with the last one.
SWAP ( c , contacts [ contact_count - 1 ] ) ;
}
contact_index - - ;
contact_count - - ;
}
}
contacts . resize ( contact_count ) ;
}
2021-10-18 21:24:30 +02:00
bool GodotBodySoftBodyPair3D : : setup ( real_t p_step ) {
2020-10-08 13:45:03 +02:00
if ( ! body - > interacts_with ( soft_body ) | | body - > has_exception ( soft_body - > get_self ( ) ) | | soft_body - > has_exception ( body - > get_self ( ) ) ) {
2021-03-12 04:33:46 +01:00
collided = false ;
return false ;
}
2021-07-15 21:38:48 +02:00
body_collides = ( body - > get_mode ( ) > PhysicsServer3D : : BODY_MODE_KINEMATIC ) & & body - > collides_with ( soft_body ) ;
soft_body_collides = soft_body - > collides_with ( body ) ;
if ( ! body_collides & & ! soft_body_collides ) {
if ( body - > get_max_contacts_reported ( ) > 0 ) {
report_contacts_only = true ;
} else {
collided = false ;
return false ;
}
}
2020-10-17 07:08:21 +02:00
const Transform3D & xform_Au = body - > get_transform ( ) ;
Transform3D xform_A = xform_Au * body - > get_shape_transform ( body_shape ) ;
2021-03-12 04:33:46 +01:00
2020-10-17 07:08:21 +02:00
Transform3D xform_Bu = soft_body - > get_transform ( ) ;
Transform3D xform_B = xform_Bu * soft_body - > get_shape_transform ( 0 ) ;
2021-03-12 04:33:46 +01:00
validate_contacts ( ) ;
2021-10-18 21:24:30 +02:00
GodotShape3D * shape_A_ptr = body - > get_shape ( body_shape ) ;
GodotShape3D * shape_B_ptr = soft_body - > get_shape ( 0 ) ;
2021-03-12 04:33:46 +01:00
2021-10-18 21:24:30 +02:00
collided = GodotCollisionSolver3D : : solve_static ( shape_A_ptr , xform_A , shape_B_ptr , xform_B , _contact_added_callback , this , & sep_axis ) ;
2021-04-20 03:38:11 +02:00
return collided ;
}
2021-10-18 21:24:30 +02:00
bool GodotBodySoftBodyPair3D : : pre_solve ( real_t p_step ) {
2021-04-20 03:38:11 +02:00
if ( ! collided ) {
return false ;
}
2021-03-12 04:33:46 +01:00
real_t max_penetration = space - > get_contact_max_allowed_penetration ( ) ;
2021-12-03 18:38:40 +01:00
real_t bias = space - > get_contact_bias ( ) ;
2021-04-20 03:38:11 +02:00
2021-10-18 21:24:30 +02:00
GodotShape3D * shape_A_ptr = body - > get_shape ( body_shape ) ;
2021-04-20 03:38:11 +02:00
2021-03-12 04:33:46 +01:00
if ( shape_A_ptr - > get_custom_bias ( ) ) {
bias = shape_A_ptr - > get_custom_bias ( ) ;
}
real_t inv_dt = 1.0 / p_step ;
2021-04-13 01:22:06 +02:00
bool do_process = false ;
2020-10-17 07:08:21 +02:00
const Transform3D & transform_A = body - > get_transform ( ) ;
2021-04-20 03:38:11 +02:00
2021-07-15 21:38:48 +02:00
Basis zero_basis ;
zero_basis . set_zero ( ) ;
const Basis & body_inv_inertia_tensor = body_collides ? body - > get_inv_inertia_tensor ( ) : zero_basis ;
real_t body_inv_mass = body_collides ? body - > get_inv_mass ( ) : 0.0 ;
2021-03-12 04:33:46 +01:00
uint32_t contact_count = contacts . size ( ) ;
for ( uint32_t contact_index = 0 ; contact_index < contact_count ; + + contact_index ) {
Contact & c = contacts [ contact_index ] ;
c . active = false ;
2021-07-15 21:38:48 +02:00
real_t node_inv_mass = soft_body_collides ? soft_body - > get_node_inv_mass ( c . index_B ) : 0.0 ;
if ( ( node_inv_mass = = 0.0 ) & & ( body_inv_mass = = 0.0 ) ) {
2021-03-12 04:33:46 +01:00
continue ;
}
2021-04-20 03:38:11 +02:00
Vector3 global_A = transform_A . xform ( c . local_A ) ;
2021-03-12 04:33:46 +01:00
Vector3 global_B = soft_body - > get_node_position ( c . index_B ) + c . local_B ;
2021-04-20 03:38:11 +02:00
Vector3 axis = global_A - global_B ;
real_t depth = axis . dot ( c . normal ) ;
2021-03-12 04:33:46 +01:00
2021-07-15 21:38:48 +02:00
if ( depth < = 0.0 ) {
2021-03-12 04:33:46 +01:00
continue ;
}
# ifdef DEBUG_ENABLED
if ( space - > is_debugging_contacts ( ) ) {
space - > add_debug_contact ( global_A ) ;
space - > add_debug_contact ( global_B ) ;
}
# endif
2021-04-20 03:38:11 +02:00
c . rA = global_A - transform_A . origin - body - > get_center_of_mass ( ) ;
2021-03-12 04:33:46 +01:00
c . rB = global_B ;
// Precompute normal mass, tangent mass, and bias.
2021-07-15 21:38:48 +02:00
Vector3 inertia_A = body_inv_inertia_tensor . xform ( c . rA . cross ( c . normal ) ) ;
real_t kNormal = body_inv_mass + node_inv_mass ;
2021-03-12 04:33:46 +01:00
kNormal + = c . normal . dot ( inertia_A . cross ( c . rA ) ) ;
c . mass_normal = 1.0f / kNormal ;
c . bias = - bias * inv_dt * MIN ( 0.0f , - depth + max_penetration ) ;
c . depth = depth ;
Vector3 j_vec = c . normal * c . acc_normal_impulse + c . acc_tangent_impulse ;
2021-07-15 21:38:48 +02:00
if ( body_collides ) {
2021-04-20 03:38:11 +02:00
body - > apply_impulse ( - j_vec , c . rA + body - > get_center_of_mass ( ) ) ;
}
2021-07-15 21:38:48 +02:00
if ( soft_body_collides ) {
soft_body - > apply_node_impulse ( c . index_B , j_vec ) ;
}
2023-01-08 04:34:28 +01:00
c . acc_impulse - = j_vec ;
if ( body - > can_report_contacts ( ) ) {
Vector3 crA = body - > get_angular_velocity ( ) . cross ( c . rA ) + body - > get_linear_velocity ( ) ;
body - > add_contact ( global_A , - c . normal , depth , body_shape , global_B , 0 , soft_body - > get_instance_id ( ) , soft_body - > get_self ( ) , crA , c . acc_impulse ) ;
}
if ( report_contacts_only ) {
collided = false ;
continue ;
}
c . active = true ;
do_process = true ;
if ( body_collides ) {
body - > set_active ( true ) ;
}
2021-03-12 04:33:46 +01:00
c . bounce = body - > get_bounce ( ) ;
if ( c . bounce ) {
Vector3 crA = body - > get_angular_velocity ( ) . cross ( c . rA ) ;
Vector3 dv = soft_body - > get_node_velocity ( c . index_B ) - body - > get_linear_velocity ( ) - crA ;
// Normal impulse.
c . bounce = c . bounce * dv . dot ( c . normal ) ;
}
}
2021-04-13 01:22:06 +02:00
return do_process ;
2021-03-12 04:33:46 +01:00
}
2021-10-18 21:24:30 +02:00
void GodotBodySoftBodyPair3D : : solve ( real_t p_step ) {
2021-03-12 04:33:46 +01:00
if ( ! collided ) {
return ;
}
2021-04-20 03:38:11 +02:00
const real_t max_bias_av = MAX_BIAS_ROTATION / p_step ;
2021-07-15 21:38:48 +02:00
Basis zero_basis ;
zero_basis . set_zero ( ) ;
const Basis & body_inv_inertia_tensor = body_collides ? body - > get_inv_inertia_tensor ( ) : zero_basis ;
real_t body_inv_mass = body_collides ? body - > get_inv_mass ( ) : 0.0 ;
2021-03-12 04:33:46 +01:00
uint32_t contact_count = contacts . size ( ) ;
for ( uint32_t contact_index = 0 ; contact_index < contact_count ; + + contact_index ) {
Contact & c = contacts [ contact_index ] ;
if ( ! c . active ) {
continue ;
}
c . active = false ;
2021-07-15 21:38:48 +02:00
real_t node_inv_mass = soft_body_collides ? soft_body - > get_node_inv_mass ( c . index_B ) : 0.0 ;
2021-03-12 04:33:46 +01:00
// Bias impulse.
Vector3 crbA = body - > get_biased_angular_velocity ( ) . cross ( c . rA ) ;
Vector3 dbv = soft_body - > get_node_biased_velocity ( c . index_B ) - body - > get_biased_linear_velocity ( ) - crbA ;
real_t vbn = dbv . dot ( c . normal ) ;
if ( Math : : abs ( - vbn + c . bias ) > MIN_VELOCITY ) {
real_t jbn = ( - vbn + c . bias ) * c . mass_normal ;
real_t jbnOld = c . acc_bias_impulse ;
c . acc_bias_impulse = MAX ( jbnOld + jbn , 0.0f ) ;
Vector3 jb = c . normal * ( c . acc_bias_impulse - jbnOld ) ;
2021-07-15 21:38:48 +02:00
if ( body_collides ) {
2021-04-20 03:38:11 +02:00
body - > apply_bias_impulse ( - jb , c . rA + body - > get_center_of_mass ( ) , max_bias_av ) ;
}
2021-07-15 21:38:48 +02:00
if ( soft_body_collides ) {
soft_body - > apply_node_bias_impulse ( c . index_B , jb ) ;
}
2021-03-12 04:33:46 +01:00
crbA = body - > get_biased_angular_velocity ( ) . cross ( c . rA ) ;
dbv = soft_body - > get_node_biased_velocity ( c . index_B ) - body - > get_biased_linear_velocity ( ) - crbA ;
vbn = dbv . dot ( c . normal ) ;
if ( Math : : abs ( - vbn + c . bias ) > MIN_VELOCITY ) {
2021-07-15 21:38:48 +02:00
real_t jbn_com = ( - vbn + c . bias ) / ( body_inv_mass + node_inv_mass ) ;
2021-03-12 04:33:46 +01:00
real_t jbnOld_com = c . acc_bias_impulse_center_of_mass ;
c . acc_bias_impulse_center_of_mass = MAX ( jbnOld_com + jbn_com , 0.0f ) ;
Vector3 jb_com = c . normal * ( c . acc_bias_impulse_center_of_mass - jbnOld_com ) ;
2021-07-15 21:38:48 +02:00
if ( body_collides ) {
2021-04-20 03:38:11 +02:00
body - > apply_bias_impulse ( - jb_com , body - > get_center_of_mass ( ) , 0.0f ) ;
}
2021-07-15 21:38:48 +02:00
if ( soft_body_collides ) {
soft_body - > apply_node_bias_impulse ( c . index_B , jb_com ) ;
}
2021-03-12 04:33:46 +01:00
}
c . active = true ;
}
Vector3 crA = body - > get_angular_velocity ( ) . cross ( c . rA ) ;
Vector3 dv = soft_body - > get_node_velocity ( c . index_B ) - body - > get_linear_velocity ( ) - crA ;
// Normal impulse.
real_t vn = dv . dot ( c . normal ) ;
if ( Math : : abs ( vn ) > MIN_VELOCITY ) {
real_t jn = - ( c . bounce + vn ) * c . mass_normal ;
real_t jnOld = c . acc_normal_impulse ;
c . acc_normal_impulse = MAX ( jnOld + jn , 0.0f ) ;
Vector3 j = c . normal * ( c . acc_normal_impulse - jnOld ) ;
2021-07-15 21:38:48 +02:00
if ( body_collides ) {
2021-04-20 03:38:11 +02:00
body - > apply_impulse ( - j , c . rA + body - > get_center_of_mass ( ) ) ;
}
2021-07-15 21:38:48 +02:00
if ( soft_body_collides ) {
soft_body - > apply_node_impulse ( c . index_B , j ) ;
}
2023-01-08 04:34:28 +01:00
c . acc_impulse - = j ;
2021-03-12 04:33:46 +01:00
c . active = true ;
}
// Friction impulse.
real_t friction = body - > get_friction ( ) ;
Vector3 lvA = body - > get_linear_velocity ( ) + body - > get_angular_velocity ( ) . cross ( c . rA ) ;
Vector3 lvB = soft_body - > get_node_velocity ( c . index_B ) ;
Vector3 dtv = lvB - lvA ;
real_t tn = c . normal . dot ( dtv ) ;
// Tangential velocity.
Vector3 tv = dtv - c . normal * tn ;
real_t tvl = tv . length ( ) ;
if ( tvl > MIN_VELOCITY ) {
tv / = tvl ;
2021-07-15 21:38:48 +02:00
Vector3 temp1 = body_inv_inertia_tensor . xform ( c . rA . cross ( tv ) ) ;
2021-03-12 04:33:46 +01:00
2021-10-28 15:19:35 +02:00
real_t t = - tvl / ( body_inv_mass + node_inv_mass + tv . dot ( temp1 . cross ( c . rA ) ) ) ;
2021-03-12 04:33:46 +01:00
Vector3 jt = t * tv ;
Vector3 jtOld = c . acc_tangent_impulse ;
c . acc_tangent_impulse + = jt ;
real_t fi_len = c . acc_tangent_impulse . length ( ) ;
real_t jtMax = c . acc_normal_impulse * friction ;
if ( fi_len > CMP_EPSILON & & fi_len > jtMax ) {
c . acc_tangent_impulse * = jtMax / fi_len ;
}
jt = c . acc_tangent_impulse - jtOld ;
2021-07-15 21:38:48 +02:00
if ( body_collides ) {
2021-04-20 03:38:11 +02:00
body - > apply_impulse ( - jt , c . rA + body - > get_center_of_mass ( ) ) ;
}
2021-07-15 21:38:48 +02:00
if ( soft_body_collides ) {
soft_body - > apply_node_impulse ( c . index_B , jt ) ;
}
2023-01-08 04:34:28 +01:00
c . acc_impulse - = jt ;
2021-03-12 04:33:46 +01:00
c . active = true ;
}
}
}
2021-10-18 21:24:30 +02:00
GodotBodySoftBodyPair3D : : GodotBodySoftBodyPair3D ( GodotBody3D * p_A , int p_shape_A , GodotSoftBody3D * p_B ) :
GodotBodyContact3D ( & body , 1 ) {
2021-03-12 04:33:46 +01:00
body = p_A ;
soft_body = p_B ;
body_shape = p_shape_A ;
space = p_A - > get_space ( ) ;
body - > add_constraint ( this , 0 ) ;
soft_body - > add_constraint ( this ) ;
}
2021-10-18 21:24:30 +02:00
GodotBodySoftBodyPair3D : : ~ GodotBodySoftBodyPair3D ( ) {
2021-03-12 04:33:46 +01:00
body - > remove_constraint ( this ) ;
soft_body - > remove_constraint ( this ) ;
}