From 3ece13de9367ae063cb5589aab77719f94bc2cf4 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 Renamed vector3 -> Vector3 Added contact velocity Added the velocity at the point of contact for body A, to complement the already existing velocity for body B. Includes new get_contact_velocity_at_position method for PhysicsDirectBodyState3D Contact collider pos now relative to it's origin The collider's position was still in object_A's space, and needed to be translated. Documentation update Updated documentation to include the new function Formatting corrections Added missing get_contact_velocity_at_position method Added the new get_contact_velocity_at_position method to the PhysicsDirectBodyState3DExtension class. Fixed body pair A can_report_contacts Body pair A was only getting contacts reported if they were enabled for body B. Updated to new schema Changed to Added get_contact_velocity to the extension bindings PhysicsDirectBodyState3DExtension needed the new get_contact_velocity_at_position binding. --- 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 | 19 ++++++++++++------- servers/physics_server_3d.cpp | 1 + servers/physics_server_3d.h | 1 + 10 files changed, 39 insertions(+), 9 deletions(-) diff --git a/doc/classes/PhysicsDirectBodyState3D.xml b/doc/classes/PhysicsDirectBodyState3D.xml index efe63e4093f3..a12561fb2d00 100644 --- a/doc/classes/PhysicsDirectBodyState3D.xml +++ b/doc/classes/PhysicsDirectBodyState3D.xml @@ -177,6 +177,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 ade197eadc5d..3e4b520f19ab 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 800284dc602b..5bea9a35f8d8 100644 --- a/servers/extensions/physics_server_3d_extension.cpp +++ b/servers/extensions/physics_server_3d_extension.cpp @@ -99,6 +99,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_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 b6ed346a3d14..079acde2491c 100644 --- a/servers/extensions/physics_server_3d_extension.h +++ b/servers/extensions/physics_server_3d_extension.h @@ -96,6 +96,7 @@ class PhysicsDirectBodyState3DExtension : public PhysicsDirectBodyState3D { EXBIND1RC(Vector3, get_contact_local_normal, int) EXBIND1RC(real_t, get_contact_impulse, int) EXBIND1RC(int, get_contact_local_shape, int) + EXBIND1RC(Vector3, get_contact_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 2153ca4e915f..75930104f91f 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 velocity_at_pos; real_t depth = 0.0; int local_shape = 0; Vector3 collider_pos; @@ -184,7 +185,7 @@ class GodotBody3D : public GodotCollisionObject3D { _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_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); _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 @@ class GodotBody3D : public GodotCollisionObject3D { //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_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) { 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].velocity_at_pos = p_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 25088d33f3d2..fd41af8be7b4 100644 --- a/servers/physics_3d/godot_body_direct_state_3d.cpp +++ b/servers/physics_3d/godot_body_direct_state_3d.cpp @@ -192,6 +192,11 @@ real_t GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) con return 0.0f; // Only implemented for bullet } +Vector3 GodotPhysicsDirectBodyState3D::get_contact_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].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 483cfb929865..b0d98b31e75e 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 @@ class GodotPhysicsDirectBodyState3D : public PhysicsDirectBodyState3D { 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 int get_contact_local_shape(int p_contact_idx) const override; + virtual Vector3 get_contact_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 eebbe0196d82..0647f70f118d 100644 --- a/servers/physics_3d/godot_body_pair_3d.cpp +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -355,14 +355,18 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) { // contact query reporting... - if (A->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(); 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); - } + Vector3 wlB = global_B - offset_B; - 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 (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); + } + + 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); + } } if (report_contacts_only) { @@ -736,7 +740,8 @@ 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); + 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); } if (report_contacts_only) { diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp index b4f30d764997..babb4faff815 100644 --- a/servers/physics_server_3d.cpp +++ b/servers/physics_server_3d.cpp @@ -138,6 +138,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_velocity_at_position", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_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 6ad5e2681520..4fbf4a90b3d5 100644 --- a/servers/physics_server_3d.h +++ b/servers/physics_server_3d.h @@ -97,6 +97,7 @@ class PhysicsDirectBodyState3D : public Object { 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 int get_contact_local_shape(int p_contact_idx) const = 0; + virtual Vector3 get_contact_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;