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
/*************************************************************************/
2019-01-03 11:41:35 +01:00
/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2019 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 ( ) ;
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 + + ) {
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
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 ) ) {
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
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
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < = contact_count ; i + + ) {
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
Contact & c = ( i = = contact_count ) ? contact : contacts [ i ] ;
2014-02-10 02:10:30 +01:00
Vector2 global_A = A - > get_transform ( ) . basis_xform ( c . local_A ) ;
2017-03-05 16:44:50 +01:00
Vector2 global_B = B - > get_transform ( ) . 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 ) {
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
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
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < contact_count ; i + + ) {
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
Contact & c = contacts [ i ] ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
bool erase = false ;
if ( c . reused = = false ) {
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
Vector2 global_A = A - > get_transform ( ) . basis_xform ( c . local_A ) ;
2017-03-05 16:44:50 +01:00
Vector2 global_B = B - > get_transform ( ) . 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 ) {
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
Vector2 motion = p_A - > get_linear_velocity ( ) * p_step ;
2014-02-10 02:10:30 +01:00
real_t mlen = motion . length ( ) ;
2017-03-05 16:44:50 +01:00
if ( mlen < CMP_EPSILON )
2014-02-10 02:10:30 +01:00
return false ;
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 ;
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 ;
//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
if ( p_swap_result )
2017-03-05 16:44:50 +01:00
_contact_added_callback ( contact_B , contact_A ) ;
2014-02-10 02:10:30 +01:00
else
2017-03-05 16:44:50 +01:00
_contact_added_callback ( contact_A , contact_B ) ;
2014-02-10 02:10:30 +01:00
return true ;
}
2017-02-14 00:25:05 +01:00
bool BodyPair2DSW : : setup ( real_t p_step ) {
2014-02-10 02:10:30 +01:00
2014-02-19 15:57:14 +01:00
//cannot collide
2017-03-05 16:44:50 +01:00
if ( ! A - > test_collision_mask ( B ) | | A - > has_exception ( B - > get_self ( ) ) | | B - > has_exception ( A - > get_self ( ) ) | | ( A - > get_mode ( ) < = Physics2DServer : : BODY_MODE_KINEMATIC & & B - > get_mode ( ) < = Physics2DServer : : BODY_MODE_KINEMATIC & & A - > get_max_contacts_reported ( ) = = 0 & & B - > get_max_contacts_reported ( ) = = 0 ) ) {
collided = false ;
2014-02-19 15:57:14 +01:00
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 ( ) ;
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 ( ) ;
2017-03-05 16:44:50 +01:00
xform_Bu . elements [ 2 ] - = A - > get_transform ( ) . get_origin ( ) ;
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
2017-03-05 16:44:50 +01:00
if ( A - > get_continuous_collision_detection_mode ( ) = = Physics2DServer : : CCD_MODE_CAST_SHAPE ) {
motion_A = A - > get_motion ( ) ;
2016-03-09 00:00:52 +01:00
}
2017-03-05 16:44:50 +01:00
if ( B - > get_continuous_collision_detection_mode ( ) = = Physics2DServer : : CCD_MODE_CAST_SHAPE ) {
motion_B = B - > get_motion ( ) ;
2016-03-09 00:00:52 +01:00
}
2014-02-19 15:57:14 +01:00
//faster to set than to check..
2015-05-11 00:24:09 +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
2017-03-05 16:44:50 +01:00
if ( A - > get_continuous_collision_detection_mode ( ) = = Physics2DServer : : CCD_MODE_CAST_RAY & & A - > get_mode ( ) > Physics2DServer : : BODY_MODE_KINEMATIC ) {
if ( _test_ccd ( p_step , A , shape_A , xform_A , B , shape_B , xform_B ) )
collided = true ;
2014-02-10 02:10:30 +01:00
}
2017-03-05 16:44:50 +01:00
if ( B - > get_continuous_collision_detection_mode ( ) = = Physics2DServer : : CCD_MODE_CAST_RAY & & B - > get_mode ( ) > Physics2DServer : : BODY_MODE_KINEMATIC ) {
if ( _test_ccd ( p_step , B , shape_B , xform_B , A , shape_A , xform_A , true ) )
collided = true ;
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
}
2015-05-11 00:24:09 +02:00
if ( oneway_disabled )
return false ;
//if (!prev_collided) {
{
2015-05-03 23:18:21 +02:00
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 ;
if ( B - > get_linear_velocity ( ) . dot ( direction ) > = 0 ) {
for ( int i = 0 ; i < contact_count ; i + + ) {
Contact & c = contacts [ i ] ;
2016-06-24 08:43:23 +02:00
if ( ! c . reused )
continue ;
2017-03-05 16:44:50 +01:00
if ( c . normal . dot ( direction ) < 0 )
2016-06-24 08:43:23 +02:00
continue ;
2017-03-05 16:44:50 +01:00
valid = true ;
2016-06-24 08:43:23 +02:00
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 ;
if ( A - > get_linear_velocity ( ) . dot ( direction ) > = 0 ) {
for ( int i = 0 ; i < contact_count ; i + + ) {
Contact & c = contacts [ i ] ;
2016-06-24 08:43:23 +02:00
if ( ! c . reused )
continue ;
2017-03-05 16:44:50 +01:00
if ( c . normal . dot ( direction ) < 0 )
2016-06-24 08:43:23 +02:00
continue ;
2017-03-05 16:44:50 +01:00
valid = true ;
2016-06-24 08:43:23 +02:00
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 ;
}
}
}
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 ;
2014-02-10 02:10:30 +01:00
if ( shape_A_ptr - > get_custom_bias ( ) | | shape_B_ptr - > get_custom_bias ( ) ) {
2017-03-05 16:44:50 +01:00
if ( shape_A_ptr - > get_custom_bias ( ) = = 0 )
bias = shape_B_ptr - > get_custom_bias ( ) ;
else if ( shape_B_ptr - > get_custom_bias ( ) = = 0 )
bias = shape_A_ptr - > get_custom_bias ( ) ;
2014-02-10 02:10:30 +01: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 ;
2014-02-10 02:10:30 +01:00
}
2017-03-05 16:44:50 +01:00
cc = 0 ;
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
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
Vector2 global_A = xform_Au . xform ( c . local_A ) ;
Vector2 global_B = xform_Bu . xform ( c . local_B ) ;
real_t depth = c . normal . dot ( global_A - global_B ) ;
2017-03-05 16:44:50 +01:00
if ( depth < = 0 | | ! c . reused ) {
c . active = false ;
2014-02-10 02:10:30 +01:00
continue ;
}
2017-03-05 16:44:50 +01:00
c . active = true ;
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
int gather_A = A - > can_report_contacts ( ) ;
int gather_B = B - > can_report_contacts ( ) ;
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
if ( gather_A | gather_B ) {
//Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
2017-03-05 16:44:50 +01:00
global_A + = offset_A ;
global_B + = offset_A ;
2014-02-10 02:10:30 +01:00
if ( gather_A ) {
2017-03-05 16:44:50 +01:00
Vector2 crB ( - B - > get_angular_velocity ( ) * c . rB . y , B - > get_angular_velocity ( ) * c . rB . x ) ;
A - > add_contact ( global_A , - c . normal , depth , shape_A , global_B , shape_B , B - > get_instance_id ( ) , B - > get_self ( ) , crB + B - > get_linear_velocity ( ) ) ;
2014-02-10 02:10:30 +01:00
}
if ( gather_B ) {
2017-03-05 16:44:50 +01:00
Vector2 crA ( - A - > get_angular_velocity ( ) * c . rA . y , A - > get_angular_velocity ( ) * c . rA . x ) ;
B - > add_contact ( global_B , c . normal , depth , shape_B , global_A , shape_A , A - > get_instance_id ( ) , A - > get_self ( ) , crA + A - > get_linear_velocity ( ) ) ;
2014-02-10 02:10:30 +01:00
}
}
2017-06-24 04:30:43 +02:00
if ( ( A - > get_mode ( ) < = Physics2DServer : : BODY_MODE_KINEMATIC & & B - > get_mode ( ) < = Physics2DServer : : BODY_MODE_KINEMATIC ) ) {
2017-03-05 16:44:50 +01:00
c . active = false ;
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 ;
Vector2 tangent = c . normal . tangent ( ) ;
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 ;
2017-03-05 16:44:50 +01:00
A - > apply_impulse ( c . rA , - P ) ;
2014-02-10 02:10:30 +01:00
B - > apply_impulse ( c . rB , P ) ;
}
# endif
2014-04-05 17:39:30 +02:00
2017-03-05 16:44:50 +01:00
c . bounce = MAX ( A - > get_bounce ( ) , B - > get_bounce ( ) ) ;
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 ) ;
}
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 ) {
2014-02-10 02:10:30 +01:00
if ( ! collided )
return ;
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
cc + + ;
if ( ! c . active )
continue ;
// 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 ) ;
Vector2 tangent = c . normal . tangent ( ) ;
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 ) ;
2017-03-05 16:44:50 +01:00
A - > apply_bias_impulse ( c . rA , - jb ) ;
2014-02-10 02:10:30 +01:00
B - > apply_bias_impulse ( c . rB , jb ) ;
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 ) ;
real_t friction = A - > get_friction ( ) * B - > get_friction ( ) ;
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
2017-03-05 16:44:50 +01:00
A - > apply_impulse ( c . rA , - j ) ;
2014-02-10 02:10:30 +01:00
B - > apply_impulse ( c . rB , j ) ;
}
}
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 ) ;
contact_count = 0 ;
collided = false ;
oneway_disabled = false ;
2014-02-10 02:10:30 +01:00
}
BodyPair2DSW : : ~ BodyPair2DSW ( ) {
A - > remove_constraint ( this ) ;
B - > remove_constraint ( this ) ;
}