From aacb20437a8d796cc58fb7d8a9df4731379cd0c6 Mon Sep 17 00:00:00 2001 From: Ricardo Buring Date: Thu, 13 Apr 2023 21:45:37 +0200 Subject: [PATCH] PhysicsDirectBodyState2D: add get_contact_local_velocity_at_position --- doc/classes/PhysicsDirectBodyState2D.xml | 9 ++++++++- .../PhysicsDirectBodyState2DExtension.xml | 6 ++++++ .../extensions/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 +++++ servers/physics_2d/godot_body_direct_state_2d.h | 1 + servers/physics_2d/godot_body_pair_2d.cpp | 17 +++++++++-------- servers/physics_server_2d.cpp | 1 + servers/physics_server_2d.h | 1 + 10 files changed, 37 insertions(+), 11 deletions(-) diff --git a/doc/classes/PhysicsDirectBodyState2D.xml b/doc/classes/PhysicsDirectBodyState2D.xml index 0716d71bae8..aedd9373b14 100644 --- a/doc/classes/PhysicsDirectBodyState2D.xml +++ b/doc/classes/PhysicsDirectBodyState2D.xml @@ -141,7 +141,7 @@ - Returns the linear velocity vector at the collider's contact point. + Returns the velocity vector at the collider's contact point. @@ -179,6 +179,13 @@ Returns the local shape index of the collision. + + + + + Returns the velocity vector at the body's contact point. + + diff --git a/doc/classes/PhysicsDirectBodyState2DExtension.xml b/doc/classes/PhysicsDirectBodyState2DExtension.xml index e94d59480ba..b6c0adb91de 100644 --- a/doc/classes/PhysicsDirectBodyState2DExtension.xml +++ b/doc/classes/PhysicsDirectBodyState2DExtension.xml @@ -154,6 +154,12 @@ + + + + + + diff --git a/servers/extensions/physics_server_2d_extension.cpp b/servers/extensions/physics_server_2d_extension.cpp index 2dbb59a3231..96f09a8b1cc 100644 --- a/servers/extensions/physics_server_2d_extension.cpp +++ b/servers/extensions/physics_server_2d_extension.cpp @@ -97,6 +97,7 @@ void PhysicsDirectBodyState2DExtension::_bind_methods() { GDVIRTUAL_BIND(_get_contact_local_position, "contact_idx"); GDVIRTUAL_BIND(_get_contact_local_normal, "contact_idx"); GDVIRTUAL_BIND(_get_contact_local_shape, "contact_idx"); + GDVIRTUAL_BIND(_get_contact_local_velocity_at_position, "contact_idx"); GDVIRTUAL_BIND(_get_contact_collider, "contact_idx"); GDVIRTUAL_BIND(_get_contact_collider_position, "contact_idx"); GDVIRTUAL_BIND(_get_contact_collider_id, "contact_idx"); diff --git a/servers/extensions/physics_server_2d_extension.h b/servers/extensions/physics_server_2d_extension.h index 0008653f663..253a58d78e5 100644 --- a/servers/extensions/physics_server_2d_extension.h +++ b/servers/extensions/physics_server_2d_extension.h @@ -93,6 +93,7 @@ public: EXBIND1RC(Vector2, get_contact_local_position, int) EXBIND1RC(Vector2, get_contact_local_normal, int) + EXBIND1RC(Vector2, get_contact_local_velocity_at_position, int) EXBIND1RC(int, get_contact_local_shape, int) EXBIND1RC(RID, get_contact_collider, int) EXBIND1RC(Vector2, get_contact_collider_position, int) diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h index 71dc8266048..529305dbb2f 100644 --- a/servers/physics_2d/godot_body_2d.h +++ b/servers/physics_2d/godot_body_2d.h @@ -125,6 +125,7 @@ class GodotBody2D : public GodotCollisionObject2D { struct Contact { Vector2 local_pos; Vector2 local_normal; + Vector2 local_velocity_at_pos; real_t depth = 0.0; int local_shape = 0; Vector2 collider_pos; @@ -191,7 +192,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, const Vector2 &p_impulse); + _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_local_velocity_at_pos, 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); } @@ -341,7 +342,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, const Vector2 &p_impulse) { +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_local_velocity_at_pos, 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) { @@ -374,6 +375,7 @@ void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local c[idx].local_pos = p_local_pos; c[idx].local_normal = p_local_normal; + c[idx].local_velocity_at_pos = p_local_velocity_at_pos; c[idx].depth = p_depth; c[idx].local_shape = p_local_shape; c[idx].collider_pos = p_collider_pos; diff --git a/servers/physics_2d/godot_body_direct_state_2d.cpp b/servers/physics_2d/godot_body_direct_state_2d.cpp index 2fa933ce170..b34c70831d3 100644 --- a/servers/physics_2d/godot_body_direct_state_2d.cpp +++ b/servers/physics_2d/godot_body_direct_state_2d.cpp @@ -186,6 +186,11 @@ int GodotPhysicsDirectBodyState2D::get_contact_local_shape(int p_contact_idx) co return body->contacts[p_contact_idx].local_shape; } +Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].local_velocity_at_pos; +} + RID GodotPhysicsDirectBodyState2D::get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); return body->contacts[p_contact_idx].collider; diff --git a/servers/physics_2d/godot_body_direct_state_2d.h b/servers/physics_2d/godot_body_direct_state_2d.h index 545d52ad238..90b7c1d369e 100644 --- a/servers/physics_2d/godot_body_direct_state_2d.h +++ b/servers/physics_2d/godot_body_direct_state_2d.h @@ -87,6 +87,7 @@ public: virtual Vector2 get_contact_local_position(int p_contact_idx) const override; virtual Vector2 get_contact_local_normal(int p_contact_idx) const override; virtual int get_contact_local_shape(int p_contact_idx) const override; + virtual Vector2 get_contact_local_velocity_at_position(int p_contact_idx) const override; virtual RID get_contact_collider(int p_contact_idx) const override; virtual Vector2 get_contact_collider_position(int p_contact_idx) const override; diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp index 40dbb4fcf42..fe617ac965e 100644 --- a/servers/physics_2d/godot_body_pair_2d.cpp +++ b/servers/physics_2d/godot_body_pair_2d.cpp @@ -455,14 +455,15 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) { 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 (A->can_report_contacts() || B->can_report_contacts()) { + Vector2 crB = Vector2(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x) + B->get_linear_velocity(); + Vector2 crA = Vector2(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x) + A->get_linear_velocity(); + if (A->can_report_contacts()) { + A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, crA, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse); + } + if (B->can_report_contacts()) { + B->add_contact(global_B + offset_A, c.normal, depth, shape_B, crB, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA, c.acc_impulse); + } } if (report_contacts_only) { diff --git a/servers/physics_server_2d.cpp b/servers/physics_server_2d.cpp index fb5092e4981..edd178761af 100644 --- a/servers/physics_server_2d.cpp +++ b/servers/physics_server_2d.cpp @@ -120,6 +120,7 @@ void PhysicsDirectBodyState2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_contact_local_position", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_local_position); ClassDB::bind_method(D_METHOD("get_contact_local_normal", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_local_normal); ClassDB::bind_method(D_METHOD("get_contact_local_shape", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_local_shape); + ClassDB::bind_method(D_METHOD("get_contact_local_velocity_at_position", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_local_velocity_at_position); ClassDB::bind_method(D_METHOD("get_contact_collider", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_collider); ClassDB::bind_method(D_METHOD("get_contact_collider_position", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_collider_position); ClassDB::bind_method(D_METHOD("get_contact_collider_id", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_collider_id); diff --git a/servers/physics_server_2d.h b/servers/physics_server_2d.h index 56ec94ba9fb..f1f6e843215 100644 --- a/servers/physics_server_2d.h +++ b/servers/physics_server_2d.h @@ -92,6 +92,7 @@ public: virtual Vector2 get_contact_local_position(int p_contact_idx) const = 0; virtual Vector2 get_contact_local_normal(int p_contact_idx) const = 0; virtual int get_contact_local_shape(int p_contact_idx) const = 0; + virtual Vector2 get_contact_local_velocity_at_position(int p_contact_idx) const = 0; virtual RID get_contact_collider(int p_contact_idx) const = 0; virtual Vector2 get_contact_collider_position(int p_contact_idx) const = 0;