From fffc6aba589a6488bb1e65737330a9b9987ac19b Mon Sep 17 00:00:00 2001 From: Tim Gift Date: Mon, 7 Mar 2022 10:07:46 -0800 Subject: [PATCH] Resolved issues with script PhysicsDirectBodyState3d contacts Resolved a problem with PhysicsDirectBodyState3D sometimes returning incorrect contact positions and added a new get_contact_local_velocity_at_position method to compliment the existing one for the collider. --- doc/classes/PhysicsDirectBodyState3D.xml | 7 ++++++ .../PhysicsDirectBodyState3DExtension.xml | 6 +++++ .../physics_server_3d_extension.cpp | 1 + .../extensions/physics_server_3d_extension.h | 1 + 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 | 1 + servers/physics_3d/godot_body_pair_3d.cpp | 22 +++++++++++-------- servers/physics_server_3d.cpp | 1 + servers/physics_server_3d.h | 1 + 10 files changed, 40 insertions(+), 11 deletions(-) diff --git a/doc/classes/PhysicsDirectBodyState3D.xml b/doc/classes/PhysicsDirectBodyState3D.xml index b257cbc1edb..def9ca7a503 100644 --- a/doc/classes/PhysicsDirectBodyState3D.xml +++ b/doc/classes/PhysicsDirectBodyState3D.xml @@ -179,6 +179,13 @@ Returns the local shape index of the collision. + + + + + Returns the linear velocity vector at the body's contact point. + + diff --git a/doc/classes/PhysicsDirectBodyState3DExtension.xml b/doc/classes/PhysicsDirectBodyState3DExtension.xml index 6b0c50f6603..3220733d234 100644 --- a/doc/classes/PhysicsDirectBodyState3DExtension.xml +++ b/doc/classes/PhysicsDirectBodyState3DExtension.xml @@ -154,6 +154,12 @@ + + + + + + diff --git a/servers/extensions/physics_server_3d_extension.cpp b/servers/extensions/physics_server_3d_extension.cpp index 997daaa9f2a..0937021c641 100644 --- a/servers/extensions/physics_server_3d_extension.cpp +++ b/servers/extensions/physics_server_3d_extension.cpp @@ -102,6 +102,7 @@ void PhysicsDirectBodyState3DExtension::_bind_methods() { GDVIRTUAL_BIND(_get_contact_local_normal, "contact_idx"); GDVIRTUAL_BIND(_get_contact_impulse, "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_3d_extension.h b/servers/extensions/physics_server_3d_extension.h index a9d04b1f057..eb8b6272458 100644 --- a/servers/extensions/physics_server_3d_extension.h +++ b/servers/extensions/physics_server_3d_extension.h @@ -96,6 +96,7 @@ public: EXBIND1RC(Vector3, get_contact_local_normal, int) EXBIND1RC(Vector3, get_contact_impulse, int) EXBIND1RC(int, get_contact_local_shape, int) + EXBIND1RC(Vector3, get_contact_local_velocity_at_position, int) EXBIND1RC(RID, get_contact_collider, int) EXBIND1RC(Vector3, get_contact_collider_position, int) EXBIND1RC(ObjectID, get_contact_collider_id, int) diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h index 51b360d7050..81b668122a6 100644 --- a/servers/physics_3d/godot_body_3d.h +++ b/servers/physics_3d/godot_body_3d.h @@ -119,6 +119,7 @@ class GodotBody3D : public GodotCollisionObject3D { struct Contact { Vector3 local_pos; Vector3 local_normal; + Vector3 local_velocity_at_pos; real_t depth = 0.0; int local_shape = 0; Vector3 collider_pos; @@ -184,7 +185,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, const Vector3 &p_impulse); + _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_local_velocity_at_pos, 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); } @@ -348,7 +349,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, const Vector3 &p_impulse) { +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_local_velocity_at_pos, 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) { @@ -381,6 +382,7 @@ void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &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_3d/godot_body_direct_state_3d.cpp b/servers/physics_3d/godot_body_direct_state_3d.cpp index 9f28f3809a5..0af746c68d9 100644 --- a/servers/physics_3d/godot_body_direct_state_3d.cpp +++ b/servers/physics_3d/godot_body_direct_state_3d.cpp @@ -193,6 +193,11 @@ Vector3 GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) co return body->contacts[p_contact_idx].impulse; } +Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].local_velocity_at_pos; +} + int GodotPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); return body->contacts[p_contact_idx].local_shape; diff --git a/servers/physics_3d/godot_body_direct_state_3d.h b/servers/physics_3d/godot_body_direct_state_3d.h index be2e851b4d2..8066050c9f5 100644 --- a/servers/physics_3d/godot_body_direct_state_3d.h +++ b/servers/physics_3d/godot_body_direct_state_3d.h @@ -91,6 +91,7 @@ public: virtual Vector3 get_contact_local_normal(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 Vector3 get_contact_local_velocity_at_position(int p_contact_idx) const override; virtual RID get_contact_collider(int p_contact_idx) const override; virtual Vector3 get_contact_collider_position(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 292563b6ab5..c467a583ba0 100644 --- a/servers/physics_3d/godot_body_pair_3d.cpp +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -407,14 +407,18 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) { // 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()) { + if (A->can_report_contacts() || 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); + Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity(); + Vector3 wlB = global_B - offset_B; + + if (A->can_report_contacts()) { + A->add_contact(global_A, -c.normal, depth, shape_A, crA, wlB, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse); + } + + if (B->can_report_contacts()) { + B->add_contact(wlB, c.normal, depth, shape_B, crB, global_A, shape_A, A->get_instance_id(), A->get_self(), crA, -c.acc_impulse); + } } if (report_contacts_only) { @@ -797,9 +801,9 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) { 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); + Vector3 crB = soft_body->get_node_velocity(c.index_B); + body->add_contact(global_A, -c.normal, depth, body_shape, crA, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crB, c.acc_impulse); } - if (report_contacts_only) { collided = false; continue; diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp index ee749de1c63..34c364d9603 100644 --- a/servers/physics_server_3d.cpp +++ b/servers/physics_server_3d.cpp @@ -139,6 +139,7 @@ void PhysicsDirectBodyState3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_contact_local_normal", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_local_normal); ClassDB::bind_method(D_METHOD("get_contact_impulse", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_impulse); ClassDB::bind_method(D_METHOD("get_contact_local_shape", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_local_shape); + ClassDB::bind_method(D_METHOD("get_contact_local_velocity_at_position", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_local_velocity_at_position); ClassDB::bind_method(D_METHOD("get_contact_collider", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_collider); ClassDB::bind_method(D_METHOD("get_contact_collider_position", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_collider_position); ClassDB::bind_method(D_METHOD("get_contact_collider_id", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_collider_id); diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h index 0275aee8188..33e9f8a7c9e 100644 --- a/servers/physics_server_3d.h +++ b/servers/physics_server_3d.h @@ -97,6 +97,7 @@ public: virtual Vector3 get_contact_local_normal(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 Vector3 get_contact_local_velocity_at_position(int p_contact_idx) const = 0; virtual RID get_contact_collider(int p_contact_idx) const = 0; virtual Vector3 get_contact_collider_position(int p_contact_idx) const = 0;