/*************************************************************************/ /* body_pair_2d_sw.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ /* Copyright (c) 2007-2015 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. */ /*************************************************************************/ #include "body_pair_2d_sw.h" #include "collision_solver_2d_sw.h" #include "space_2d_sw.h" #define POSITION_CORRECTION #define ACCUMULATE_IMPULSES void BodyPair2DSW::_add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self) { BodyPair2DSW *self = (BodyPair2DSW *)p_self; self->_contact_added_callback(p_point_A,p_point_B); } void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B) { // check if we already have the contact Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A); Vector2 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; 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(); // attempt to determine if the contact will be reused real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius(); for (int i=0;iget_transform().basis_xform(c.local_A); Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B; Vector2 axis = global_A - global_B; float depth = axis.dot( c.normal ); if (depthget_contact_max_separation(); real_t max_separation2 = max_separation*max_separation; for (int i=0;iget_transform().basis_xform(c.local_A); Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B; Vector2 axis = global_A - global_B; float depth = axis.dot( c.normal ); if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { erase=true; } } if (erase) { // contact no longer needed, remove if ((i+1) < contact_count) { // swap with the last one SWAP( contacts[i], contacts[ contact_count-1 ] ); } i--; contact_count--; } } } bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,bool p_swap_result) { Vector2 motion = p_A->get_linear_velocity()*p_step; real_t mlen = motion.length(); if (mlenget_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 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 return false; } //cast a segment from support in motion normal, in the same direction of motion by motion length //support is the worst case collision point, so real collision happened before int a; Vector2 s[2]; p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(),s,a); Vector2 from = p_xform_A.xform(s[0]); Vector2 to = from + motion; Matrix32 from_inv = p_xform_B.affine_inverse(); Vector2 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box Vector2 local_to = from_inv.xform(to); Vector2 rpos,rnorm; if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm)) return false; //ray hit something Vector2 hitpos = p_xform_B.xform(rpos); Vector2 contact_A = to; Vector2 contact_B = hitpos; //create a contact if (p_swap_result) _contact_added_callback(contact_B,contact_A); else _contact_added_callback(contact_A,contact_B); return true; } bool BodyPair2DSW::setup(float p_step) { //cannot collide if ((A->get_layer_mask()&B->get_layer_mask())==0 || 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; } //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(); Matrix32 xform_Au = A->get_transform().untranslated(); Matrix32 xform_A = xform_Au * A->get_shape_transform(shape_A); Matrix32 xform_Bu = B->get_transform(); xform_Bu.elements[2]-=A->get_transform().get_origin(); Matrix32 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.. 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) 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; } cc=0; real_t inv_dt = 1.0/p_step; 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; 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->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (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); } } return true; } void BodyPair2DSW::solve(float p_step) { if (!collided) return; for (int i = 0; i < contact_count; ++i) { Contact& c = contacts[i]; cc++; if (!c.active) continue; // Relative velocity at contact 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; 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 ); 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); real_t jbn = (c.bias - vbn)*c.mass_normal; 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); A->apply_bias_impulse(c.rA,-jb); B->apply_bias_impulse(c.rB, jb); 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); real_t friction = A->get_friction() * B->get_friction(); real_t jtMax = friction*c.acc_normal_impulse; real_t jt = -vt*c.mass_tangent; real_t jtOld = c.acc_tangent_impulse; c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax); Vector2 j =c.normal * (c.acc_normal_impulse - jnOld) + tangent * ( c.acc_tangent_impulse - jtOld ); A->apply_impulse(c.rA,-j); B->apply_impulse(c.rB, j); } } BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_shape_B) : Constraint2DSW(_arr,2) { 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; } BodyPair2DSW::~BodyPair2DSW() { A->remove_constraint(this); B->remove_constraint(this); }