2014-02-10 02:10:30 +01:00
/*************************************************************************/
/* body_pair_2d_sw.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
2017-08-27 14:16:55 +02:00
/* https://godotengine.org */
2014-02-10 02:10:30 +01:00
/*************************************************************************/
2021-01-01 20:13:46 +01:00
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
2014-02-10 02:10:30 +01:00
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
2018-01-05 00:50:27 +01:00
2014-02-10 02:10:30 +01:00
# include "body_pair_2d_sw.h"
# include "collision_solver_2d_sw.h"
# include "space_2d_sw.h"
# define POSITION_CORRECTION
# define ACCUMULATE_IMPULSES
2017-03-05 16:44:50 +01:00
void BodyPair2DSW : : _add_contact ( const Vector2 & p_point_A , const Vector2 & p_point_B , void * p_self ) {
2014-02-10 02:10:30 +01:00
BodyPair2DSW * self = ( BodyPair2DSW * ) p_self ;
2017-03-05 16:44:50 +01:00
self - > _contact_added_callback ( p_point_A , p_point_B ) ;
2014-02-10 02:10:30 +01:00
}
2017-03-05 16:44:50 +01:00
void BodyPair2DSW : : _contact_added_callback ( const Vector2 & p_point_A , const Vector2 & p_point_B ) {
2014-02-10 02:10:30 +01:00
// check if we already have the contact
Vector2 local_A = A - > get_inv_transform ( ) . basis_xform ( p_point_A ) ;
2017-03-05 16:44:50 +01:00
Vector2 local_B = B - > get_inv_transform ( ) . basis_xform ( p_point_B - offset_B ) ;
2014-02-10 02:10:30 +01:00
int new_index = contact_count ;
2017-03-05 16:44:50 +01:00
ERR_FAIL_COND ( new_index > = ( MAX_CONTACTS + 1 ) ) ;
2014-02-10 02:10:30 +01:00
Contact contact ;
2017-03-05 16:44:50 +01:00
contact . acc_normal_impulse = 0 ;
contact . acc_bias_impulse = 0 ;
contact . acc_tangent_impulse = 0 ;
contact . local_A = local_A ;
contact . local_B = local_B ;
contact . reused = true ;
contact . normal = ( p_point_A - p_point_B ) . normalized ( ) ;
2018-04-19 13:04:41 +02:00
contact . mass_normal = 0 ; // will be computed in setup()
2014-02-10 02:10:30 +01:00
// attempt to determine if the contact will be reused
real_t recycle_radius_2 = space - > get_contact_recycle_radius ( ) * space - > get_contact_recycle_radius ( ) ;
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < contact_count ; i + + ) {
Contact & c = contacts [ i ] ;
2014-02-10 02:10:30 +01:00
if (
2017-03-05 16:44:50 +01:00
c . local_A . distance_squared_to ( local_A ) < ( recycle_radius_2 ) & &
c . local_B . distance_squared_to ( local_B ) < ( recycle_radius_2 ) ) {
contact . acc_normal_impulse = c . acc_normal_impulse ;
contact . acc_tangent_impulse = c . acc_tangent_impulse ;
contact . acc_bias_impulse = c . acc_bias_impulse ;
new_index = i ;
2014-02-10 02:10:30 +01:00
break ;
}
}
// figure out if the contact amount must be reduced to fit the new contact
if ( new_index = = MAX_CONTACTS ) {
// remove the contact with the minimum depth
2017-03-05 16:44:50 +01:00
int least_deep = - 1 ;
real_t min_depth = 1e10 ;
2014-02-10 02:10:30 +01:00
2021-04-20 03:38:11 +02:00
const Transform2D & transform_A = A - > get_transform ( ) ;
const Transform2D & transform_B = B - > get_transform ( ) ;
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < = contact_count ; i + + ) {
Contact & c = ( i = = contact_count ) ? contact : contacts [ i ] ;
2021-04-20 03:38:11 +02:00
Vector2 global_A = transform_A . basis_xform ( c . local_A ) ;
Vector2 global_B = transform_B . basis_xform ( c . local_B ) + offset_B ;
2014-02-10 02:10:30 +01:00
Vector2 axis = global_A - global_B ;
2017-03-05 16:44:50 +01:00
real_t depth = axis . dot ( c . normal ) ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
if ( depth < min_depth ) {
min_depth = depth ;
least_deep = i ;
2014-02-10 02:10:30 +01:00
}
}
2017-03-05 16:44:50 +01:00
ERR_FAIL_COND ( least_deep = = - 1 ) ;
2014-02-10 02:10:30 +01:00
if ( least_deep < contact_count ) { //replace the last deep contact by the new one
2017-03-05 16:44:50 +01:00
contacts [ least_deep ] = contact ;
2014-02-10 02:10:30 +01:00
}
return ;
}
2017-03-05 16:44:50 +01:00
contacts [ new_index ] = contact ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
if ( new_index = = contact_count ) {
2014-02-10 02:10:30 +01:00
contact_count + + ;
}
}
void BodyPair2DSW : : _validate_contacts ( ) {
//make sure to erase contacts that are no longer valid
real_t max_separation = space - > get_contact_max_separation ( ) ;
2017-03-05 16:44:50 +01:00
real_t max_separation2 = max_separation * max_separation ;
2014-02-10 02:10:30 +01:00
2021-04-20 03:38:11 +02:00
const Transform2D & transform_A = A - > get_transform ( ) ;
const Transform2D & transform_B = B - > get_transform ( ) ;
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < contact_count ; i + + ) {
Contact & c = contacts [ i ] ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
bool erase = false ;
2018-10-06 22:20:41 +02:00
if ( ! c . reused ) {
2014-02-19 15:57:14 +01:00
//was left behind in previous frame
2017-03-05 16:44:50 +01:00
erase = true ;
2014-02-19 15:57:14 +01:00
} else {
2017-03-05 16:44:50 +01:00
c . reused = false ;
2014-02-19 15:57:14 +01:00
2021-04-20 03:38:11 +02:00
Vector2 global_A = transform_A . basis_xform ( c . local_A ) ;
Vector2 global_B = transform_B . basis_xform ( c . local_B ) + offset_B ;
2014-02-19 15:57:14 +01:00
Vector2 axis = global_A - global_B ;
2017-03-05 16:44:50 +01:00
real_t depth = axis . dot ( c . normal ) ;
2014-02-19 15:57:14 +01:00
if ( depth < - max_separation | | ( global_B + c . normal * depth - global_A ) . length_squared ( ) > max_separation2 ) {
2017-03-05 16:44:50 +01:00
erase = true ;
2014-02-19 15:57:14 +01:00
}
}
2014-02-10 02:10:30 +01:00
2014-02-19 15:57:14 +01:00
if ( erase ) {
2014-02-10 02:10:30 +01:00
// contact no longer needed, remove
2017-03-05 16:44:50 +01:00
if ( ( i + 1 ) < contact_count ) {
2014-02-10 02:10:30 +01:00
// swap with the last one
2017-03-05 16:44:50 +01:00
SWAP ( contacts [ i ] , contacts [ contact_count - 1 ] ) ;
2014-02-10 02:10:30 +01:00
}
i - - ;
contact_count - - ;
}
}
}
2017-03-05 16:44:50 +01:00
bool BodyPair2DSW : : _test_ccd ( real_t p_step , Body2DSW * p_A , int p_shape_A , const Transform2D & p_xform_A , Body2DSW * p_B , int p_shape_B , const Transform2D & p_xform_B , bool p_swap_result ) {
Vector2 motion = p_A - > get_linear_velocity ( ) * p_step ;
2014-02-10 02:10:30 +01:00
real_t mlen = motion . length ( ) ;
2020-05-14 16:41:43 +02:00
if ( mlen < CMP_EPSILON ) {
2014-02-10 02:10:30 +01:00
return false ;
2020-05-14 16:41:43 +02:00
}
2014-02-10 02:10:30 +01:00
Vector2 mnormal = motion / mlen ;
2017-03-05 16:44:50 +01:00
real_t min , max ;
p_A - > get_shape ( p_shape_A ) - > project_rangev ( mnormal , p_xform_A , min , max ) ;
bool fast_object = mlen > ( max - min ) * 0.3 ; //going too fast in that direction
2014-02-19 15:57:14 +01:00
2015-01-06 03:00:35 +01:00
if ( ! fast_object ) { //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
2014-02-10 02:10:30 +01:00
return false ;
2014-02-19 15:57:14 +01:00
}
2014-02-10 02:10:30 +01:00
//cast a segment from support in motion normal, in the same direction of motion by motion length
2014-02-19 15:57:14 +01:00
//support is the worst case collision point, so real collision happened before
2014-02-10 02:10:30 +01:00
int a ;
Vector2 s [ 2 ] ;
2017-03-05 16:44:50 +01:00
p_A - > get_shape ( p_shape_A ) - > get_supports ( p_xform_A . basis_xform ( mnormal ) . normalized ( ) , s , a ) ;
2014-02-10 02:10:30 +01:00
Vector2 from = p_xform_A . xform ( s [ 0 ] ) ;
Vector2 to = from + motion ;
2017-01-11 04:52:51 +01:00
Transform2D from_inv = p_xform_B . affine_inverse ( ) ;
2014-02-19 15:57:14 +01:00
2017-03-05 16:44:50 +01:00
Vector2 local_from = from_inv . xform ( from - mnormal * mlen * 0.1 ) ; //start from a little inside the bounding box
2014-02-19 15:57:14 +01:00
Vector2 local_to = from_inv . xform ( to ) ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
Vector2 rpos , rnorm ;
2020-05-14 16:41:43 +02:00
if ( ! p_B - > get_shape ( p_shape_B ) - > intersect_segment ( local_from , local_to , rpos , rnorm ) ) {
2014-02-10 02:10:30 +01:00
return false ;
2020-05-14 16:41:43 +02:00
}
2014-02-10 02:10:30 +01:00
//ray hit something
2014-02-19 15:57:14 +01:00
Vector2 hitpos = p_xform_B . xform ( rpos ) ;
2014-02-10 02:10:30 +01:00
Vector2 contact_A = to ;
2014-02-19 15:57:14 +01:00
Vector2 contact_B = hitpos ;
2014-02-10 02:10:30 +01:00
//create a contact
2020-05-14 16:41:43 +02:00
if ( p_swap_result ) {
2017-03-05 16:44:50 +01:00
_contact_added_callback ( contact_B , contact_A ) ;
2020-05-14 16:41:43 +02:00
} else {
2017-03-05 16:44:50 +01:00
_contact_added_callback ( contact_A , contact_B ) ;
2020-05-14 16:41:43 +02:00
}
2014-02-10 02:10:30 +01:00
return true ;
}
2017-10-24 18:10:30 +02:00
real_t combine_bounce ( Body2DSW * A , Body2DSW * 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
}
real_t combine_friction ( Body2DSW * A , Body2DSW * 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
}
2017-02-14 00:25:05 +01:00
bool BodyPair2DSW : : setup ( real_t p_step ) {
2021-04-20 03:38:11 +02:00
dynamic_A = ( A - > get_mode ( ) > PhysicsServer2D : : BODY_MODE_KINEMATIC ) ;
dynamic_B = ( B - > get_mode ( ) > PhysicsServer2D : : BODY_MODE_KINEMATIC ) ;
2021-03-12 02:06:00 +01:00
if ( ! A - > test_collision_mask ( B ) | | A - > has_exception ( B - > get_self ( ) ) | | B - > has_exception ( A - > get_self ( ) ) ) {
2017-03-05 16:44:50 +01:00
collided = false ;
2014-02-19 15:57:14 +01:00
return false ;
}
2021-04-20 03:38:11 +02:00
report_contacts_only = false ;
if ( ! dynamic_A & & ! dynamic_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 ;
}
}
2017-06-24 04:30:43 +02:00
if ( A - > is_shape_set_as_disabled ( shape_A ) | | B - > is_shape_set_as_disabled ( shape_B ) ) {
collided = false ;
return false ;
}
2014-02-10 02:10:30 +01:00
//use local A coordinates to avoid numerical issues on collision detection
offset_B = B - > get_transform ( ) . get_origin ( ) - A - > get_transform ( ) . get_origin ( ) ;
_validate_contacts ( ) ;
2021-04-20 03:38:11 +02:00
const Vector2 & offset_A = A - > get_transform ( ) . get_origin ( ) ;
2017-01-11 04:52:51 +01:00
Transform2D xform_Au = A - > get_transform ( ) . untranslated ( ) ;
Transform2D xform_A = xform_Au * A - > get_shape_transform ( shape_A ) ;
2014-02-10 02:10:30 +01:00
2017-01-11 04:52:51 +01:00
Transform2D xform_Bu = B - > get_transform ( ) ;
2021-04-20 03:38:11 +02:00
xform_Bu . elements [ 2 ] - = offset_A ;
2017-01-11 04:52:51 +01:00
Transform2D xform_B = xform_Bu * B - > get_shape_transform ( shape_B ) ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
Shape2DSW * shape_A_ptr = A - > get_shape ( shape_A ) ;
Shape2DSW * shape_B_ptr = B - > get_shape ( shape_B ) ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
Vector2 motion_A , motion_B ;
2014-02-19 15:57:14 +01:00
2020-03-27 19:21:27 +01:00
if ( A - > get_continuous_collision_detection_mode ( ) = = PhysicsServer2D : : CCD_MODE_CAST_SHAPE ) {
2017-03-05 16:44:50 +01:00
motion_A = A - > get_motion ( ) ;
2016-03-09 00:00:52 +01:00
}
2020-03-27 19:21:27 +01:00
if ( B - > get_continuous_collision_detection_mode ( ) = = PhysicsServer2D : : CCD_MODE_CAST_SHAPE ) {
2017-03-05 16:44:50 +01:00
motion_B = B - > get_motion ( ) ;
2016-03-09 00:00:52 +01:00
}
2014-02-19 15:57:14 +01:00
2020-06-27 15:02:51 +02:00
bool prev_collided = collided ;
2015-05-03 23:18:21 +02:00
2017-03-05 16:44:50 +01:00
collided = CollisionSolver2DSW : : solve ( shape_A_ptr , xform_A , motion_A , shape_B_ptr , xform_B , motion_B , _add_contact , this , & sep_axis ) ;
2014-02-10 02:10:30 +01:00
if ( ! collided ) {
//test ccd (currently just a raycast)
2014-02-19 15:57:14 +01:00
2021-04-20 03:38:11 +02:00
if ( A - > get_continuous_collision_detection_mode ( ) = = PhysicsServer2D : : CCD_MODE_CAST_RAY & & dynamic_A ) {
2020-05-14 16:41:43 +02:00
if ( _test_ccd ( p_step , A , shape_A , xform_A , B , shape_B , xform_B ) ) {
2017-03-05 16:44:50 +01:00
collided = true ;
2020-05-14 16:41:43 +02:00
}
2014-02-10 02:10:30 +01:00
}
2021-04-20 03:38:11 +02:00
if ( B - > get_continuous_collision_detection_mode ( ) = = PhysicsServer2D : : CCD_MODE_CAST_RAY & & dynamic_B ) {
2020-05-14 16:41:43 +02:00
if ( _test_ccd ( p_step , B , shape_B , xform_B , A , shape_A , xform_A , true ) ) {
2017-03-05 16:44:50 +01:00
collided = true ;
2020-05-14 16:41:43 +02:00
}
2014-02-10 02:10:30 +01:00
}
2015-05-11 00:24:09 +02:00
if ( ! collided ) {
2017-03-05 16:44:50 +01:00
oneway_disabled = false ;
2014-02-10 02:10:30 +01:00
return false ;
2015-05-11 00:24:09 +02:00
}
2014-02-10 02:10:30 +01:00
}
2020-05-14 16:41:43 +02:00
if ( oneway_disabled ) {
2015-05-11 00:24:09 +02:00
return false ;
2020-05-14 16:41:43 +02:00
}
2015-05-11 00:24:09 +02:00
2020-06-27 15:02:51 +02:00
if ( ! prev_collided ) {
2017-06-24 04:30:43 +02:00
if ( A - > is_shape_set_as_one_way_collision ( shape_A ) ) {
Vector2 direction = xform_A . get_axis ( 1 ) . normalized ( ) ;
2017-03-05 16:44:50 +01:00
bool valid = false ;
2020-10-05 15:55:11 +02:00
for ( int i = 0 ; i < contact_count ; i + + ) {
Contact & c = contacts [ i ] ;
if ( ! c . reused ) {
continue ;
2015-05-03 23:18:21 +02:00
}
2020-10-05 15:55:11 +02:00
if ( c . normal . dot ( direction ) > - CMP_EPSILON ) { //greater (normal inverted)
continue ;
}
valid = true ;
break ;
2015-05-03 23:18:21 +02:00
}
if ( ! valid ) {
2017-03-05 16:44:50 +01:00
collided = false ;
oneway_disabled = true ;
2015-05-03 23:18:21 +02:00
return false ;
}
}
2017-06-24 04:30:43 +02:00
if ( B - > is_shape_set_as_one_way_collision ( shape_B ) ) {
Vector2 direction = xform_B . get_axis ( 1 ) . normalized ( ) ;
2017-03-05 16:44:50 +01:00
bool valid = false ;
2020-10-05 15:55:11 +02:00
for ( int i = 0 ; i < contact_count ; i + + ) {
Contact & c = contacts [ i ] ;
if ( ! c . reused ) {
continue ;
}
if ( c . normal . dot ( direction ) < CMP_EPSILON ) { //less (normal ok)
continue ;
2015-05-03 23:18:21 +02:00
}
2020-10-05 15:55:11 +02:00
valid = true ;
break ;
2015-05-03 23:18:21 +02:00
}
if ( ! valid ) {
2017-03-05 16:44:50 +01:00
collided = false ;
oneway_disabled = true ;
2015-05-03 23:18:21 +02:00
return false ;
}
}
}
2021-04-20 03:38:11 +02:00
return true ;
}
bool BodyPair2DSW : : pre_solve ( real_t p_step ) {
if ( ! collided | | oneway_disabled ) {
return false ;
}
2014-02-10 02:10:30 +01:00
real_t max_penetration = space - > get_contact_max_allowed_penetration ( ) ;
2017-02-14 00:25:05 +01:00
real_t bias = 0.3 ;
2021-04-20 03:38:11 +02:00
Shape2DSW * shape_A_ptr = A - > get_shape ( shape_A ) ;
Shape2DSW * shape_B_ptr = B - > get_shape ( shape_B ) ;
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 ) {
2017-03-05 16:44:50 +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 ) {
2017-03-05 16:44:50 +01:00
bias = shape_A_ptr - > get_custom_bias ( ) ;
2020-05-14 16:41:43 +02:00
} else {
2017-03-05 16:44:50 +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
}
2017-03-05 16:44:50 +01:00
real_t inv_dt = 1.0 / p_step ;
2016-01-01 16:11:46 +01:00
2017-03-05 16:44:50 +01:00
bool do_process = false ;
2016-01-01 16:11:46 +01:00
2021-04-20 03:38:11 +02:00
const Vector2 & offset_A = A - > get_transform ( ) . get_origin ( ) ;
const Transform2D & transform_A = A - > get_transform ( ) ;
const Transform2D & transform_B = B - > get_transform ( ) ;
2014-02-10 02:10:30 +01:00
for ( int i = 0 ; i < contact_count ; i + + ) {
2017-03-05 16:44:50 +01:00
Contact & c = contacts [ i ] ;
2021-03-12 02:06:00 +01:00
c . active = false ;
2021-04-20 03:38:11 +02:00
Vector2 global_A = transform_A . basis_xform ( c . local_A ) ;
Vector2 global_B = transform_B . basis_xform ( c . local_B ) + offset_B ;
2014-02-10 02:10:30 +01:00
2021-04-20 03:38:11 +02:00
Vector2 axis = global_A - global_B ;
real_t depth = axis . dot ( c . normal ) ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
if ( depth < = 0 | | ! c . reused ) {
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 ( ) ) {
2017-03-05 16:44:50 +01:00
space - > add_debug_contact ( global_A + offset_A ) ;
space - > add_debug_contact ( global_B + offset_A ) ;
2015-09-20 18:03:46 +02:00
}
# endif
2014-02-10 02:10:30 +01:00
c . rA = global_A ;
2017-03-05 16:44:50 +01:00
c . rB = global_B - offset_B ;
2014-02-10 02:10:30 +01:00
2021-03-12 02:06:00 +01:00
if ( A - > can_report_contacts ( ) ) {
Vector2 crB ( - B - > get_angular_velocity ( ) * c . rB . y , B - > get_angular_velocity ( ) * c . rB . x ) ;
A - > add_contact ( global_A + offset_A , - c . normal , depth , shape_A , global_B + offset_A , shape_B , B - > get_instance_id ( ) , B - > get_self ( ) , crB + B - > get_linear_velocity ( ) ) ;
}
2014-02-10 02:10:30 +01:00
2021-03-12 02:06:00 +01:00
if ( B - > can_report_contacts ( ) ) {
Vector2 crA ( - A - > get_angular_velocity ( ) * c . rA . y , A - > get_angular_velocity ( ) * c . rA . x ) ;
B - > add_contact ( global_B + offset_A , c . normal , depth , shape_B , global_A + offset_A , shape_A , A - > get_instance_id ( ) , A - > get_self ( ) , crA + A - > get_linear_velocity ( ) ) ;
2014-02-10 02:10:30 +01:00
}
2021-03-12 02:06:00 +01:00
if ( report_contacts_only ) {
2017-03-05 16:44:50 +01:00
collided = false ;
2014-10-03 13:58:41 +02:00
continue ;
2014-05-14 06:22:15 +02:00
}
2014-02-10 02:10:30 +01:00
// Precompute normal mass, tangent mass, and bias.
real_t rnA = c . rA . dot ( c . normal ) ;
real_t rnB = c . rB . dot ( c . normal ) ;
real_t kNormal = A - > get_inv_mass ( ) + B - > get_inv_mass ( ) ;
kNormal + = A - > get_inv_inertia ( ) * ( c . rA . dot ( c . rA ) - rnA * rnA ) + B - > get_inv_inertia ( ) * ( c . rB . dot ( c . rB ) - rnB * rnB ) ;
c . mass_normal = 1.0f / kNormal ;
2020-12-06 19:16:06 +01:00
Vector2 tangent = c . normal . orthogonal ( ) ;
2014-02-10 02:10:30 +01:00
real_t rtA = c . rA . dot ( tangent ) ;
real_t rtB = c . rB . dot ( tangent ) ;
real_t kTangent = A - > get_inv_mass ( ) + B - > get_inv_mass ( ) ;
kTangent + = A - > get_inv_inertia ( ) * ( c . rA . dot ( c . rA ) - rtA * rtA ) + B - > get_inv_inertia ( ) * ( c . rB . dot ( c . rB ) - rtB * rtB ) ;
2017-03-05 16:44:50 +01:00
c . mass_tangent = 1.0f / kTangent ;
2014-02-10 02:10:30 +01:00
c . bias = - bias * inv_dt * MIN ( 0.0f , - depth + max_penetration ) ;
2017-03-05 16:44:50 +01:00
c . depth = depth ;
2017-12-06 21:36:34 +01:00
//c.acc_bias_impulse=0;
2014-02-10 02:10:30 +01:00
# ifdef ACCUMULATE_IMPULSES
{
// Apply normal + friction impulse
Vector2 P = c . acc_normal_impulse * c . normal + c . acc_tangent_impulse * tangent ;
2021-04-20 03:38:11 +02:00
if ( dynamic_A ) {
A - > apply_impulse ( - P , c . rA ) ;
}
if ( dynamic_B ) {
B - > apply_impulse ( P , c . rB ) ;
}
2014-02-10 02:10:30 +01:00
}
# endif
2014-04-05 17:39:30 +02:00
2017-10-24 18:10:30 +02:00
c . bounce = combine_bounce ( A , B ) ;
2014-04-05 17:39:30 +02:00
if ( c . bounce ) {
2017-03-05 16:44:50 +01:00
Vector2 crA ( - A - > get_angular_velocity ( ) * c . rA . y , A - > get_angular_velocity ( ) * c . rA . x ) ;
Vector2 crB ( - B - > get_angular_velocity ( ) * c . rB . y , B - > get_angular_velocity ( ) * c . rB . x ) ;
2014-04-05 17:39:30 +02:00
Vector2 dv = B - > get_linear_velocity ( ) + crB - A - > get_linear_velocity ( ) - crA ;
c . bounce = c . bounce * dv . dot ( c . normal ) ;
}
2021-04-20 03:38:11 +02:00
c . active = true ;
2017-03-05 16:44:50 +01:00
do_process = true ;
2014-02-10 02:10:30 +01:00
}
2016-01-01 16:11:46 +01:00
return do_process ;
2014-02-10 02:10:30 +01:00
}
2017-02-14 00:25:05 +01:00
void BodyPair2DSW : : solve ( real_t p_step ) {
2021-04-20 03:38:11 +02:00
if ( ! collided | | oneway_disabled ) {
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
for ( int i = 0 ; i < contact_count ; + + i ) {
2017-03-05 16:44:50 +01:00
Contact & c = contacts [ i ] ;
2014-02-10 02:10:30 +01:00
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
// Relative velocity at contact
2017-03-05 16:44:50 +01:00
Vector2 crA ( - A - > get_angular_velocity ( ) * c . rA . y , A - > get_angular_velocity ( ) * c . rA . x ) ;
Vector2 crB ( - B - > get_angular_velocity ( ) * c . rB . y , B - > get_angular_velocity ( ) * c . rB . x ) ;
2014-02-10 02:10:30 +01:00
Vector2 dv = B - > get_linear_velocity ( ) + crB - A - > get_linear_velocity ( ) - crA ;
2017-03-05 16:44:50 +01:00
Vector2 crbA ( - A - > get_biased_angular_velocity ( ) * c . rA . y , A - > get_biased_angular_velocity ( ) * c . rA . x ) ;
Vector2 crbB ( - B - > get_biased_angular_velocity ( ) * c . rB . y , B - > get_biased_angular_velocity ( ) * c . rB . x ) ;
2014-02-10 02:10:30 +01:00
Vector2 dbv = B - > get_biased_linear_velocity ( ) + crbB - A - > get_biased_linear_velocity ( ) - crbA ;
real_t vn = dv . dot ( c . normal ) ;
real_t vbn = dbv . dot ( c . normal ) ;
2020-12-06 19:16:06 +01:00
Vector2 tangent = c . normal . orthogonal ( ) ;
2014-02-10 02:10:30 +01:00
real_t vt = dv . dot ( tangent ) ;
2017-03-05 16:44:50 +01:00
real_t jbn = ( c . bias - vbn ) * c . mass_normal ;
2014-02-10 02:10:30 +01:00
real_t jbnOld = c . acc_bias_impulse ;
c . acc_bias_impulse = MAX ( jbnOld + jbn , 0.0f ) ;
Vector2 jb = c . normal * ( c . acc_bias_impulse - jbnOld ) ;
2021-04-20 03:38:11 +02:00
if ( dynamic_A ) {
A - > apply_bias_impulse ( - jb , c . rA ) ;
}
if ( dynamic_B ) {
B - > apply_bias_impulse ( jb , c . rB ) ;
}
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01: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 ) ;
2017-10-24 18:10:30 +02:00
real_t friction = combine_friction ( A , B ) ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
real_t jtMax = friction * c . acc_normal_impulse ;
real_t jt = - vt * c . mass_tangent ;
2014-02-10 02:10:30 +01:00
real_t jtOld = c . acc_tangent_impulse ;
c . acc_tangent_impulse = CLAMP ( jtOld + jt , - jtMax , jtMax ) ;
2017-03-05 16:44:50 +01:00
Vector2 j = c . normal * ( c . acc_normal_impulse - jnOld ) + tangent * ( c . acc_tangent_impulse - jtOld ) ;
2014-02-10 02:10:30 +01:00
2021-04-20 03:38:11 +02:00
if ( dynamic_A ) {
A - > apply_impulse ( - j , c . rA ) ;
}
if ( dynamic_B ) {
B - > apply_impulse ( j , c . rB ) ;
}
2014-02-10 02:10:30 +01:00
}
}
2017-12-06 21:36:34 +01:00
BodyPair2DSW : : BodyPair2DSW ( Body2DSW * p_A , int p_shape_A , Body2DSW * p_B , int p_shape_B ) :
Constraint2DSW ( _arr , 2 ) {
2017-03-05 16:44:50 +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 ) ;
2014-02-10 02:10:30 +01:00
}
BodyPair2DSW : : ~ BodyPair2DSW ( ) {
2020-12-05 05:12:47 +01:00
A - > remove_constraint ( this , 0 ) ;
B - > remove_constraint ( this , 1 ) ;
2014-02-10 02:10:30 +01:00
}