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 <argument> to <param> Added get_contact_velocity to the extension bindings PhysicsDirectBodyState3DExtension needed the new get_contact_velocity_at_position binding.
godotengine · Sep 20, 2022 · 3ece13d · 3ece13d
1 parent ae2d9be
commit 3ece13d
Show file tree

Hide file tree

Showing 10 changed files with 39 additions and 9 deletions.
diff --git a/doc/classes/PhysicsDirectBodyState3D.xml b/doc/classes/PhysicsDirectBodyState3D.xml
@@ -177,6 +177,13 @@
  Returns the local shape index of the collision.
  </description>
  </method>
+ <method name="get_contact_velocity_at_position" qualifiers="const">
+ <return type="Vector3" />
+ <param index="0" name="contact_idx" type="int" />
+ <description>
+ Returns the linear velocity vector at the body's contact point.
+ </description>
+ </method>
  <method name="get_space_state">
  <return type="PhysicsDirectSpaceState3D" />
  <description>

diff --git a/doc/classes/PhysicsDirectBodyState3DExtension.xml b/doc/classes/PhysicsDirectBodyState3DExtension.xml
@@ -154,6 +154,12 @@
  <description>
  </description>
  </method>
+ <method name="_get_contact_velocity_at_position" qualifiers="virtual const">
+ <return type="Vector3" />
+ <param index="0" name="contact_idx" type="int" />
+ <description>
+ </description>
+ </method>
  <method name="_get_inverse_inertia" qualifiers="virtual const">
  <return type="Vector3" />
  <description>

diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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;