asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Improve RigidDynamicBody contacts in 2D and 3D

Browse source 
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.
This commit is contained in:
PouleyKetchoupp 2021-12-03 10:38:40 -07:00
parent 80e292b3e0
commit 5cbc7149a1
7 changed files with 157 additions and 116 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

10
servers/physics_2d/godot_body_2d.h
View file

@ -234,9 +234,15 @@ public:
angular_velocity += _inv_inertia * p_torque;
}
_FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) {
_FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2(), real_t p_max_delta_av = -1.0) {
biased_linear_velocity += p_impulse * _inv_mass;
biased_angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse);
if (p_max_delta_av != 0.0) {
real_t delta_av = _inv_inertia * (p_position - center_of_mass).cross(p_impulse);
if (p_max_delta_av > 0 && delta_av > p_max_delta_av) {
delta_av = p_max_delta_av;
}
biased_angular_velocity += delta_av;
}
}
void set_active(bool p_active);

125
servers/physics_2d/godot_body_pair_2d.cpp
View file

@ -34,6 +34,9 @@
#define ACCUMULATE_IMPULSES
#define MIN_VELOCITY 0.001
#define MAX_BIAS_ROTATION (Math_PI / 8)
void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) {
GodotBodyPair2D *self = (GodotBodyPair2D *)p_self;
@ -41,8 +44,6 @@ void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_po
}
void GodotBodyPair2D::_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);
@ -51,46 +52,48 @@ void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Ve
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();
contact.mass_normal = 0; // will be computed in setup()
// attempt to determine if the contact will be reused
contact.used = true;
// 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; i < contact_count; i++) {
Contact &c = contacts[i];
if (
c.local_A.distance_squared_to(local_A) < (recycle_radius_2) &&
if (c.local_A.distance_squared_to(local_A) < (recycle_radius_2) &&
c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) {
contact.acc_normal_impulse = c.acc_normal_impulse;
contact.acc_tangent_impulse = c.acc_tangent_impulse;
contact.acc_bias_impulse = c.acc_bias_impulse;
new_index = i;
break;
contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass;
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
int least_deep = -1;
real_t min_depth = 1e10;
// Remove the contact with the minimum depth.
const Transform2D &transform_A = A->get_transform();
const Transform2D &transform_B = B->get_transform();
for (int i = 0; i <= contact_count; i++) {
Contact &c = (i == contact_count) ? contact : contacts[i];
int least_deep = -1;
real_t min_depth;
// Start with depth for new contact.
{
Vector2 global_A = transform_A.basis_xform(contact.local_A);
Vector2 global_B = transform_B.basis_xform(contact.local_B) + offset_B;
Vector2 axis = global_A - global_B;
min_depth = axis.dot(contact.normal);
}
for (int i = 0; i < contact_count; i++) {
const Contact &c = contacts[i];
Vector2 global_A = transform_A.basis_xform(c.local_A);
Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
@ -103,10 +106,8 @@ void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Ve
}
}
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;
}
@ -114,15 +115,11 @@ void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Ve
}
contacts[new_index] = contact;
if (new_index == contact_count) {
contact_count++;
}
contact_count++;
}
void GodotBodyPair2D::_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;
@ -133,11 +130,11 @@ void GodotBodyPair2D::_validate_contacts() {
Contact &c = contacts[i];
bool erase = false;
if (!c.reused) {
//was left behind in previous frame
if (!c.used) {
// Was left behind in previous frame.
erase = true;
} else {
c.reused = false;
c.used = false;
Vector2 global_A = transform_A.basis_xform(c.local_A);
Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
@ -150,10 +147,10 @@ void GodotBodyPair2D::_validate_contacts() {
}
if (erase) {
// contact no longer needed, remove
// 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]);
}
@ -302,9 +299,6 @@ bool GodotBodyPair2D::setup(real_t p_step) {
bool valid = false;
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
if (!c.reused) {
continue;
}
if (c.normal.dot(direction) > -CMP_EPSILON) { //greater (normal inverted)
continue;
}
@ -323,9 +317,6 @@ bool GodotBodyPair2D::setup(real_t p_step) {
bool valid = false;
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
if (!c.reused) {
continue;
}
if (c.normal.dot(direction) < CMP_EPSILON) { //less (normal ok)
continue;
}
@ -350,7 +341,7 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
real_t max_penetration = space->get_contact_max_allowed_penetration();
real_t bias = 0.3;
real_t bias = 0.8;
GodotShape2D *shape_A_ptr = A->get_shape(shape_A);
GodotShape2D *shape_B_ptr = B->get_shape(shape_B);
@ -389,7 +380,7 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
Vector2 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
if (depth <= 0.0 || !c.reused) {
if (depth <= 0.0) {
continue;
}
@ -400,8 +391,8 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
}
#endif
c.rA = global_A;
c.rB = global_B - offset_B;
c.rA = global_A - A->get_center_of_mass();
c.rB = global_B - B->get_center_of_mass() - offset_B;
if (A->can_report_contacts()) {
Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
@ -434,7 +425,6 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
c.depth = depth;
//c.acc_bias_impulse=0;
#ifdef ACCUMULATE_IMPULSES
{
@ -442,10 +432,10 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
if (collide_A) {
A->apply_impulse(-P, c.rA);
A->apply_impulse(-P, c.rA + A->get_center_of_mass());
}
if (collide_B) {
B->apply_impulse(P, c.rB);
B->apply_impulse(P, c.rB + B->get_center_of_mass());
}
}
#endif
@ -470,6 +460,11 @@ void GodotBodyPair2D::solve(real_t p_step) {
return;
}
const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
for (int i = 0; i < contact_count; ++i) {
Contact &c = contacts[i];
@ -489,6 +484,7 @@ void GodotBodyPair2D::solve(real_t p_step) {
real_t vn = dv.dot(c.normal);
real_t vbn = dbv.dot(c.normal);
Vector2 tangent = c.normal.orthogonal();
real_t vt = dv.dot(tangent);
@ -499,10 +495,31 @@ void GodotBodyPair2D::solve(real_t p_step) {
Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
if (collide_A) {
A->apply_bias_impulse(-jb, c.rA);
A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av);
}
if (collide_B) {
B->apply_bias_impulse(jb, c.rB);
B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av);
}
crbA = Vector2(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x);
crbB = Vector2(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x);
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) / (inv_mass_A + inv_mass_B);
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);
Vector2 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
if (collide_A) {
A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f);
}
if (collide_B) {
B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f);
}
}
real_t jn = -(c.bounce + vn) * c.mass_normal;
@ -519,10 +536,10 @@ void GodotBodyPair2D::solve(real_t p_step) {
Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld);
if (collide_A) {
A->apply_impulse(-j, c.rA);
A->apply_impulse(-j, c.rA + A->get_center_of_mass());
}
if (collide_B) {
B->apply_impulse(j, c.rB);
B->apply_impulse(j, c.rB + B->get_center_of_mass());
}
}
}

