diff options
| -rw-r--r-- | servers/physics/body_pair_sw.cpp | 38 | ||||
| -rw-r--r-- | servers/physics/body_pair_sw.h | 1 | ||||
| -rw-r--r-- | servers/physics/body_sw.h | 10 |
3 files changed, 37 insertions, 12 deletions
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index a289b4b0ca..ef54eb58cf 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -46,6 +46,7 @@ //#define ALLOWED_PENETRATION 0.01 #define RELAXATION_TIMESTEPS 3 #define MIN_VELOCITY 0.0001 +#define MAX_BIAS_ROTATION (Math_PI / 8) void BodyPairSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { @@ -71,6 +72,7 @@ void BodyPairSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 contact.acc_normal_impulse = 0; contact.acc_bias_impulse = 0; + contact.acc_bias_impulse_center_of_mass = 0; contact.acc_tangent_impulse = Vector3(); contact.local_A = local_A; contact.local_B = local_B; @@ -82,12 +84,12 @@ void BodyPairSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 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) && + 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_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; contact.acc_tangent_impulse = c.acc_tangent_impulse; new_index = i; break; @@ -325,9 +327,7 @@ bool BodyPairSW::setup(real_t p_step) { A->apply_impulse(c.rA + A->get_center_of_mass(), -j_vec); B->apply_impulse(c.rB + B->get_center_of_mass(), j_vec); c.acc_bias_impulse = 0; - Vector3 jb_vec = c.normal * c.acc_bias_impulse; - A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb_vec); - B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb_vec); + c.acc_bias_impulse_center_of_mass = 0; c.bounce = MAX(A->get_bounce(), B->get_bounce()); if (c.bounce) { @@ -356,7 +356,7 @@ void BodyPairSW::solve(real_t p_step) { c.active = false; //try to deactivate, will activate itself if still needed - //bias impule + //bias impulse Vector3 crbA = A->get_biased_angular_velocity().cross(c.rA); Vector3 crbB = B->get_biased_angular_velocity().cross(c.rB); @@ -372,8 +372,26 @@ void BodyPairSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb); - B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb); + A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb, MAX_BIAS_ROTATION / p_step); + B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb, MAX_BIAS_ROTATION / p_step); + + crbA = A->get_biased_angular_velocity().cross(c.rA); + crbB = B->get_biased_angular_velocity().cross(c.rB); + dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + + real_t jbn_com = (-vbn + c.bias) / (A->get_inv_mass() + B->get_inv_mass()); + real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; + c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); + + Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); + + A->apply_bias_impulse(A->get_center_of_mass(), -jb_com, 0.0f); + B->apply_bias_impulse(B->get_center_of_mass(), jb_com, 0.0f); + } c.active = true; } @@ -382,7 +400,7 @@ void BodyPairSW::solve(real_t p_step) { Vector3 crB = B->get_angular_velocity().cross(c.rB); Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; - //normal impule + //normal impulse real_t vn = dv.dot(c.normal); if (Math::abs(vn) > MIN_VELOCITY) { @@ -399,7 +417,7 @@ void BodyPairSW::solve(real_t p_step) { c.active = true; } - //friction impule + //friction impulse real_t friction = A->get_friction() * B->get_friction(); diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h index f09c977950..74fda60998 100644 --- a/servers/physics/body_pair_sw.h +++ b/servers/physics/body_pair_sw.h @@ -59,6 +59,7 @@ class BodyPairSW : public ConstraintSW { real_t acc_normal_impulse; // accumulated normal impulse (Pn) Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb) + real_t acc_bias_impulse_center_of_mass; // accumulated normal impulse for position bias applied to com real_t mass_normal; real_t bias; real_t bounce; diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h index 7e8d31f8eb..97994df6fa 100644 --- a/servers/physics/body_sw.h +++ b/servers/physics/body_sw.h @@ -223,10 +223,16 @@ public: angular_velocity += _inv_inertia_tensor.xform(p_j); } - _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_pos, const Vector3 &p_j) { + _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_pos, const Vector3 &p_j, real_t p_max_delta_av = -1.0) { biased_linear_velocity += p_j * _inv_mass; - biased_angular_velocity += _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); + if (p_max_delta_av != 0.0) { + Vector3 delta_av = _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); + if (p_max_delta_av > 0 && delta_av.length() > p_max_delta_av) { + delta_av = delta_av.normalized() * p_max_delta_av; + } + biased_angular_velocity += delta_av; + } } _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_j) { |