From 8ee6264cc914fe1d007faf440fe4fda28b567cc6 Mon Sep 17 00:00:00 2001 From: Lily Garcia Date: Sat, 7 Jan 2023 22:34:28 -0500 Subject: [PATCH] Implement collision impulse in Godot Physics 3D --- doc/classes/PhysicsDirectBodyState3D.xml | 4 +- .../PhysicsDirectBodyState3DExtension.xml | 2 +- .../extensions/physics_server_3d_extension.h | 2 +- servers/physics_3d/godot_body_3d.h | 6 +- .../physics_3d/godot_body_direct_state_3d.cpp | 5 +- .../physics_3d/godot_body_direct_state_3d.h | 2 +- servers/physics_3d/godot_body_pair_3d.cpp | 82 ++++++++++--------- servers/physics_3d/godot_body_pair_3d.h | 1 + servers/physics_server_3d.h | 2 +- 9 files changed, 59 insertions(+), 47 deletions(-) diff --git a/doc/classes/PhysicsDirectBodyState3D.xml b/doc/classes/PhysicsDirectBodyState3D.xml index a8093846429..edd219e66e9 100644 --- a/doc/classes/PhysicsDirectBodyState3D.xml +++ b/doc/classes/PhysicsDirectBodyState3D.xml @@ -152,10 +152,10 @@ - + - Impulse created by the contact. Only implemented for Bullet physics. + Impulse created by the contact. diff --git a/doc/classes/PhysicsDirectBodyState3DExtension.xml b/doc/classes/PhysicsDirectBodyState3DExtension.xml index 4432f89b9dc..35cf4d4dd5c 100644 --- a/doc/classes/PhysicsDirectBodyState3DExtension.xml +++ b/doc/classes/PhysicsDirectBodyState3DExtension.xml @@ -131,7 +131,7 @@ - + diff --git a/servers/extensions/physics_server_3d_extension.h b/servers/extensions/physics_server_3d_extension.h index a85df0683c3..65b90067155 100644 --- a/servers/extensions/physics_server_3d_extension.h +++ b/servers/extensions/physics_server_3d_extension.h @@ -94,7 +94,7 @@ public: EXBIND1RC(Vector3, get_contact_local_position, int) EXBIND1RC(Vector3, get_contact_local_normal, int) - EXBIND1RC(real_t, get_contact_impulse, int) + EXBIND1RC(Vector3, get_contact_impulse, int) EXBIND1RC(int, get_contact_local_shape, int) EXBIND1RC(RID, get_contact_collider, int) EXBIND1RC(Vector3, get_contact_collider_position, int) diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h index 412cbebc7d9..6fb7129c836 100644 --- a/servers/physics_3d/godot_body_3d.h +++ b/servers/physics_3d/godot_body_3d.h @@ -126,6 +126,7 @@ class GodotBody3D : public GodotCollisionObject3D { ObjectID collider_instance_id; RID collider; Vector3 collider_velocity_at_pos; + Vector3 impulse; }; Vector contacts; //no contacts by default @@ -183,7 +184,7 @@ public: _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } - _FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos); + _FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &p_impulse); _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } @@ -347,7 +348,7 @@ public: //add contact inline -void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) { +void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &p_impulse) { int c_max = contacts.size(); if (c_max == 0) { @@ -387,6 +388,7 @@ void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local c[idx].collider_instance_id = p_collider_instance_id; c[idx].collider = p_collider; c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; + c[idx].impulse = p_impulse; } #endif // GODOT_BODY_3D_H diff --git a/servers/physics_3d/godot_body_direct_state_3d.cpp b/servers/physics_3d/godot_body_direct_state_3d.cpp index 25088d33f3d..1a714ca586b 100644 --- a/servers/physics_3d/godot_body_direct_state_3d.cpp +++ b/servers/physics_3d/godot_body_direct_state_3d.cpp @@ -188,8 +188,9 @@ Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_normal(int p_contact_id return body->contacts[p_contact_idx].local_normal; } -real_t GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const { - return 0.0f; // Only implemented for bullet +Vector3 GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].impulse; } int GodotPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const { diff --git a/servers/physics_3d/godot_body_direct_state_3d.h b/servers/physics_3d/godot_body_direct_state_3d.h index 483cfb92986..6ee5171ded5 100644 --- a/servers/physics_3d/godot_body_direct_state_3d.h +++ b/servers/physics_3d/godot_body_direct_state_3d.h @@ -89,7 +89,7 @@ public: virtual Vector3 get_contact_local_position(int p_contact_idx) const override; virtual Vector3 get_contact_local_normal(int p_contact_idx) const override; - virtual real_t get_contact_impulse(int p_contact_idx) const override; + virtual Vector3 get_contact_impulse(int p_contact_idx) const override; virtual int get_contact_local_shape(int p_contact_idx) const override; virtual RID get_contact_collider(int p_contact_idx) const override; diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp index 00b7941292e..31496860c02 100644 --- a/servers/physics_3d/godot_body_pair_3d.cpp +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -364,26 +364,6 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) { c.rA = global_A - A->get_center_of_mass(); c.rB = global_B - B->get_center_of_mass() - offset_B; - // contact query reporting... - - if (A->can_report_contacts()) { - Vector3 crA = 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(), crA); - } - - if (B->can_report_contacts()) { - Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity(); - B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB); - } - - if (report_contacts_only) { - collided = false; - continue; - } - - c.active = true; - do_process = true; - // Precompute normal mass, tangent mass, and bias. Vector3 inertia_A = inv_inertia_tensor_A.xform(c.rA.cross(c.normal)); Vector3 inertia_B = inv_inertia_tensor_B.xform(c.rB.cross(c.normal)); @@ -395,6 +375,29 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) { c.depth = depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; + + c.acc_impulse -= j_vec; + + // contact query reporting... + + if (A->can_report_contacts()) { + Vector3 crA = 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(), crA, c.acc_impulse); + } + + if (B->can_report_contacts()) { + Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity(); + B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB, -c.acc_impulse); + } + + if (report_contacts_only) { + collided = false; + continue; + } + + c.active = true; + do_process = true; + if (collide_A) { A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); } @@ -504,6 +507,7 @@ void GodotBodyPair3D::solve(real_t p_step) { if (collide_B) { B->apply_impulse(j, c.rB + B->get_center_of_mass()); } + c.acc_impulse -= j; c.active = true; } @@ -550,6 +554,7 @@ void GodotBodyPair3D::solve(real_t p_step) { if (collide_B) { B->apply_impulse(jt, c.rB + B->get_center_of_mass()); } + c.acc_impulse -= jt; c.active = true; } @@ -745,23 +750,6 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) { c.rA = global_A - transform_A.origin - body->get_center_of_mass(); c.rB = global_B; - if (body->can_report_contacts()) { - Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity(); - body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA); - } - - if (report_contacts_only) { - collided = false; - continue; - } - - c.active = true; - do_process = true; - - if (body_collides) { - body->set_active(true); - } - // Precompute normal mass, tangent mass, and bias. Vector3 inertia_A = body_inv_inertia_tensor.xform(c.rA.cross(c.normal)); real_t kNormal = body_inv_mass + node_inv_mass; @@ -778,6 +766,24 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) { if (soft_body_collides) { soft_body->apply_node_impulse(c.index_B, j_vec); } + c.acc_impulse -= j_vec; + + if (body->can_report_contacts()) { + Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity(); + body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA, c.acc_impulse); + } + + if (report_contacts_only) { + collided = false; + continue; + } + + c.active = true; + do_process = true; + + if (body_collides) { + body->set_active(true); + } c.bounce = body->get_bounce(); @@ -880,6 +886,7 @@ void GodotBodySoftBodyPair3D::solve(real_t p_step) { if (soft_body_collides) { soft_body->apply_node_impulse(c.index_B, j); } + c.acc_impulse -= j; c.active = true; } @@ -924,6 +931,7 @@ void GodotBodySoftBodyPair3D::solve(real_t p_step) { if (soft_body_collides) { soft_body->apply_node_impulse(c.index_B, jt); } + c.acc_impulse -= jt; c.active = true; } diff --git a/servers/physics_3d/godot_body_pair_3d.h b/servers/physics_3d/godot_body_pair_3d.h index 7a7309f9d5e..fd6c6b35d83 100644 --- a/servers/physics_3d/godot_body_pair_3d.h +++ b/servers/physics_3d/godot_body_pair_3d.h @@ -44,6 +44,7 @@ protected: Vector3 normal; int index_A = 0, index_B = 0; Vector3 local_A, local_B; + Vector3 acc_impulse; // accumulated impulse - only one of the object's impulse is needed as impulse_a == -impulse_b real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn) Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h index ca1ff57c995..2e64057538c 100644 --- a/servers/physics_server_3d.h +++ b/servers/physics_server_3d.h @@ -95,7 +95,7 @@ public: virtual Vector3 get_contact_local_position(int p_contact_idx) const = 0; virtual Vector3 get_contact_local_normal(int p_contact_idx) const = 0; - virtual real_t get_contact_impulse(int p_contact_idx) const = 0; + virtual Vector3 get_contact_impulse(int p_contact_idx) const = 0; virtual int get_contact_local_shape(int p_contact_idx) const = 0; virtual RID get_contact_collider(int p_contact_idx) const = 0;