3
servers/physics_2d/godot_body_pair_2d.h
View file

@ -62,13 +62,14 @@ class GodotBodyPair2D : public GodotConstraint2D {
real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn)
real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt)
real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb)
real_t acc_bias_impulse_center_of_mass = 0.0; // accumulated normal impulse for position bias applied to com
real_t mass_normal, mass_tangent = 0.0;
real_t bias = 0.0;
real_t depth = 0.0;
bool active = false;
bool used = false;
Vector2 rA, rB;
bool reused = false;
real_t bounce = 0.0;
};

2
servers/physics_2d/godot_physics_server_2d.cpp
View file

@ -1226,7 +1226,7 @@ void GodotPhysicsServer2D::set_collision_iterations(int p_iterations) {
void GodotPhysicsServer2D::init() {
doing_sync = false;
iterations = 8; // 8?
iterations = 16;
stepper = memnew(GodotStep2D);
};

130
servers/physics_3d/godot_body_pair_3d.cpp
View file

@ -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;
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;
// 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++) {
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;
if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) {
// contact no longer needed, remove
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 < -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;
if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) {
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 (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();

1
servers/physics_3d/godot_body_pair_3d.h
View file

@ -54,6 +54,7 @@ protected:
real_t depth = 0.0;
bool active = false;
bool used = false;
Vector3 rA, rB; // Offset in world orientation with respect to center of mass
};

2
servers/physics_3d/godot_physics_server_3d.cpp
View file

@ -1585,7 +1585,7 @@ void GodotPhysicsServer3D::set_collision_iterations(int p_iterations) {
};
void GodotPhysicsServer3D::init() {
iterations = 8; // 8?
iterations = 16;
stepper = memnew(GodotStep3D);
};