From 3efa10554824d2e3af797061b04e0653cd463292 Mon Sep 17 00:00:00 2001 From: Ricardo Buring Date: Tue, 10 Jan 2023 23:28:02 +0100 Subject: [PATCH] Add get_contact_impulse method to PhysicsDirectBodyState2D This makes it consistent with 3D. --- doc/classes/PhysicsDirectBodyState2D.xml | 7 ++++ .../PhysicsDirectBodyState2DExtension.xml | 6 +++ .../physics_server_2d_extension.cpp | 1 + .../extensions/physics_server_2d_extension.h | 1 + servers/physics_2d/godot_body_2d.h | 6 ++- .../physics_2d/godot_body_direct_state_2d.cpp | 5 +++ .../physics_2d/godot_body_direct_state_2d.h | 2 +- servers/physics_2d/godot_body_pair_2d.cpp | 37 ++++++++++--------- servers/physics_2d/godot_body_pair_2d.h | 1 + servers/physics_server_2d.cpp | 1 + servers/physics_server_2d.h | 1 + 11 files changed, 48 insertions(+), 20 deletions(-) diff --git a/doc/classes/PhysicsDirectBodyState2D.xml b/doc/classes/PhysicsDirectBodyState2D.xml index eca6a1cbc742..a46de4c1898a 100644 --- a/doc/classes/PhysicsDirectBodyState2D.xml +++ b/doc/classes/PhysicsDirectBodyState2D.xml @@ -151,6 +151,13 @@ [b]Note:[/b] By default, this returns 0 unless bodies are configured to monitor contacts. See [member RigidBody2D.contact_monitor]. + + + + + Returns the impulse created by the contact. + + diff --git a/doc/classes/PhysicsDirectBodyState2DExtension.xml b/doc/classes/PhysicsDirectBodyState2DExtension.xml index 8fd34c12435c..496cbf91362b 100644 --- a/doc/classes/PhysicsDirectBodyState2DExtension.xml +++ b/doc/classes/PhysicsDirectBodyState2DExtension.xml @@ -130,6 +130,12 @@ + + + + + + diff --git a/servers/extensions/physics_server_2d_extension.cpp b/servers/extensions/physics_server_2d_extension.cpp index a0c082ee449f..a174ceadac6f 100644 --- a/servers/extensions/physics_server_2d_extension.cpp +++ b/servers/extensions/physics_server_2d_extension.cpp @@ -101,6 +101,7 @@ void PhysicsDirectBodyState2DExtension::_bind_methods() { GDVIRTUAL_BIND(_get_contact_collider_object, "contact_idx"); GDVIRTUAL_BIND(_get_contact_collider_shape, "contact_idx"); GDVIRTUAL_BIND(_get_contact_collider_velocity_at_position, "contact_idx"); + GDVIRTUAL_BIND(_get_contact_impulse, "contact_idx"); GDVIRTUAL_BIND(_get_step); GDVIRTUAL_BIND(_integrate_forces); diff --git a/servers/extensions/physics_server_2d_extension.h b/servers/extensions/physics_server_2d_extension.h index c4970f6398ba..0008653f6631 100644 --- a/servers/extensions/physics_server_2d_extension.h +++ b/servers/extensions/physics_server_2d_extension.h @@ -100,6 +100,7 @@ public: EXBIND1RC(Object *, get_contact_collider_object, int) EXBIND1RC(int, get_contact_collider_shape, int) EXBIND1RC(Vector2, get_contact_collider_velocity_at_position, int) + EXBIND1RC(Vector2, get_contact_impulse, int) EXBIND0RC(real_t, get_step) EXBIND0(integrate_forces) diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h index 34da64dac985..71dc82660484 100644 --- a/servers/physics_2d/godot_body_2d.h +++ b/servers/physics_2d/godot_body_2d.h @@ -132,6 +132,7 @@ class GodotBody2D : public GodotCollisionObject2D { ObjectID collider_instance_id; RID collider; Vector2 collider_velocity_at_pos; + Vector2 impulse; }; Vector contacts; //no contacts by default @@ -190,7 +191,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 Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos); + _FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos, const Vector2 &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); } @@ -340,7 +341,7 @@ public: //add contact inline -void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos) { +void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos, const Vector2 &p_impulse) { int c_max = contacts.size(); if (c_max == 0) { @@ -380,6 +381,7 @@ void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &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_2D_H diff --git a/servers/physics_2d/godot_body_direct_state_2d.cpp b/servers/physics_2d/godot_body_direct_state_2d.cpp index 35092b631af9..2fa933ce170f 100644 --- a/servers/physics_2d/godot_body_direct_state_2d.cpp +++ b/servers/physics_2d/godot_body_direct_state_2d.cpp @@ -210,6 +210,11 @@ Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_velocity_at_position return body->contacts[p_contact_idx].collider_velocity_at_pos; } +Vector2 GodotPhysicsDirectBodyState2D::get_contact_impulse(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].impulse; +} + PhysicsDirectSpaceState2D *GodotPhysicsDirectBodyState2D::get_space_state() { return body->get_space()->get_direct_state(); } diff --git a/servers/physics_2d/godot_body_direct_state_2d.h b/servers/physics_2d/godot_body_direct_state_2d.h index c01ee629209b..545d52ad238a 100644 --- a/servers/physics_2d/godot_body_direct_state_2d.h +++ b/servers/physics_2d/godot_body_direct_state_2d.h @@ -92,8 +92,8 @@ public: virtual Vector2 get_contact_collider_position(int p_contact_idx) const override; virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; virtual int get_contact_collider_shape(int p_contact_idx) const override; - virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override; + virtual Vector2 get_contact_impulse(int p_contact_idx) const override; virtual PhysicsDirectSpaceState2D *get_space_state() override; diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp index 474367def0c9..40dbb4fcf420 100644 --- a/servers/physics_2d/godot_body_pair_2d.cpp +++ b/servers/physics_2d/godot_body_pair_2d.cpp @@ -434,21 +434,6 @@ bool GodotBodyPair2D::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; - if (A->can_report_contacts()) { - Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); - A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity()); - } - - if (B->can_report_contacts()) { - Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); - B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity()); - } - - if (report_contacts_only) { - collided = false; - continue; - } - // Precompute normal mass, tangent mass, and bias. real_t rnA = c.rA.dot(c.normal); real_t rnB = c.rB.dot(c.normal); @@ -466,11 +451,28 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) { c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); c.depth = depth; + Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; + + c.acc_impulse -= P; + + if (A->can_report_contacts()) { + Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); + A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity(), c.acc_impulse); + } + + if (B->can_report_contacts()) { + Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); + B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity(), c.acc_impulse); + } + + if (report_contacts_only) { + collided = false; + continue; + } + #ifdef ACCUMULATE_IMPULSES { // Apply normal + friction impulse - Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; - if (collide_A) { A->apply_impulse(-P, c.rA + A->get_center_of_mass()); } @@ -581,6 +583,7 @@ void GodotBodyPair2D::solve(real_t p_step) { if (collide_B) { B->apply_impulse(j, c.rB + B->get_center_of_mass()); } + c.acc_impulse -= j; } } diff --git a/servers/physics_2d/godot_body_pair_2d.h b/servers/physics_2d/godot_body_pair_2d.h index 7e7a9839c1c1..4e9bfa6022a1 100644 --- a/servers/physics_2d/godot_body_pair_2d.h +++ b/servers/physics_2d/godot_body_pair_2d.h @@ -59,6 +59,7 @@ class GodotBodyPair2D : public GodotConstraint2D { Vector2 position; Vector2 normal; Vector2 local_A, local_B; + Vector2 acc_impulse; // accumulated impulse 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) diff --git a/servers/physics_server_2d.cpp b/servers/physics_server_2d.cpp index 4973b9970a32..214de27b35b8 100644 --- a/servers/physics_server_2d.cpp +++ b/servers/physics_server_2d.cpp @@ -126,6 +126,7 @@ void PhysicsDirectBodyState2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_contact_collider_object", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_collider_object); ClassDB::bind_method(D_METHOD("get_contact_collider_shape", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_collider_shape); ClassDB::bind_method(D_METHOD("get_contact_collider_velocity_at_position", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_collider_velocity_at_position); + ClassDB::bind_method(D_METHOD("get_contact_impulse", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_impulse); ClassDB::bind_method(D_METHOD("get_step"), &PhysicsDirectBodyState2D::get_step); ClassDB::bind_method(D_METHOD("integrate_forces"), &PhysicsDirectBodyState2D::integrate_forces); ClassDB::bind_method(D_METHOD("get_space_state"), &PhysicsDirectBodyState2D::get_space_state); diff --git a/servers/physics_server_2d.h b/servers/physics_server_2d.h index aac4d9d69e3b..836ab5bd7666 100644 --- a/servers/physics_server_2d.h +++ b/servers/physics_server_2d.h @@ -99,6 +99,7 @@ public: virtual Object *get_contact_collider_object(int p_contact_idx) const; virtual int get_contact_collider_shape(int p_contact_idx) const = 0; virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const = 0; + virtual Vector2 get_contact_impulse(int p_contact_idx) const = 0; virtual real_t get_step() const = 0; virtual void integrate_forces();