Fix contacts not being reported properly when using Jolt Physics

modules/jolt_physics/objects/jolt_body_3d.cpp

@@ -161,6 +161,8 @@ void JoltBody3D::_add_to_space() {
 }
 
 void JoltBody3D::_enqueue_call_queries() {
+	// This method will be called from the body activation listener on multiple threads during the simulation step.
+
 	if (space != nullptr) {
 		space->enqueue_call_queries(&call_queries_element);
 	}
@@ -299,6 +301,14 @@ JPH::MassProperties JoltBody3D::_calculate_mass_properties() const {
 	return _calculate_mass_properties(*jolt_shape);
 }
 
+void JoltBody3D::_on_wake_up() {
+	// This method will be called from the body activation listener on multiple threads during the simulation step.
+
+	if (_should_call_queries()) {
+		_enqueue_call_queries();
+	}
+}
+
 void JoltBody3D::_update_mass_properties() {
 	if (in_space()) {
 		jolt_body->GetMotionPropertiesUnchecked()->SetMassProperties(_calculate_allowed_dofs(), _calculate_mass_properties());

modules/jolt_physics/objects/jolt_body_3d.h

@@ -56,6 +56,8 @@ public:
 	};
 
 private:
+	friend class JoltBodyActivationListener3D;
+
 	SelfList<JoltBody3D> call_queries_element;
 
 	LocalVector<RID> exceptions;
@@ -114,7 +116,6 @@ private:
 	void _dequeue_call_queries();
 
 	void _integrate_forces(float p_step, JPH::Body &p_jolt_body);
-
 	void _move_kinematic(float p_step, JPH::Body &p_jolt_body);
 
 	JPH::EAllowedDOFs _calculate_allowed_dofs() const;
@@ -122,6 +123,8 @@ private:
 	JPH::MassProperties _calculate_mass_properties(const JPH::Shape &p_shape) const;
 	JPH::MassProperties _calculate_mass_properties() const;
 
+	void _on_wake_up();
+
 	void _update_mass_properties();
 	void _update_gravity(JPH::Body &p_jolt_body);
 	void _update_damp();

modules/jolt_physics/spaces/jolt_body_activation_listener_3d.cpp

@@ -0,0 +1,41 @@
+/**************************************************************************/
+/*  jolt_body_activation_listener_3d.cpp                                  */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "jolt_body_activation_listener_3d.h"
+
+#include "../objects/jolt_body_3d.h"
+
+void JoltBodyActivationListener3D::OnBodyActivated(const JPH::BodyID &p_body_id, JPH::uint64 p_body_user_data) {
+	// This method will be called on multiple threads during the simulation step.
+
+	if (JoltBody3D *body = reinterpret_cast<JoltObject3D *>(p_body_user_data)->as_body()) {
+		body->_on_wake_up();
+	}
+}

modules/jolt_physics/spaces/jolt_body_activation_listener_3d.h

@@ -0,0 +1,42 @@
+/**************************************************************************/
+/*  jolt_body_activation_listener_3d.h                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/BodyActivationListener.h"
+
+class JoltBodyActivationListener3D final
+		: public JPH::BodyActivationListener {
+public:
+	virtual void OnBodyActivated(const JPH::BodyID &p_body_id, JPH::uint64 p_body_user_data) override;
+	virtual void OnBodyDeactivated(const JPH::BodyID &p_body_id, JPH::uint64 p_body_user_data) override {}
+};

modules/jolt_physics/spaces/jolt_space_3d.cpp

@@ -38,6 +38,7 @@
 #include "../objects/jolt_body_3d.h"
 #include "../shapes/jolt_custom_shape_type.h"
 #include "../shapes/jolt_shape_3d.h"
+#include "jolt_body_activation_listener_3d.h"
 #include "jolt_contact_listener_3d.h"
 #include "jolt_layers.h"
 #include "jolt_physics_direct_space_state_3d.h"
@@ -101,6 +102,7 @@ JoltSpace3D::JoltSpace3D(JPH::JobSystem *p_job_system) :
 		temp_allocator(new JoltTempAllocator()),
 		layers(new JoltLayers()),
 		contact_listener(new JoltContactListener3D(this)),
+		body_activation_listener(new JoltBodyActivationListener3D()),
 		physics_system(new JPH::PhysicsSystem()) {
 	physics_system->Init((JPH::uint)JoltProjectSettings::max_bodies, 0, (JPH::uint)JoltProjectSettings::max_body_pairs, (JPH::uint)JoltProjectSettings::max_contact_constraints, *layers, *layers, *layers);
 
@@ -124,6 +126,7 @@ JoltSpace3D::JoltSpace3D(JPH::JobSystem *p_job_system) :
 	physics_system->SetGravity(JPH::Vec3::sZero());
 	physics_system->SetContactListener(contact_listener);
 	physics_system->SetSoftBodyContactListener(contact_listener);
+	physics_system->SetBodyActivationListener(body_activation_listener);
 
 	physics_system->SetSimCollideBodyVsBody([](const JPH::Body &p_body1, const JPH::Body &p_body2, JPH::Mat44Arg p_transform_com1, JPH::Mat44Arg p_transform_com2, JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) {
 		if (p_body1.IsSensor() || p_body2.IsSensor()) {
@@ -157,6 +160,11 @@ JoltSpace3D::~JoltSpace3D() {
 		physics_system = nullptr;
 	}
 
+	if (body_activation_listener != nullptr) {
+		delete body_activation_listener;
+		body_activation_listener = nullptr;
+	}
+
 	if (contact_listener != nullptr) {
 		delete contact_listener;
 		contact_listener = nullptr;
@@ -488,6 +496,9 @@ void JoltSpace3D::set_is_object_sleeping(const JPH::BodyID &p_jolt_id, bool p_en
 }
 
 void JoltSpace3D::enqueue_call_queries(SelfList<JoltBody3D> *p_body) {
+	// This method will be called from the body activation listener on multiple threads during the simulation step.
+	MutexLock body_call_queries_lock(body_call_queries_mutex);
+
 	if (!p_body->in_list()) {
 		body_call_queries_list.add(p_body);
 	}

modules/jolt_physics/spaces/jolt_space_3d.h

@@ -44,6 +44,7 @@
 
 class JoltArea3D;
 class JoltBody3D;
+class JoltBodyActivationListener3D;
 class JoltContactListener3D;
 class JoltJoint3D;
 class JoltLayers;
@@ -54,6 +55,7 @@ class JoltSoftBody3D;
 
 class JoltSpace3D {
 	Mutex pending_objects_mutex;
+	Mutex body_call_queries_mutex;
 
 	SelfList<JoltBody3D>::List body_call_queries_list;
 	SelfList<JoltArea3D>::List area_call_queries_list;
@@ -69,6 +71,7 @@ class JoltSpace3D {
 	JPH::TempAllocator *temp_allocator = nullptr;
 	JoltLayers *layers = nullptr;
 	JoltContactListener3D *contact_listener = nullptr;
+	JoltBodyActivationListener3D *body_activation_listener = nullptr;
 	JPH::PhysicsSystem *physics_system = nullptr;
 	JoltPhysicsDirectSpaceState3D *direct_state = nullptr;
 	JoltArea3D *default_area = nullptr;