diff options
Diffstat (limited to 'servers/physics/body_pair_sw.cpp')
| -rw-r--r-- | servers/physics/body_pair_sw.cpp | 442 |
1 files changed, 442 insertions, 0 deletions
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp new file mode 100644 index 0000000000..06ae34098c --- /dev/null +++ b/servers/physics/body_pair_sw.cpp @@ -0,0 +1,442 @@ +/*************************************************************************/ +/* body_pair_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* 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. */ +/*************************************************************************/ +#include "body_pair_sw.h" +#include "collision_solver_sw.h" +#include "space_sw.h" + + +/* +#define NO_ACCUMULATE_IMPULSES +#define NO_SPLIT_IMPULSES + +#define NO_FRICTION +*/ + +#define NO_TANGENTIALS +/* BODY PAIR */ + + +//#define ALLOWED_PENETRATION 0.01 +#define RELAXATION_TIMESTEPS 3 +#define MIN_VELOCITY 0.0001 + +void BodyPairSW::_contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) { + + BodyPairSW* pair = (BodyPairSW*)p_userdata; + pair->contact_added_callback(p_point_A,p_point_B); + +} + +void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B) { + + // check if we already have the contact + + //Vector3 local_A = A->get_inv_transform().xform(p_point_A); + //Vector3 local_B = B->get_inv_transform().xform(p_point_B); + + 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; + + contact.acc_normal_impulse=0; + contact.acc_bias_impulse=0; + contact.acc_tangent_impulse=Vector3(); + contact.local_A=local_A; + contact.local_B=local_B; + contact.normal=(p_point_A-p_point_B).normalized(); + + + + // attempt to determine if the contact will be reused + real_t contact_recycle_radius=space->get_contact_recycle_radius(); + + for (int i=0;i<contact_count;i++) { + + Contact& c = contacts[i]; + 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_tangent_impulse=c.acc_tangent_impulse; + new_index=i; + 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 + + int least_deep=-1; + float min_depth=1e10; + + for (int i=0;i<=contact_count;i++) { + + Contact& c = (i==contact_count)?contact:contacts[i]; + Vector3 global_A = A->get_transform().basis.xform(c.local_A); + Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B; + + Vector3 axis = global_A - global_B; + float depth = axis.dot( c.normal ); + + if (depth<min_depth) { + + min_depth=depth; + least_deep=i; + } + } + + ERR_FAIL_COND(least_deep==-1); + + if (least_deep < contact_count) { //replace the last deep contact by the new one + + contacts[least_deep]=contact; + } + + return; + } + + contacts[new_index]=contact; + + if (new_index==contact_count) { + + contact_count++; + } + +} + +void BodyPairSW::validate_contacts() { + + //make sure to erase contacts that are no longer valid + + real_t contact_max_separation=space->get_contact_max_separation(); + for (int i=0;i<contact_count;i++) { + + Contact& c = contacts[i]; + + Vector3 global_A = A->get_transform().basis.xform(c.local_A); + Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B; + Vector3 axis = global_A - global_B; + float depth = axis.dot( c.normal ); + + if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { + // 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 BodyPairSW::setup(float p_step) { + + + 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) + return false; + + + //cannot collide + if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE && B->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE)) { + 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; + + 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); + } + + 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 ); + + } + + return true; +} + +void BodyPairSW::solve(float p_step) { + + if (!collided) + return; + + + for(int i=0;i<contact_count;i++) { + + Contact &c = contacts[i]; + if (!c.active) + continue; + + c.active=false; //try to deactivate, will activate itself if still needed + + //bias impule + + 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); + + + A->apply_bias_impulse(c.rA,-jb); + B->apply_bias_impulse(c.rB, jb); + + 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; + + //normal impule + real_t vn = dv.dot(c.normal); + + if (Math::abs(vn)>MIN_VELOCITY) { + + real_t bounce=0; + real_t jn = (-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); + + + A->apply_impulse(c.rA,-j); + B->apply_impulse(c.rB, j); + + c.active=true; + } + + //friction impule + + real_t friction = A->get_friction() * B->get_friction(); + + 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; + + Vector3 temp1 = A->get_inv_inertia_tensor().xform( c.rA.cross( tv ) ); + Vector3 temp2 = B->get_inv_inertia_tensor().xform( c.rB.cross( tv ) ); + + real_t t = -tvl / + (A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); + + 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; + + + A->apply_impulse( c.rA, -jt ); + B->apply_impulse( c.rB, jt ); + + c.active=true; + + } + + + } + +} + + + + + +BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A,BodySW *p_B, int p_shape_B) : ConstraintSW(_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; + +} + + +BodyPairSW::~BodyPairSW() { + + A->remove_constraint(this); + B->remove_constraint(this); + +} |