summaryrefslogtreecommitdiffstats
path: root/servers
diff options
context:
space:
mode:
Diffstat (limited to 'servers')
-rw-r--r--servers/physics/body_pair_sw.cpp38
-rw-r--r--servers/physics/body_pair_sw.h1
-rw-r--r--servers/physics/body_sw.h10
-rw-r--r--servers/physics_2d/space_2d_sw.cpp26
4 files changed, 50 insertions, 25 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 fc64ca5817..738d99c764 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -227,10 +227,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) {
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 6eaaaa777b..f9febd1093 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -626,13 +626,13 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
int amount = _cull_aabb_for_body(p_body, motion_aabb);
- for (int j = 0; j < p_body->get_shape_count(); j++) {
+ for (int body_shape_idx = 0; body_shape_idx < p_body->get_shape_count(); body_shape_idx++) {
- if (p_body->is_shape_set_as_disabled(j))
+ if (p_body->is_shape_set_as_disabled(body_shape_idx))
continue;
- Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);
- Shape2DSW *body_shape = p_body->get_shape(j);
+ Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx);
+ Shape2DSW *body_shape = p_body->get_shape(body_shape_idx);
bool stuck = false;
@@ -642,14 +642,14 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
- int shape_idx = intersection_query_subindex_results[i];
- Shape2DSW *against_shape = col_obj->get_shape(shape_idx);
+ int col_shape_idx = intersection_query_subindex_results[i];
+ Shape2DSW *against_shape = col_obj->get_shape(col_shape_idx);
bool excluded = false;
for (int k = 0; k < excluded_shape_pair_count; k++) {
- if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == shape_idx) {
+ if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == col_shape_idx) {
excluded = true;
break;
}
@@ -660,7 +660,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
continue;
}
- Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+ Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx);
//test initial overlap, does it collide if going all the way?
if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion, against_shape, col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {
continue;
@@ -669,7 +669,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
//test initial overlap
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {
- if (col_obj->is_shape_set_as_one_way_collision(j)) {
+ if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
continue;
}
@@ -698,7 +698,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
}
}
- if (col_obj->is_shape_set_as_one_way_collision(j)) {
+ if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
Vector2 cd[2];
Physics2DServerSW::CollCbkData cbk;
@@ -710,7 +710,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
cbk.valid_depth = 10e20;
Vector2 sep = mnormal; //important optimization for this to work fast enough
- bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), Physics2DServerSW::_shape_col_cbk, &cbk, &sep, 0);
+ bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_xform, Vector2(), Physics2DServerSW::_shape_col_cbk, &cbk, &sep, 0);
if (!collided || cbk.amount == 0) {
continue;
}
@@ -726,7 +726,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
safe = 0;
unsafe = 0;
- best_shape = j; //sadly it's the best
+ best_shape = body_shape_idx; //sadly it's the best
break;
}
if (best_safe == 1.0) {
@@ -736,7 +736,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
safe = best_safe;
unsafe = best_unsafe;
- best_shape = j;
+ best_shape = body_shape_idx;
}
}
}