bool BodyPair2DSW::setup(real_t p_step) {
//cannot collide
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;
return false;
}
if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
collided = false;
return false;
}
//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();
Transform2D xform_Au = A->get_transform().untranslated();
Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
Transform2D xform_Bu = B->get_transform();
xform_Bu.elements[2] -= A->get_transform().get_origin();
Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
Shape2DSW *shape_A_ptr = A->get_shape(shape_A);
Shape2DSW *shape_B_ptr = B->get_shape(shape_B);
Vector2 motion_A, motion_B;
if (A->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_SHAPE) {
motion_A = A->get_motion();
}
if (B->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_SHAPE) {
motion_B = B->get_motion();
}
//faster to set than to check..
//bool prev_collided=collided;
collided = CollisionSolver2DSW::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis);
if (!collided) {
//test ccd (currently just a raycast)
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;
}
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;
}
if (!collided) {
oneway_disabled = false;
return false;
}
}
if (oneway_disabled)
return false;
//if (!prev_collided) {
{
if (A->is_shape_set_as_one_way_collision(shape_A)) {
Vector2 direction = xform_A.get_axis(1).normalized();
bool valid = false;
if (B->get_linear_velocity().dot(direction) >= 0) {
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
if (!c.reused)
continue;
if (c.normal.dot(direction) < 0)
continue;
valid = true;
break;
}
}
if (!valid) {
collided = false;
oneway_disabled = true;
return false;
}
}
if (B->is_shape_set_as_one_way_collision(shape_B)) {
Vector2 direction = xform_B.get_axis(1).normalized();
bool valid = false;
if (A->get_linear_velocity().dot(direction) >= 0) {
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
if (!c.reused)
continue;
if (c.normal.dot(direction) < 0)
continue;
valid = true;
break;
}
}
if (!valid) {
collided = false;
oneway_disabled = true;
return false;
}
}
}
real_t max_penetration = space->get_contact_max_allowed_penetration();
real_t bias = 0.3;
if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
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();
else
bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5;
}
cc = 0;
real_t inv_dt = 1.0 / p_step;
bool do_process = false;
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
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);
if (depth <= 0 || !c.reused) {
c.active = false;
continue;
}
c.active = true;
#ifdef DEBUG_ENABLED
if (space->is_debugging_contacts()) {
space->add_debug_contact(global_A + offset_A);
space->add_debug_contact(global_B + offset_A);
}
#endif
int gather_A = A->can_report_contacts();
int gather_B = B->can_report_contacts();
c.rA = global_A;
c.rB = global_B - offset_B;
if (gather_A | gather_B) {
//Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
global_A += offset_A;
global_B += offset_A;
if (gather_A) {
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());
}
if (gather_B) {
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());
}
}
if ((A->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC)) {
c.active = false;
collided = false;
continue;
}
// 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);
c.mass_tangent = 1.0f / kTangent;
c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
c.depth = depth;
//c.acc_bias_impulse=0;
#ifdef ACCUMULATE_IMPULSES
{
// Apply normal + friction impulse
Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
A->apply_impulse(c.rA, -P);
B->apply_impulse(c.rB, P);
}
#endif
c.bounce = MAX(A->get_bounce(), B->get_bounce());
if (c.bounce) {
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);
Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
c.bounce = c.bounce * dv.dot(c.normal);
}
do_process = true;
}
return do_process;
}
bool BodyPairSW::setup(float p_step) {
//cannot collide
if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) {
collided=false;
return false;
}
offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
validate_contacts();
Vector3 offset_A = A->get_transform().get_origin();
Transform xform_Au = Transform(A->get_transform().basis,Vector3());
Transform xform_A = xform_Au * A->get_shape_transform(shape_A);
Transform xform_Bu = B->get_transform();
xform_Bu.origin-=offset_A;
Transform xform_B = xform_Bu * B->get_shape_transform(shape_B);
ShapeSW *shape_A_ptr=A->get_shape(shape_A);
ShapeSW *shape_B_ptr=B->get_shape(shape_B);
bool collided = CollisionSolverSW::solve_static(shape_A_ptr,xform_A,shape_B_ptr,xform_B,_contact_added_callback,this,&sep_axis);
this->collided=collided;
if (!collided) {
//test ccd (currently just a raycast)
if (A->is_continuous_collision_detection_enabled() && A->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) {
_test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_B);
}
if (B->is_continuous_collision_detection_enabled() && B->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) {
_test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A);
}
return false;
}
real_t max_penetration = space->get_contact_max_allowed_penetration();
float bias = 0.3f;
if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
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();
else
bias=(shape_B_ptr->get_custom_bias()+shape_A_ptr->get_custom_bias())*0.5;
}
real_t inv_dt = 1.0/p_step;
for(int i=0;i<contact_count;i++) {
Contact &c = contacts[i];
c.active=false;
Vector3 global_A = xform_Au.xform(c.local_A);
Vector3 global_B = xform_Bu.xform(c.local_B);
real_t depth = c.normal.dot(global_A - global_B);
if (depth<=0) {
c.active=false;
continue;
}
c.active=true;
#ifdef DEBUG_ENABLED
if (space->is_debugging_contacts()) {
space->add_debug_contact(global_A+offset_A);
space->add_debug_contact(global_B+offset_A);
}
#endif
int gather_A = A->can_report_contacts();
int gather_B = B->can_report_contacts();
c.rA = global_A;
c.rB = global_B-offset_B;
// contact query reporting...
#if 0
if (A->get_body_type() == PhysicsServer::BODY_CHARACTER)
static_cast<CharacterBodySW*>(A)->report_character_contact( global_A, global_B, B );
if (B->get_body_type() == PhysicsServer::BODY_CHARACTER)
static_cast<CharacterBodySW*>(B)->report_character_contact( global_B, global_A, A );
if (A->has_contact_query())
A->report_contact( global_A, global_B, B );
if (B->has_contact_query())
B->report_contact( global_B, global_A, A );
#endif
if (A->can_report_contacts()) {
Vector3 crB = A->get_angular_velocity().cross( c.rA ) + A->get_linear_velocity();
A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crB);
}
if (B->can_report_contacts()) {
Vector3 crA = A->get_angular_velocity().cross( c.rB ) + A->get_linear_velocity();
B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA);
}
if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) {
c.active=false;
collided=false;
continue;
}
c.active=true;
// Precompute normal mass, tangent mass, and bias.
Vector3 inertia_A = A->get_inv_inertia_tensor().xform( c.rA.cross( c.normal ) );
Vector3 inertia_B = B->get_inv_inertia_tensor().xform( c.rB.cross( c.normal ) );
real_t kNormal = A->get_inv_mass() + B->get_inv_mass();
kNormal += c.normal.dot( inertia_A.cross(c.rA ) ) + c.normal.dot( inertia_B.cross( c.rB ));
c.mass_normal = 1.0f / kNormal;
#if 1
c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
#else
if (depth > max_penetration) {
c.bias = (depth - max_penetration) * (1.0/(p_step*(1.0/RELAXATION_TIMESTEPS)));
} else {
float approach = -0.1f * (depth - max_penetration) / (CMP_EPSILON + max_penetration);
approach = CLAMP( approach, CMP_EPSILON, 1.0 );
c.bias = approach * (depth - max_penetration) * (1.0/p_step);
}
#endif
c.depth=depth;
Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
A->apply_impulse( c.rA, -j_vec );
B->apply_impulse( c.rB, j_vec );
c.acc_bias_impulse=0;
Vector3 jb_vec = c.normal * c.acc_bias_impulse;
A->apply_bias_impulse( c.rA, -jb_vec );
B->apply_bias_impulse( c.rB, jb_vec );
c.bounce = MAX(A->get_bounce(),B->get_bounce());
if (c.bounce) {
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;
//normal impule
c.bounce = c.bounce * dv.dot(c.normal);
}
}
return true;
}
