summaryrefslogtreecommitdiffstats
path: root/servers/physics_3d/godot_body_pair_3d.cpp
diff options
context:
space:
mode:
authorPouleyKetchoupp <pouleyketchoup@gmail.com>2021-12-03 10:38:40 -0700
committerPouleyKetchoupp <pouleyketchoup@gmail.com>2021-12-03 10:40:15 -0700
commit5cbc7149a1cd9a55ffccf8ca96e48b872417ce89 (patch)
tree9de8fbff38701bf81ded98d8bbf7f3d877df308b /servers/physics_3d/godot_body_pair_3d.cpp
parent80e292b3e019d4084b09431572bba12f87e81023 (diff)
downloadredot-engine-5cbc7149a1cd9a55ffccf8ca96e48b872417ce89.tar.gz
Improve RigidDynamicBody contacts in 2D and 3D
Changed the algorithm for solving contacts to keep previous contacts as long as they are under the max separation threshold to keep contact impulses more consistent and contacts more stable. Also made 2D consistent with 3D and changed some default parameters: -Contact bias is now 0.8 instead of 0.3 to avoid springy contacts -Solver iterations are 16 instead of 8 by default for better stability Performance considerations: Tested with stress tests that include lots of contacts from overlapping bodies. 3D: There's no measurable difference in performance. 2D: Performance is a bit lower (close to 10% slower in extreme cases) The benefit for 2D physics to be much more stable outweighs the slight decrease in performance, and this could be alleviated by changing the algorithm to use jacobians for contact solving to help with cache efficiency and memory allocations.
Diffstat (limited to 'servers/physics_3d/godot_body_pair_3d.cpp')
-rw-r--r--servers/physics_3d/godot_body_pair_3d.cpp130
1 files changed, 73 insertions, 57 deletions
diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index 8a701466ed..28bb6fb466 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -44,11 +44,6 @@ void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_in
}
void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_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);
@@ -57,19 +52,14 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind
ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1));
Contact contact;
-
- contact.acc_normal_impulse = 0;
- contact.acc_bias_impulse = 0;
- contact.acc_bias_impulse_center_of_mass = 0;
- contact.acc_tangent_impulse = Vector3();
contact.index_A = p_index_A;
contact.index_B = p_index_B;
contact.local_A = local_A;
contact.local_B = local_B;
contact.normal = (p_point_A - p_point_B).normalized();
- contact.mass_normal = 0; // will be computed in setup()
+ contact.used = true;
- // attempt to determine if the contact will be reused
+ // 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++) {
@@ -80,23 +70,34 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind
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;
+ c = contact;
+ return;
}
}
- // figure out if the contact amount must be reduced to fit the new contact
-
+ // 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
+ // Remove the contact with the minimum depth.
+
+ const Basis &basis_A = A->get_transform().basis;
+ const Basis &basis_B = B->get_transform().basis;
int least_deep = -1;
- real_t min_depth = 1e10;
+ real_t min_depth;
+
+ // Start with depth for new contact.
+ {
+ Vector3 global_A = basis_A.xform(contact.local_A);
+ Vector3 global_B = basis_B.xform(contact.local_B) + offset_B;
+
+ Vector3 axis = global_A - global_B;
+ min_depth = axis.dot(contact.normal);
+ }
- 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;
+ for (int i = 0; i < contact_count; i++) {
+ const Contact &c = contacts[i];
+ Vector3 global_A = basis_A.xform(c.local_A);
+ Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
Vector3 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
@@ -107,10 +108,8 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind
}
}
- ERR_FAIL_COND(least_deep == -1);
-
- if (least_deep < contact_count) { //replace the last deep contact by the new one
-
+ if (least_deep > -1) {
+ // Replace the least deep contact by the new one.
contacts[least_deep] = contact;
}
@@ -118,29 +117,41 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind
}
contacts[new_index] = contact;
-
- if (new_index == contact_count) {
- contact_count++;
- }
+ contact_count++;
}
void GodotBodyPair3D::validate_contacts() {
- //make sure to erase contacts that are no longer valid
+ // Make sure to erase contacts that are no longer valid.
+ real_t max_separation = space->get_contact_max_separation();
+ real_t max_separation2 = max_separation * max_separation;
+
+ const Basis &basis_A = A->get_transform().basis;
+ const Basis &basis_B = B->get_transform().basis;
- 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;
- real_t depth = axis.dot(c.normal);
+ bool erase = false;
+ if (!c.used) {
+ // Was left behind in previous frame.
+ erase = true;
+ } else {
+ c.used = false;
+
+ Vector3 global_A = basis_A.xform(c.local_A);
+ Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
+ Vector3 axis = global_A - global_B;
+ real_t 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 (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 with the last one.
SWAP(contacts[i], contacts[contact_count - 1]);
}
@@ -260,7 +271,7 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) {
real_t max_penetration = space->get_contact_max_allowed_penetration();
- real_t bias = (real_t)0.3;
+ real_t bias = 0.8;
GodotShape3D *shape_A_ptr = A->get_shape(shape_A);
GodotShape3D *shape_B_ptr = B->get_shape(shape_B);
@@ -353,8 +364,6 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) {
if (collide_B) {
B->apply_impulse(j_vec, c.rB + B->get_center_of_mass());
}
- c.acc_bias_impulse = 0;
- c.acc_bias_impulse_center_of_mass = 0;
c.bounce = combine_bounce(A, B);
if (c.bounce) {
@@ -538,14 +547,10 @@ void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, i
Contact contact;
contact.index_A = p_index_A;
contact.index_B = p_index_B;
- 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;
contact.normal = (p_point_A - p_point_B).normalized();
- contact.mass_normal = 0;
+ contact.used = true;
// Attempt to determine if the contact will be reused.
real_t contact_recycle_radius = space->get_contact_recycle_radius();
@@ -571,20 +576,33 @@ void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, i
void GodotBodySoftBodyPair3D::validate_contacts() {
// Make sure to erase contacts that are no longer valid.
- const Transform3D &transform_A = body->get_transform();
+ real_t max_separation = space->get_contact_max_separation();
+ real_t max_separation2 = max_separation * max_separation;
- real_t contact_max_separation = space->get_contact_max_separation();
+ const Transform3D &transform_A = body->get_transform();
uint32_t contact_count = contacts.size();
for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
Contact &c = contacts[contact_index];
- Vector3 global_A = transform_A.xform(c.local_A);
- Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
- Vector3 axis = global_A - global_B;
- real_t depth = axis.dot(c.normal);
+ bool erase = false;
+ if (!c.used) {
+ // Was left behind in previous frame.
+ erase = true;
+ } else {
+ c.used = false;
+
+ Vector3 global_A = transform_A.xform(c.local_A);
+ Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
+ Vector3 axis = global_A - global_B;
+ real_t depth = axis.dot(c.normal);
+
+ if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
+ erase = true;
+ }
+ }
- if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) {
+ if (erase) {
// Contact no longer needed, remove.
if ((contact_index + 1) < contact_count) {
// Swap with the last one.
@@ -640,7 +658,7 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
real_t max_penetration = space->get_contact_max_allowed_penetration();
- real_t bias = (real_t)0.3;
+ real_t bias = 0.8;
GodotShape3D *shape_A_ptr = body->get_shape(body_shape);
@@ -723,8 +741,6 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
if (soft_body_collides) {
soft_body->apply_node_impulse(c.index_B, j_vec);
}
- c.acc_bias_impulse = 0;
- c.acc_bias_impulse_center_of_mass = 0;
c.bounce = body->get_bounce();