initial commit, 4.5 stable

modules/jolt_physics/objects/jolt_area_3d.cpp

@@ -0,0 +1,638 @@
+/**************************************************************************/
+/*  jolt_area_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_area_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_math_funcs.h"
+#include "../misc/jolt_type_conversions.h"
+#include "../shapes/jolt_shape_3d.h"
+#include "../spaces/jolt_broad_phase_layer.h"
+#include "../spaces/jolt_space_3d.h"
+#include "jolt_body_3d.h"
+#include "jolt_group_filter.h"
+#include "jolt_soft_body_3d.h"
+
+namespace {
+
+constexpr double AREA_DEFAULT_WIND_MAGNITUDE = 0.0;
+constexpr double AREA_DEFAULT_WIND_ATTENUATION = 0.0;
+
+const Vector3 AREA_DEFAULT_WIND_SOURCE = Vector3();
+const Vector3 AREA_DEFAULT_WIND_DIRECTION = Vector3();
+
+} // namespace
+
+JPH::BroadPhaseLayer JoltArea3D::_get_broad_phase_layer() const {
+	return monitorable ? JoltBroadPhaseLayer::AREA_DETECTABLE : JoltBroadPhaseLayer::AREA_UNDETECTABLE;
+}
+
+JPH::ObjectLayer JoltArea3D::_get_object_layer() const {
+	ERR_FAIL_NULL_V(space, 0);
+
+	if (jolt_shape == nullptr || jolt_shape->GetType() == JPH::EShapeType::Empty) {
+		// No point doing collision checks against a shapeless object.
+		return space->map_to_object_layer(_get_broad_phase_layer(), 0, 0);
+	}
+
+	return space->map_to_object_layer(_get_broad_phase_layer(), collision_layer, collision_mask);
+}
+
+void JoltArea3D::_add_to_space() {
+	jolt_shape = build_shapes(true);
+
+	JPH::CollisionGroup::GroupID group_id = 0;
+	JPH::CollisionGroup::SubGroupID sub_group_id = 0;
+	JoltGroupFilter::encode_object(this, group_id, sub_group_id);
+
+	jolt_settings->mUserData = reinterpret_cast<JPH::uint64>(this);
+	jolt_settings->mObjectLayer = _get_object_layer();
+	jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id);
+	jolt_settings->mMotionType = _get_motion_type();
+	jolt_settings->mIsSensor = true;
+	jolt_settings->mCollideKinematicVsNonDynamic = true;
+	jolt_settings->mUseManifoldReduction = false;
+	jolt_settings->mOverrideMassProperties = JPH::EOverrideMassProperties::MassAndInertiaProvided;
+	jolt_settings->mMassPropertiesOverride.mMass = 1.0f;
+	jolt_settings->mMassPropertiesOverride.mInertia = JPH::Mat44::sIdentity();
+
+	jolt_settings->SetShape(jolt_shape);
+
+	JPH::Body *new_jolt_body = space->add_object(*this, *jolt_settings, _should_sleep());
+	if (new_jolt_body == nullptr) {
+		return;
+	}
+
+	jolt_body = new_jolt_body;
+
+	delete jolt_settings;
+	jolt_settings = nullptr;
+}
+
+void JoltArea3D::_enqueue_call_queries() {
+	if (space != nullptr) {
+		space->enqueue_call_queries(&call_queries_element);
+	}
+}
+
+void JoltArea3D::_dequeue_call_queries() {
+	if (space != nullptr) {
+		space->dequeue_call_queries(&call_queries_element);
+	}
+}
+
+void JoltArea3D::_add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	const JoltShapedObject3D *other_object = space->try_get_shaped(p_body_id);
+	ERR_FAIL_NULL(other_object);
+
+	p_overlap.rid = other_object->get_rid();
+	p_overlap.instance_id = other_object->get_instance_id();
+
+	HashMap<ShapeIDPair, ShapeIndexPair, ShapeIDPair>::Iterator shape_pair = p_overlap.shape_pairs.find(ShapeIDPair(p_other_shape_id, p_self_shape_id));
+	if (shape_pair == p_overlap.shape_pairs.end()) {
+		const int other_shape_index = other_object->find_shape_index(p_other_shape_id);
+		const int self_shape_index = find_shape_index(p_self_shape_id);
+		shape_pair = p_overlap.shape_pairs.insert(ShapeIDPair(p_other_shape_id, p_self_shape_id), ShapeIndexPair(other_shape_index, self_shape_index));
+	}
+
+	p_overlap.pending_added.push_back(shape_pair->value);
+
+	_events_changed();
+}
+
+bool JoltArea3D::_remove_shape_pair(Overlap &p_overlap, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	HashMap<ShapeIDPair, ShapeIndexPair, ShapeIDPair>::Iterator shape_pair = p_overlap.shape_pairs.find(ShapeIDPair(p_other_shape_id, p_self_shape_id));
+
+	if (shape_pair == p_overlap.shape_pairs.end()) {
+		return false;
+	}
+
+	p_overlap.pending_removed.push_back(shape_pair->value);
+	p_overlap.shape_pairs.remove(shape_pair);
+
+	_events_changed();
+
+	return true;
+}
+
+void JoltArea3D::_flush_events(OverlapsById &p_objects, const Callable &p_callback) {
+	for (OverlapsById::Iterator E = p_objects.begin(); E;) {
+		Overlap &overlap = E->value;
+
+		if (p_callback.is_valid()) {
+			for (const ShapeIndexPair &shape_indices : overlap.pending_added) {
+				int &ref_count = overlap.ref_counts[shape_indices];
+				if (ref_count++ == 0) {
+					_report_event(p_callback, PhysicsServer3D::AREA_BODY_ADDED, overlap.rid, overlap.instance_id, shape_indices.other, shape_indices.self);
+				}
+			}
+
+			for (const ShapeIndexPair &shape_indices : overlap.pending_removed) {
+				int &ref_count = overlap.ref_counts[shape_indices];
+				ERR_CONTINUE(ref_count <= 0);
+				if (--ref_count == 0) {
+					_report_event(p_callback, PhysicsServer3D::AREA_BODY_REMOVED, overlap.rid, overlap.instance_id, shape_indices.other, shape_indices.self);
+					overlap.ref_counts.erase(shape_indices);
+				}
+			}
+		}
+
+		overlap.pending_removed.clear();
+		overlap.pending_added.clear();
+
+		OverlapsById::Iterator next = E;
+		++next;
+
+		if (overlap.shape_pairs.is_empty()) {
+			p_objects.remove(E);
+		}
+
+		E = next;
+	}
+}
+
+void JoltArea3D::_report_event(const Callable &p_callback, PhysicsServer3D::AreaBodyStatus p_status, const RID &p_other_rid, ObjectID p_other_instance_id, int p_other_shape_index, int p_self_shape_index) const {
+	ERR_FAIL_COND(!p_callback.is_valid());
+
+	const Variant arg1 = p_status;
+	const Variant arg2 = p_other_rid;
+	const Variant arg3 = p_other_instance_id;
+	const Variant arg4 = p_other_shape_index;
+	const Variant arg5 = p_self_shape_index;
+	const Variant *args[5] = { &arg1, &arg2, &arg3, &arg4, &arg5 };
+
+	Callable::CallError ce;
+	Variant ret;
+	p_callback.callp(args, 5, ret, ce);
+
+	if (unlikely(ce.error != Callable::CallError::CALL_OK)) {
+		ERR_PRINT_ONCE(vformat("Failed to call area monitor callback for '%s'. It returned the following error: '%s'.", to_string(), Variant::get_callable_error_text(p_callback, args, 5, ce)));
+	}
+}
+
+void JoltArea3D::_notify_body_entered(const JPH::BodyID &p_body_id) {
+	if (JoltBody3D *other_body = space->try_get_body(p_body_id)) {
+		other_body->add_area(this);
+	}
+}
+
+void JoltArea3D::_notify_body_exited(const JPH::BodyID &p_body_id) {
+	if (JoltBody3D *other_body = space->try_get_body(p_body_id)) {
+		other_body->remove_area(this);
+	}
+}
+
+void JoltArea3D::_remove_all_overlaps() {
+	for (KeyValue<JPH::BodyID, Overlap> &E : bodies_by_id) {
+		_notify_body_exited(E.key);
+	}
+
+	bodies_by_id.clear();
+	areas_by_id.clear();
+}
+
+void JoltArea3D::_update_sleeping() {
+	if (!in_space()) {
+		return;
+	}
+
+	space->set_is_object_sleeping(jolt_body->GetID(), _should_sleep());
+}
+
+void JoltArea3D::_update_group_filter() {
+	if (!in_space()) {
+		return;
+	}
+
+	jolt_body->GetCollisionGroup().SetGroupFilter(JoltGroupFilter::instance);
+}
+
+void JoltArea3D::_update_default_gravity() {
+	if (is_default_area()) {
+		space->get_physics_system().SetGravity(to_jolt(gravity_vector) * gravity);
+	}
+}
+
+void JoltArea3D::_space_changing() {
+	JoltShapedObject3D::_space_changing();
+
+	_remove_all_overlaps();
+	_dequeue_call_queries();
+}
+
+void JoltArea3D::_space_changed() {
+	JoltShapedObject3D::_space_changed();
+
+	_update_group_filter();
+	_update_default_gravity();
+}
+
+void JoltArea3D::_events_changed() {
+	_enqueue_call_queries();
+}
+
+void JoltArea3D::_body_monitoring_changed() {
+	_update_sleeping();
+}
+
+void JoltArea3D::_area_monitoring_changed() {
+	_update_sleeping();
+}
+
+void JoltArea3D::_monitorable_changed() {
+	_update_object_layer();
+}
+
+void JoltArea3D::_gravity_changed() {
+	_update_default_gravity();
+}
+
+JoltArea3D::JoltArea3D() :
+		JoltShapedObject3D(OBJECT_TYPE_AREA),
+		call_queries_element(this) {
+}
+
+bool JoltArea3D::is_default_area() const {
+	return space != nullptr && space->get_default_area() == this;
+}
+
+void JoltArea3D::set_default_area(bool p_value) {
+	if (p_value) {
+		_update_default_gravity();
+	}
+}
+
+void JoltArea3D::set_transform(Transform3D p_transform) {
+	JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed to area '%s'.", to_string()));
+
+	Vector3 new_scale;
+	JoltMath::decompose(p_transform, new_scale);
+
+	// Ideally we would do an exact comparison here, but due to floating-point precision this would be invalidated very often.
+	if (!scale.is_equal_approx(new_scale)) {
+		scale = new_scale;
+		_shapes_changed();
+	}
+
+	if (!in_space()) {
+		jolt_settings->mPosition = to_jolt_r(p_transform.origin);
+		jolt_settings->mRotation = to_jolt(p_transform.basis);
+	} else {
+		space->get_body_iface().SetPositionAndRotation(jolt_body->GetID(), to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate);
+	}
+}
+
+Variant JoltArea3D::get_param(PhysicsServer3D::AreaParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: {
+			return get_gravity_mode();
+		}
+		case PhysicsServer3D::AREA_PARAM_GRAVITY: {
+			return get_gravity();
+		}
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: {
+			return get_gravity_vector();
+		}
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: {
+			return is_point_gravity();
+		}
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE: {
+			return get_point_gravity_distance();
+		}
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: {
+			return get_linear_damp_mode();
+		}
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: {
+			return get_linear_damp();
+		}
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: {
+			return get_angular_damp_mode();
+		}
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: {
+			return get_angular_damp();
+		}
+		case PhysicsServer3D::AREA_PARAM_PRIORITY: {
+			return get_priority();
+		}
+		case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: {
+			return AREA_DEFAULT_WIND_MAGNITUDE;
+		}
+		case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: {
+			return AREA_DEFAULT_WIND_SOURCE;
+		}
+		case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: {
+			return AREA_DEFAULT_WIND_DIRECTION;
+		}
+		case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: {
+			return AREA_DEFAULT_WIND_ATTENUATION;
+		}
+		default: {
+			ERR_FAIL_V_MSG(Variant(), vformat("Unhandled area parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltArea3D::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: {
+			set_gravity_mode((OverrideMode)(int)p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY: {
+			set_gravity(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: {
+			set_gravity_vector(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: {
+			set_point_gravity(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE: {
+			set_point_gravity_distance(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: {
+			set_linear_damp_mode((OverrideMode)(int)p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: {
+			set_area_linear_damp(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: {
+			set_angular_damp_mode((OverrideMode)(int)p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: {
+			set_area_angular_damp(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_PRIORITY: {
+			set_priority(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: {
+			if (!Math::is_equal_approx((double)p_value, AREA_DEFAULT_WIND_MAGNITUDE)) {
+				WARN_PRINT(vformat("Invalid wind force magnitude for '%s'. Area wind force magnitude is not supported when using Jolt Physics. Any such value will be ignored.", to_string()));
+			}
+		} break;
+		case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: {
+			if (!((Vector3)p_value).is_equal_approx(AREA_DEFAULT_WIND_SOURCE)) {
+				WARN_PRINT(vformat("Invalid wind source for '%s'. Area wind source is not supported when using Jolt Physics. Any such value will be ignored.", to_string()));
+			}
+		} break;
+		case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: {
+			if (!((Vector3)p_value).is_equal_approx(AREA_DEFAULT_WIND_DIRECTION)) {
+				WARN_PRINT(vformat("Invalid wind direction for '%s'. Area wind direction is not supported when using Jolt Physics. Any such value will be ignored.", to_string()));
+			}
+		} break;
+		case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: {
+			if (!Math::is_equal_approx((double)p_value, AREA_DEFAULT_WIND_ATTENUATION)) {
+				WARN_PRINT(vformat("Invalid wind attenuation for '%s'. Area wind attenuation is not supported when using Jolt Physics. Any such value will be ignored.", to_string()));
+			}
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled area parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+void JoltArea3D::set_body_monitor_callback(const Callable &p_callback) {
+	if (p_callback == body_monitor_callback) {
+		return;
+	}
+
+	body_monitor_callback = p_callback;
+
+	_body_monitoring_changed();
+}
+
+void JoltArea3D::set_area_monitor_callback(const Callable &p_callback) {
+	if (p_callback == area_monitor_callback) {
+		return;
+	}
+
+	area_monitor_callback = p_callback;
+
+	_area_monitoring_changed();
+}
+
+void JoltArea3D::set_monitorable(bool p_monitorable) {
+	if (p_monitorable == monitorable) {
+		return;
+	}
+
+	monitorable = p_monitorable;
+
+	_monitorable_changed();
+}
+
+bool JoltArea3D::can_monitor(const JoltBody3D &p_other) const {
+	return is_monitoring_bodies() && (collision_mask & p_other.get_collision_layer()) != 0;
+}
+
+bool JoltArea3D::can_monitor(const JoltSoftBody3D &p_other) const {
+	return false;
+}
+
+bool JoltArea3D::can_monitor(const JoltArea3D &p_other) const {
+	return is_monitoring_areas() && p_other.is_monitorable() && (collision_mask & p_other.get_collision_layer()) != 0;
+}
+
+bool JoltArea3D::can_interact_with(const JoltBody3D &p_other) const {
+	return can_monitor(p_other);
+}
+
+bool JoltArea3D::can_interact_with(const JoltSoftBody3D &p_other) const {
+	return false;
+}
+
+bool JoltArea3D::can_interact_with(const JoltArea3D &p_other) const {
+	return can_monitor(p_other) || p_other.can_monitor(*this);
+}
+
+Vector3 JoltArea3D::get_velocity_at_position(const Vector3 &p_position) const {
+	return Vector3();
+}
+
+void JoltArea3D::set_point_gravity(bool p_enabled) {
+	if (point_gravity == p_enabled) {
+		return;
+	}
+
+	point_gravity = p_enabled;
+
+	_gravity_changed();
+}
+
+void JoltArea3D::set_gravity(float p_gravity) {
+	if (gravity == p_gravity) {
+		return;
+	}
+
+	gravity = p_gravity;
+
+	_gravity_changed();
+}
+
+void JoltArea3D::set_point_gravity_distance(float p_distance) {
+	if (point_gravity_distance == p_distance) {
+		return;
+	}
+
+	point_gravity_distance = p_distance;
+
+	_gravity_changed();
+}
+
+void JoltArea3D::set_gravity_mode(OverrideMode p_mode) {
+	if (gravity_mode == p_mode) {
+		return;
+	}
+
+	gravity_mode = p_mode;
+
+	_gravity_changed();
+}
+
+void JoltArea3D::set_gravity_vector(const Vector3 &p_vector) {
+	if (gravity_vector == p_vector) {
+		return;
+	}
+
+	gravity_vector = p_vector;
+
+	_gravity_changed();
+}
+
+Vector3 JoltArea3D::compute_gravity(const Vector3 &p_position) const {
+	if (!point_gravity) {
+		return gravity_vector * gravity;
+	}
+
+	const Vector3 point = get_transform_scaled().xform(gravity_vector);
+	const Vector3 to_point = point - p_position;
+	const real_t to_point_dist_sq = MAX(to_point.length_squared(), (real_t)CMP_EPSILON);
+	const Vector3 to_point_dir = to_point / Math::sqrt(to_point_dist_sq);
+
+	if (point_gravity_distance == 0.0f) {
+		return to_point_dir * gravity;
+	}
+
+	const float gravity_dist_sq = point_gravity_distance * point_gravity_distance;
+
+	return to_point_dir * (gravity * gravity_dist_sq / to_point_dist_sq);
+}
+
+void JoltArea3D::body_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	Overlap &overlap = bodies_by_id[p_body_id];
+
+	if (overlap.shape_pairs.is_empty()) {
+		_notify_body_entered(p_body_id);
+	}
+
+	_add_shape_pair(overlap, p_body_id, p_other_shape_id, p_self_shape_id);
+}
+
+bool JoltArea3D::body_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	Overlap *overlap = bodies_by_id.getptr(p_body_id);
+	if (overlap == nullptr) {
+		return false;
+	}
+
+	if (!_remove_shape_pair(*overlap, p_other_shape_id, p_self_shape_id)) {
+		return false;
+	}
+
+	if (overlap->shape_pairs.is_empty()) {
+		_notify_body_exited(p_body_id);
+	}
+
+	return true;
+}
+
+void JoltArea3D::area_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	_add_shape_pair(areas_by_id[p_body_id], p_body_id, p_other_shape_id, p_self_shape_id);
+}
+
+bool JoltArea3D::area_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	Overlap *overlap = areas_by_id.getptr(p_body_id);
+	if (overlap == nullptr) {
+		return false;
+	}
+
+	return _remove_shape_pair(*overlap, p_other_shape_id, p_self_shape_id);
+}
+
+bool JoltArea3D::shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	return body_shape_exited(p_body_id, p_other_shape_id, p_self_shape_id) || area_shape_exited(p_body_id, p_other_shape_id, p_self_shape_id);
+}
+
+void JoltArea3D::body_exited(const JPH::BodyID &p_body_id, bool p_notify) {
+	Overlap *overlap = bodies_by_id.getptr(p_body_id);
+	if (unlikely(overlap == nullptr)) {
+		return;
+	}
+
+	if (unlikely(overlap->shape_pairs.is_empty())) {
+		return;
+	}
+
+	for (const KeyValue<ShapeIDPair, ShapeIndexPair> &E : overlap->shape_pairs) {
+		overlap->pending_added.erase(E.value);
+		overlap->pending_removed.push_back(E.value);
+	}
+
+	_events_changed();
+
+	overlap->shape_pairs.clear();
+
+	if (p_notify) {
+		_notify_body_exited(p_body_id);
+	}
+}
+
+void JoltArea3D::area_exited(const JPH::BodyID &p_body_id) {
+	Overlap *overlap = areas_by_id.getptr(p_body_id);
+	if (unlikely(overlap == nullptr)) {
+		return;
+	}
+
+	if (unlikely(overlap->shape_pairs.is_empty())) {
+		return;
+	}
+
+	for (const KeyValue<ShapeIDPair, ShapeIndexPair> &E : overlap->shape_pairs) {
+		overlap->pending_added.erase(E.value);
+		overlap->pending_removed.push_back(E.value);
+	}
+
+	_events_changed();
+
+	overlap->shape_pairs.clear();
+}
+
+void JoltArea3D::call_queries() {
+	_flush_events(bodies_by_id, body_monitor_callback);
+	_flush_events(areas_by_id, area_monitor_callback);
+}

modules/jolt_physics/objects/jolt_area_3d.h

@@ -0,0 +1,239 @@
+/**************************************************************************/
+/*  jolt_area_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_shaped_object_3d.h"
+
+#include "servers/physics_server_3d.h"
+
+class JoltBody3D;
+class JoltSoftBody3D;
+
+class JoltArea3D final : public JoltShapedObject3D {
+public:
+	typedef PhysicsServer3D::AreaSpaceOverrideMode OverrideMode;
+
+private:
+	struct BodyIDHasher {
+		static uint32_t hash(const JPH::BodyID &p_id) { return hash_fmix32(p_id.GetIndexAndSequenceNumber()); }
+	};
+
+	struct ShapeIDPair {
+		JPH::SubShapeID other;
+		JPH::SubShapeID self;
+
+		ShapeIDPair(JPH::SubShapeID p_other, JPH::SubShapeID p_self) :
+				other(p_other), self(p_self) {}
+
+		static uint32_t hash(const ShapeIDPair &p_pair) {
+			uint32_t hash = hash_murmur3_one_32(p_pair.other.GetValue());
+			hash = hash_murmur3_one_32(p_pair.self.GetValue(), hash);
+			return hash_fmix32(hash);
+		}
+
+		friend bool operator==(const ShapeIDPair &p_lhs, const ShapeIDPair &p_rhs) {
+			return (p_lhs.other == p_rhs.other) && (p_lhs.self == p_rhs.self);
+		}
+	};
+
+	struct ShapeIndexPair {
+		int other = -1;
+		int self = -1;
+
+		ShapeIndexPair() = default;
+
+		ShapeIndexPair(int p_other, int p_self) :
+				other(p_other), self(p_self) {}
+
+		static uint32_t hash(const ShapeIndexPair &p_pair) {
+			uint32_t hash = hash_murmur3_one_32(p_pair.other);
+			hash = hash_murmur3_one_32(p_pair.self, hash);
+			return hash_fmix32(hash);
+		}
+
+		friend bool operator==(const ShapeIndexPair &p_lhs, const ShapeIndexPair &p_rhs) {
+			return (p_lhs.other == p_rhs.other) && (p_lhs.self == p_rhs.self);
+		}
+	};
+
+	struct Overlap {
+		HashMap<ShapeIDPair, ShapeIndexPair, ShapeIDPair> shape_pairs;
+		HashMap<ShapeIndexPair, int, ShapeIndexPair> ref_counts;
+		LocalVector<ShapeIndexPair> pending_added;
+		LocalVector<ShapeIndexPair> pending_removed;
+		RID rid;
+		ObjectID instance_id;
+	};
+
+	typedef HashMap<JPH::BodyID, Overlap, BodyIDHasher> OverlapsById;
+
+	SelfList<JoltArea3D> call_queries_element;
+
+	OverlapsById bodies_by_id;
+	OverlapsById areas_by_id;
+
+	Vector3 gravity_vector = Vector3(0, -1, 0);
+
+	Callable body_monitor_callback;
+	Callable area_monitor_callback;
+
+	float priority = 0.0f;
+	float gravity = 9.8f;
+	float point_gravity_distance = 0.0f;
+	float linear_damp = 0.1f;
+	float angular_damp = 0.1f;
+
+	OverrideMode gravity_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+	OverrideMode linear_damp_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+	OverrideMode angular_damp_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+
+	bool monitorable = false;
+	bool point_gravity = false;
+
+	virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const override;
+	virtual JPH::ObjectLayer _get_object_layer() const override;
+
+	virtual JPH::EMotionType _get_motion_type() const override { return JPH::EMotionType::Kinematic; }
+
+	bool _should_sleep() const { return !is_monitoring(); }
+
+	virtual void _add_to_space() override;
+
+	void _enqueue_call_queries();
+	void _dequeue_call_queries();
+
+	void _add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+	bool _remove_shape_pair(Overlap &p_overlap, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+
+	void _flush_events(OverlapsById &p_objects, const Callable &p_callback);
+
+	void _report_event(const Callable &p_callback, PhysicsServer3D::AreaBodyStatus p_status, const RID &p_other_rid, ObjectID p_other_instance_id, int p_other_shape_index, int p_self_shape_index) const;
+
+	void _notify_body_entered(const JPH::BodyID &p_body_id);
+	void _notify_body_exited(const JPH::BodyID &p_body_id);
+
+	void _remove_all_overlaps();
+
+	void _update_sleeping();
+	void _update_group_filter();
+	void _update_default_gravity();
+
+	virtual void _space_changing() override;
+	virtual void _space_changed() override;
+	void _events_changed();
+	void _body_monitoring_changed();
+	void _area_monitoring_changed();
+	void _monitorable_changed();
+	void _gravity_changed();
+
+public:
+	JoltArea3D();
+
+	bool is_default_area() const;
+	void set_default_area(bool p_value);
+
+	void set_transform(Transform3D p_transform);
+
+	Variant get_param(PhysicsServer3D::AreaParameter p_param) const;
+	void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value);
+
+	bool has_body_monitor_callback() const { return body_monitor_callback.is_valid(); }
+	void set_body_monitor_callback(const Callable &p_callback);
+
+	bool has_area_monitor_callback() const { return area_monitor_callback.is_valid(); }
+	void set_area_monitor_callback(const Callable &p_callback);
+
+	bool is_monitoring_bodies() const { return has_body_monitor_callback(); }
+	bool is_monitoring_areas() const { return has_area_monitor_callback(); }
+	bool is_monitoring() const { return is_monitoring_bodies() || is_monitoring_areas(); }
+
+	bool is_monitorable() const { return monitorable; }
+	void set_monitorable(bool p_monitorable);
+
+	bool can_monitor(const JoltBody3D &p_other) const;
+	bool can_monitor(const JoltSoftBody3D &p_other) const;
+	bool can_monitor(const JoltArea3D &p_other) const;
+
+	virtual bool can_interact_with(const JoltBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltSoftBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltArea3D &p_other) const override;
+
+	virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const override;
+
+	virtual bool reports_contacts() const override { return false; }
+
+	bool is_point_gravity() const { return point_gravity; }
+	void set_point_gravity(bool p_enabled);
+
+	float get_priority() const { return priority; }
+	void set_priority(float p_priority) { priority = p_priority; }
+
+	float get_gravity() const { return gravity; }
+	void set_gravity(float p_gravity);
+
+	float get_point_gravity_distance() const { return point_gravity_distance; }
+	void set_point_gravity_distance(float p_distance);
+
+	float get_linear_damp() const { return linear_damp; }
+	void set_area_linear_damp(float p_damp) { linear_damp = p_damp; }
+
+	float get_angular_damp() const { return angular_damp; }
+	void set_area_angular_damp(float p_damp) { angular_damp = p_damp; }
+
+	OverrideMode get_gravity_mode() const { return gravity_mode; }
+	void set_gravity_mode(OverrideMode p_mode);
+
+	OverrideMode get_linear_damp_mode() const { return linear_damp_mode; }
+	void set_linear_damp_mode(OverrideMode p_mode) { linear_damp_mode = p_mode; }
+
+	OverrideMode get_angular_damp_mode() const { return angular_damp_mode; }
+	void set_angular_damp_mode(OverrideMode p_mode) { angular_damp_mode = p_mode; }
+
+	Vector3 get_gravity_vector() const { return gravity_vector; }
+	void set_gravity_vector(const Vector3 &p_vector);
+
+	Vector3 compute_gravity(const Vector3 &p_position) const;
+
+	void body_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+	bool body_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+
+	void area_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+	bool area_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+
+	bool shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+	void body_exited(const JPH::BodyID &p_body_id, bool p_notify = true);
+	void area_exited(const JPH::BodyID &p_body_id);
+
+	void call_queries();
+
+	virtual bool has_custom_center_of_mass() const override { return false; }
+	virtual Vector3 get_center_of_mass_custom() const override { return Vector3(); }
+};

modules/jolt_physics/objects/jolt_body_3d.h

@@ -0,0 +1,309 @@
+/**************************************************************************/
+/*  jolt_body_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_physics_direct_body_state_3d.h"
+#include "jolt_shaped_object_3d.h"
+
+class JoltArea3D;
+class JoltJoint3D;
+class JoltSoftBody3D;
+
+class JoltBody3D final : public JoltShapedObject3D {
+public:
+	typedef PhysicsServer3D::BodyDampMode DampMode;
+
+	struct Contact {
+		Vector3 normal;
+		Vector3 position;
+		Vector3 collider_position;
+		Vector3 velocity;
+		Vector3 collider_velocity;
+		Vector3 impulse;
+		ObjectID collider_id;
+		RID collider_rid;
+		float depth = 0.0f;
+		int shape_index = 0;
+		int collider_shape_index = 0;
+	};
+
+private:
+	friend class JoltBodyActivationListener3D;
+
+	SelfList<JoltBody3D> call_queries_element;
+
+	LocalVector<RID> exceptions;
+	LocalVector<Contact> contacts;
+	LocalVector<JoltArea3D *> areas;
+	LocalVector<JoltJoint3D *> joints;
+
+	Variant custom_integration_userdata;
+
+	Transform3D kinematic_transform;
+
+	Vector3 inertia;
+	Vector3 center_of_mass_custom;
+	Vector3 constant_force;
+	Vector3 constant_torque;
+	Vector3 linear_surface_velocity;
+	Vector3 angular_surface_velocity;
+	Vector3 gravity;
+
+	Callable state_sync_callback;
+	Callable custom_integration_callback;
+
+	JoltPhysicsDirectBodyState3D *direct_state = nullptr;
+
+	PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_RIGID;
+
+	DampMode linear_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE;
+	DampMode angular_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE;
+
+	float mass = 1.0f;
+	float linear_damp = 0.0f;
+	float angular_damp = 0.0f;
+	float total_linear_damp = 0.0f;
+	float total_angular_damp = 0.0f;
+	float gravity_scale = 1.0f;
+	float collision_priority = 1.0f;
+
+	int contact_count = 0;
+
+	uint32_t locked_axes = 0;
+
+	bool sleep_allowed = true;
+	bool sleep_initially = false;
+	bool custom_center_of_mass = false;
+	bool custom_integrator = false;
+
+	virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const override;
+	virtual JPH::ObjectLayer _get_object_layer() const override;
+
+	virtual JPH::EMotionType _get_motion_type() const override;
+
+	virtual void _add_to_space() override;
+
+	bool _should_call_queries() const { return state_sync_callback.is_valid() || custom_integration_callback.is_valid(); }
+	void _enqueue_call_queries();
+	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;
+
+	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();
+	void _update_kinematic_transform();
+	void _update_group_filter();
+	void _update_joint_constraints();
+	void _update_possible_kinematic_contacts();
+	void _update_sleep_allowed();
+
+	void _destroy_joint_constraints();
+
+	void _exit_all_areas();
+
+	void _mode_changed();
+	virtual void _shapes_committed() override;
+	virtual void _space_changing() override;
+	virtual void _space_changed() override;
+	void _areas_changed();
+	void _joints_changed();
+	void _transform_changed();
+	void _motion_changed();
+	void _exceptions_changed();
+	void _axis_lock_changed();
+	void _contact_reporting_changed();
+	void _sleep_allowed_changed();
+
+public:
+	JoltBody3D();
+	virtual ~JoltBody3D() override;
+
+	void set_transform(Transform3D p_transform);
+
+	Variant get_state(PhysicsServer3D::BodyState p_state) const;
+	void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value);
+
+	Variant get_param(PhysicsServer3D::BodyParameter p_param) const;
+	void set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value);
+
+	bool has_state_sync_callback() const { return state_sync_callback.is_valid(); }
+	void set_state_sync_callback(const Callable &p_callback) { state_sync_callback = p_callback; }
+
+	bool has_custom_integration_callback() const { return custom_integration_callback.is_valid(); }
+	void set_custom_integration_callback(const Callable &p_callback, const Variant &p_userdata) {
+		custom_integration_callback = p_callback;
+		custom_integration_userdata = p_userdata;
+	}
+
+	bool has_custom_integrator() const { return custom_integrator; }
+	void set_custom_integrator(bool p_enabled);
+
+	bool is_sleeping() const;
+	void set_is_sleeping(bool p_enabled);
+
+	void put_to_sleep() { set_is_sleeping(true); }
+	void wake_up() { set_is_sleeping(false); }
+
+	bool is_sleep_allowed() const { return sleep_allowed; }
+	bool is_sleep_actually_allowed() const;
+	void set_is_sleep_allowed(bool p_enabled);
+
+	Basis get_principal_inertia_axes() const;
+	Vector3 get_inverse_inertia() const;
+	Basis get_inverse_inertia_tensor() const;
+
+	void set_linear_velocity(const Vector3 &p_velocity);
+	void set_angular_velocity(const Vector3 &p_velocity);
+	void set_axis_velocity(const Vector3 &p_axis_velocity);
+
+	virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const override;
+
+	virtual bool has_custom_center_of_mass() const override { return custom_center_of_mass; }
+	virtual Vector3 get_center_of_mass_custom() const override { return center_of_mass_custom; }
+	void set_center_of_mass_custom(const Vector3 &p_center_of_mass);
+
+	int get_max_contacts_reported() const { return contacts.size(); }
+	void set_max_contacts_reported(int p_count);
+
+	int get_contact_count() const { return contact_count; }
+	const Contact &get_contact(int p_index) { return contacts[p_index]; }
+	virtual bool reports_contacts() const override { return !contacts.is_empty(); }
+
+	bool reports_all_kinematic_contacts() const;
+
+	void add_contact(const JoltBody3D *p_collider, float p_depth, int p_shape_index, int p_collider_shape_index, const Vector3 &p_normal, const Vector3 &p_position, const Vector3 &p_collider_position, const Vector3 &p_velocity, const Vector3 &p_collider_velocity, const Vector3 &p_impulse);
+
+	void reset_mass_properties();
+
+	void apply_force(const Vector3 &p_force, const Vector3 &p_position);
+	void apply_central_force(const Vector3 &p_force);
+	void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position);
+
+	void apply_central_impulse(const Vector3 &p_impulse);
+	void apply_torque(const Vector3 &p_torque);
+	void apply_torque_impulse(const Vector3 &p_impulse);
+
+	void add_constant_central_force(const Vector3 &p_force);
+	void add_constant_force(const Vector3 &p_force, const Vector3 &p_position);
+	void add_constant_torque(const Vector3 &p_torque);
+
+	Vector3 get_constant_force() const;
+	void set_constant_force(const Vector3 &p_force);
+
+	Vector3 get_constant_torque() const;
+	void set_constant_torque(const Vector3 &p_torque);
+
+	Vector3 get_linear_surface_velocity() const { return linear_surface_velocity; }
+	Vector3 get_angular_surface_velocity() const { return angular_surface_velocity; }
+
+	void add_collision_exception(const RID &p_excepted_body);
+	void remove_collision_exception(const RID &p_excepted_body);
+	bool has_collision_exception(const RID &p_excepted_body) const;
+
+	const LocalVector<RID> &get_collision_exceptions() const { return exceptions; }
+
+	void add_area(JoltArea3D *p_area);
+	void remove_area(JoltArea3D *p_area);
+
+	void add_joint(JoltJoint3D *p_joint);
+	void remove_joint(JoltJoint3D *p_joint);
+
+	void call_queries();
+
+	virtual void pre_step(float p_step, JPH::Body &p_jolt_body) override;
+
+	JoltPhysicsDirectBodyState3D *get_direct_state();
+
+	PhysicsServer3D::BodyMode get_mode() const { return mode; }
+
+	void set_mode(PhysicsServer3D::BodyMode p_mode);
+
+	bool is_static() const { return mode == PhysicsServer3D::BODY_MODE_STATIC; }
+	bool is_kinematic() const { return mode == PhysicsServer3D::BODY_MODE_KINEMATIC; }
+	bool is_rigid_free() const { return mode == PhysicsServer3D::BODY_MODE_RIGID; }
+	bool is_rigid_linear() const { return mode == PhysicsServer3D::BODY_MODE_RIGID_LINEAR; }
+	bool is_rigid() const { return is_rigid_free() || is_rigid_linear(); }
+
+	bool is_ccd_enabled() const;
+	void set_ccd_enabled(bool p_enabled);
+
+	float get_mass() const { return mass; }
+	void set_mass(float p_mass);
+
+	Vector3 get_inertia() const { return inertia; }
+	void set_inertia(const Vector3 &p_inertia);
+
+	float get_bounce() const;
+	void set_bounce(float p_bounce);
+
+	float get_friction() const;
+	void set_friction(float p_friction);
+
+	float get_gravity_scale() const { return gravity_scale; }
+	void set_gravity_scale(float p_scale);
+
+	Vector3 get_gravity() const { return gravity; }
+
+	float get_linear_damp() const { return linear_damp; }
+	void set_linear_damp(float p_damp);
+
+	float get_angular_damp() const { return angular_damp; }
+	void set_angular_damp(float p_damp);
+
+	float get_total_linear_damp() const { return total_linear_damp; }
+	float get_total_angular_damp() const { return total_angular_damp; }
+
+	float get_collision_priority() const { return collision_priority; }
+	void set_collision_priority(float p_priority) { collision_priority = p_priority; }
+
+	DampMode get_linear_damp_mode() const { return linear_damp_mode; }
+	void set_linear_damp_mode(DampMode p_mode);
+
+	DampMode get_angular_damp_mode() const { return angular_damp_mode; }
+	void set_angular_damp_mode(DampMode p_mode);
+
+	bool is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const;
+	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_enabled);
+	bool are_axes_locked() const { return locked_axes != 0; }
+
+	virtual bool can_interact_with(const JoltBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltSoftBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltArea3D &p_other) const override;
+};

modules/jolt_physics/objects/jolt_group_filter.cpp

@@ -0,0 +1,60 @@
+/**************************************************************************/
+/*  jolt_group_filter.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_group_filter.h"
+
+#include "jolt_area_3d.h"
+#include "jolt_body_3d.h"
+#include "jolt_object_3d.h"
+
+bool JoltGroupFilter::CanCollide(const JPH::CollisionGroup &p_group1, const JPH::CollisionGroup &p_group2) const {
+	const JoltObject3D *object1 = decode_object(p_group1.GetGroupID(), p_group1.GetSubGroupID());
+	const JoltObject3D *object2 = decode_object(p_group2.GetGroupID(), p_group2.GetSubGroupID());
+	return object1->can_interact_with(*object2);
+}
+
+void JoltGroupFilter::encode_object(const JoltObject3D *p_object, JPH::CollisionGroup::GroupID &r_group_id, JPH::CollisionGroup::SubGroupID &r_sub_group_id) {
+	// Since group filters don't grant us access to the bodies or their user data we are instead forced use the
+	// collision group to carry the upper and lower bits of our pointer, which we can access and decode in `CanCollide`.
+	const uint64_t address = reinterpret_cast<uint64_t>(p_object);
+	r_group_id = JPH::CollisionGroup::GroupID(address >> 32U);
+	r_sub_group_id = JPH::CollisionGroup::SubGroupID(address & 0xFFFFFFFFULL);
+}
+
+const JoltObject3D *JoltGroupFilter::decode_object(JPH::CollisionGroup::GroupID p_group_id, JPH::CollisionGroup::SubGroupID p_sub_group_id) {
+	const uint64_t upper_bits = (uint64_t)p_group_id << 32U;
+	const uint64_t lower_bits = (uint64_t)p_sub_group_id;
+	const uint64_t address = uint64_t(upper_bits | lower_bits);
+	return reinterpret_cast<const JoltObject3D *>(address);
+}
+
+static_assert(sizeof(JoltObject3D *) <= 8, "Pointer size greater than expected.");
+static_assert(sizeof(JPH::CollisionGroup::GroupID) == 4, "Size of Jolt's collision group ID has changed.");
+static_assert(sizeof(JPH::CollisionGroup::SubGroupID) == 4, "Size of Jolt's collision sub-group ID has changed.");

modules/jolt_physics/objects/jolt_group_filter.h

@@ -0,0 +1,48 @@
+/**************************************************************************/
+/*  jolt_group_filter.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/Collision/CollisionGroup.h"
+#include "Jolt/Physics/Collision/GroupFilter.h"
+
+class JoltObject3D;
+
+class JoltGroupFilter final : public JPH::GroupFilter {
+	virtual bool CanCollide(const JPH::CollisionGroup &p_group1, const JPH::CollisionGroup &p_group2) const override;
+
+public:
+	inline static JoltGroupFilter *instance = nullptr;
+
+	static void encode_object(const JoltObject3D *p_object, JPH::CollisionGroup::GroupID &r_group_id, JPH::CollisionGroup::SubGroupID &r_sub_group_id);
+	static const JoltObject3D *decode_object(JPH::CollisionGroup::GroupID p_group_id, JPH::CollisionGroup::SubGroupID p_sub_group_id);
+};

modules/jolt_physics/objects/jolt_object_3d.cpp

@@ -0,0 +1,148 @@
+/**************************************************************************/
+/*  jolt_object_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_object_3d.h"
+
+#include "../jolt_physics_server_3d.h"
+#include "../jolt_project_settings.h"
+#include "../spaces/jolt_layers.h"
+#include "../spaces/jolt_space_3d.h"
+#include "jolt_group_filter.h"
+
+void JoltObject3D::_remove_from_space() {
+	if (!in_space()) {
+		return;
+	}
+
+	space->remove_object(jolt_body->GetID());
+	jolt_body = nullptr;
+}
+
+void JoltObject3D::_reset_space() {
+	ERR_FAIL_NULL(space);
+
+	_space_changing();
+	_remove_from_space();
+	_add_to_space();
+	_space_changed();
+}
+
+void JoltObject3D::_update_object_layer() {
+	if (!in_space()) {
+		return;
+	}
+
+	space->get_body_iface().SetObjectLayer(jolt_body->GetID(), _get_object_layer());
+}
+
+void JoltObject3D::_collision_layer_changed() {
+	_update_object_layer();
+}
+
+void JoltObject3D::_collision_mask_changed() {
+	_update_object_layer();
+}
+
+JoltObject3D::JoltObject3D(ObjectType p_object_type) :
+		object_type(p_object_type) {
+}
+
+JoltObject3D::~JoltObject3D() = default;
+
+Object *JoltObject3D::get_instance() const {
+	return ObjectDB::get_instance(instance_id);
+}
+
+void JoltObject3D::set_space(JoltSpace3D *p_space) {
+	if (space == p_space) {
+		return;
+	}
+
+	_space_changing();
+
+	if (space != nullptr) {
+		_remove_from_space();
+	}
+
+	space = p_space;
+
+	if (space != nullptr) {
+		_add_to_space();
+	}
+
+	_space_changed();
+}
+
+void JoltObject3D::set_collision_layer(uint32_t p_layer) {
+	if (p_layer == collision_layer) {
+		return;
+	}
+
+	collision_layer = p_layer;
+
+	_collision_layer_changed();
+}
+
+void JoltObject3D::set_collision_mask(uint32_t p_mask) {
+	if (p_mask == collision_mask) {
+		return;
+	}
+
+	collision_mask = p_mask;
+
+	_collision_mask_changed();
+}
+
+bool JoltObject3D::can_collide_with(const JoltObject3D &p_other) const {
+	return (collision_mask & p_other.get_collision_layer()) != 0;
+}
+
+bool JoltObject3D::can_interact_with(const JoltObject3D &p_other) const {
+	if (const JoltBody3D *other_body = p_other.as_body()) {
+		return can_interact_with(*other_body);
+	} else if (const JoltArea3D *other_area = p_other.as_area()) {
+		return can_interact_with(*other_area);
+	} else if (const JoltSoftBody3D *other_soft_body = p_other.as_soft_body()) {
+		return can_interact_with(*other_soft_body);
+	} else {
+		ERR_FAIL_V_MSG(false, vformat("Unhandled object type: '%d'. This should not happen. Please report this.", p_other.get_type()));
+	}
+}
+
+String JoltObject3D::to_string() const {
+	static const String fallback_name = "<unknown>";
+
+	if (JoltPhysicsServer3D::get_singleton()->is_on_separate_thread()) {
+		return fallback_name; // Calling `Object::to_string` is not thread-safe.
+	}
+
+	Object *instance = get_instance();
+	return instance != nullptr ? instance->to_string() : fallback_name;
+}

modules/jolt_physics/objects/jolt_object_3d.h

@@ -0,0 +1,154 @@
+/**************************************************************************/
+/*  jolt_object_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 "../shapes/jolt_shape_instance_3d.h"
+
+#include "core/math/vector3.h"
+#include "core/object/object.h"
+#include "core/string/ustring.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/rid.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+#include "Jolt/Physics/Collision/ObjectLayer.h"
+
+class JoltArea3D;
+class JoltBody3D;
+class JoltShapedObject3D;
+class JoltShape3D;
+class JoltSoftBody3D;
+class JoltSpace3D;
+
+class JoltObject3D {
+public:
+	enum ObjectType : char {
+		OBJECT_TYPE_INVALID,
+		OBJECT_TYPE_BODY,
+		OBJECT_TYPE_SOFT_BODY,
+		OBJECT_TYPE_AREA,
+	};
+
+protected:
+	LocalVector<JoltShapeInstance3D> shapes;
+
+	RID rid;
+	ObjectID instance_id;
+	JoltSpace3D *space = nullptr;
+	JPH::Body *jolt_body = nullptr;
+
+	uint32_t collision_layer = 1;
+	uint32_t collision_mask = 1;
+
+	ObjectType object_type = OBJECT_TYPE_INVALID;
+
+	bool pickable = false;
+
+	virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const = 0;
+	virtual JPH::ObjectLayer _get_object_layer() const = 0;
+
+	virtual void _add_to_space() = 0;
+	virtual void _remove_from_space();
+
+	void _reset_space();
+
+	void _update_object_layer();
+
+	virtual void _collision_layer_changed();
+	virtual void _collision_mask_changed();
+
+	virtual void _space_changing() {}
+	virtual void _space_changed() {}
+
+public:
+	explicit JoltObject3D(ObjectType p_object_type);
+	virtual ~JoltObject3D() = 0;
+
+	ObjectType get_type() const { return object_type; }
+
+	bool is_body() const { return object_type == OBJECT_TYPE_BODY; }
+	bool is_soft_body() const { return object_type == OBJECT_TYPE_SOFT_BODY; }
+	bool is_area() const { return object_type == OBJECT_TYPE_AREA; }
+	bool is_shaped() const { return object_type != OBJECT_TYPE_SOFT_BODY; }
+
+	JoltShapedObject3D *as_shaped() { return is_shaped() ? reinterpret_cast<JoltShapedObject3D *>(this) : nullptr; }
+	const JoltShapedObject3D *as_shaped() const { return is_shaped() ? reinterpret_cast<const JoltShapedObject3D *>(this) : nullptr; }
+
+	JoltBody3D *as_body() { return is_body() ? reinterpret_cast<JoltBody3D *>(this) : nullptr; }
+	const JoltBody3D *as_body() const { return is_body() ? reinterpret_cast<const JoltBody3D *>(this) : nullptr; }
+
+	JoltSoftBody3D *as_soft_body() { return is_soft_body() ? reinterpret_cast<JoltSoftBody3D *>(this) : nullptr; }
+	const JoltSoftBody3D *as_soft_body() const { return is_soft_body() ? reinterpret_cast<const JoltSoftBody3D *>(this) : nullptr; }
+
+	JoltArea3D *as_area() { return is_area() ? reinterpret_cast<JoltArea3D *>(this) : nullptr; }
+	const JoltArea3D *as_area() const { return is_area() ? reinterpret_cast<const JoltArea3D *>(this) : nullptr; }
+
+	RID get_rid() const { return rid; }
+	void set_rid(const RID &p_rid) { rid = p_rid; }
+
+	ObjectID get_instance_id() const { return instance_id; }
+	void set_instance_id(ObjectID p_id) { instance_id = p_id; }
+	Object *get_instance() const;
+
+	JPH::Body *get_jolt_body() const { return jolt_body; }
+	JPH::BodyID get_jolt_id() const { return jolt_body->GetID(); }
+
+	JoltSpace3D *get_space() const { return space; }
+	void set_space(JoltSpace3D *p_space);
+	bool in_space() const { return space != nullptr && jolt_body != nullptr; }
+
+	uint32_t get_collision_layer() const { return collision_layer; }
+	void set_collision_layer(uint32_t p_layer);
+
+	uint32_t get_collision_mask() const { return collision_mask; }
+	void set_collision_mask(uint32_t p_mask);
+
+	virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const = 0;
+
+	bool is_pickable() const { return pickable; }
+	void set_pickable(bool p_enabled) { pickable = p_enabled; }
+
+	bool can_collide_with(const JoltObject3D &p_other) const;
+	bool can_interact_with(const JoltObject3D &p_other) const;
+
+	virtual bool can_interact_with(const JoltBody3D &p_other) const = 0;
+	virtual bool can_interact_with(const JoltSoftBody3D &p_other) const = 0;
+	virtual bool can_interact_with(const JoltArea3D &p_other) const = 0;
+
+	virtual bool reports_contacts() const = 0;
+
+	virtual void pre_step(float p_step, JPH::Body &p_jolt_body) {}
+
+	String to_string() const;
+};

modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.cpp

@@ -0,0 +1,261 @@
+/**************************************************************************/
+/*  jolt_physics_direct_body_state_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_physics_direct_body_state_3d.h"
+
+#include "../spaces/jolt_physics_direct_space_state_3d.h"
+#include "../spaces/jolt_space_3d.h"
+#include "jolt_body_3d.h"
+
+JoltPhysicsDirectBodyState3D::JoltPhysicsDirectBodyState3D(JoltBody3D *p_body) :
+		body(p_body) {
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_total_gravity() const {
+	return body->get_gravity();
+}
+
+real_t JoltPhysicsDirectBodyState3D::get_total_angular_damp() const {
+	return (real_t)body->get_total_angular_damp();
+}
+
+real_t JoltPhysicsDirectBodyState3D::get_total_linear_damp() const {
+	return (real_t)body->get_total_linear_damp();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_center_of_mass() const {
+	return body->get_center_of_mass_relative();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_center_of_mass_local() const {
+	return body->get_center_of_mass_local();
+}
+
+Basis JoltPhysicsDirectBodyState3D::get_principal_inertia_axes() const {
+	return body->get_principal_inertia_axes();
+}
+
+real_t JoltPhysicsDirectBodyState3D::get_inverse_mass() const {
+	return 1.0 / body->get_mass();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_inverse_inertia() const {
+	return body->get_inverse_inertia();
+}
+
+Basis JoltPhysicsDirectBodyState3D::get_inverse_inertia_tensor() const {
+	return body->get_inverse_inertia_tensor();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_linear_velocity() const {
+	return body->get_linear_velocity();
+}
+
+void JoltPhysicsDirectBodyState3D::set_linear_velocity(const Vector3 &p_velocity) {
+	return body->set_linear_velocity(p_velocity);
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_angular_velocity() const {
+	return body->get_angular_velocity();
+}
+
+void JoltPhysicsDirectBodyState3D::set_angular_velocity(const Vector3 &p_velocity) {
+	return body->set_angular_velocity(p_velocity);
+}
+
+void JoltPhysicsDirectBodyState3D::set_transform(const Transform3D &p_transform) {
+	return body->set_transform(p_transform);
+}
+
+Transform3D JoltPhysicsDirectBodyState3D::get_transform() const {
+	return body->get_transform_scaled();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_velocity_at_local_position(const Vector3 &p_local_position) const {
+	return body->get_velocity_at_position(body->get_position() + p_local_position);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) {
+	return body->apply_central_impulse(p_impulse);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
+	return body->apply_impulse(p_impulse, p_position);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) {
+	return body->apply_torque_impulse(p_impulse);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_central_force(const Vector3 &p_force) {
+	return body->apply_central_force(p_force);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_force(const Vector3 &p_force, const Vector3 &p_position) {
+	return body->apply_force(p_force, p_position);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_torque(const Vector3 &p_torque) {
+	return body->apply_torque(p_torque);
+}
+
+void JoltPhysicsDirectBodyState3D::add_constant_central_force(const Vector3 &p_force) {
+	return body->add_constant_central_force(p_force);
+}
+
+void JoltPhysicsDirectBodyState3D::add_constant_force(const Vector3 &p_force, const Vector3 &p_position) {
+	return body->add_constant_force(p_force, p_position);
+}
+
+void JoltPhysicsDirectBodyState3D::add_constant_torque(const Vector3 &p_torque) {
+	return body->add_constant_torque(p_torque);
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_constant_force() const {
+	return body->get_constant_force();
+}
+
+void JoltPhysicsDirectBodyState3D::set_constant_force(const Vector3 &p_force) {
+	return body->set_constant_force(p_force);
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_constant_torque() const {
+	return body->get_constant_torque();
+}
+
+void JoltPhysicsDirectBodyState3D::set_constant_torque(const Vector3 &p_torque) {
+	return body->set_constant_torque(p_torque);
+}
+
+bool JoltPhysicsDirectBodyState3D::is_sleeping() const {
+	return body->is_sleeping();
+}
+
+void JoltPhysicsDirectBodyState3D::set_sleep_state(bool p_enabled) {
+	body->set_is_sleeping(p_enabled);
+}
+
+void JoltPhysicsDirectBodyState3D::set_collision_layer(uint32_t p_layer) {
+	body->set_collision_layer(p_layer);
+}
+
+uint32_t JoltPhysicsDirectBodyState3D::get_collision_layer() const {
+	return body->get_collision_layer();
+}
+
+void JoltPhysicsDirectBodyState3D::set_collision_mask(uint32_t p_mask) {
+	body->set_collision_mask(p_mask);
+}
+
+uint32_t JoltPhysicsDirectBodyState3D::get_collision_mask() const {
+	return body->get_collision_mask();
+}
+
+int JoltPhysicsDirectBodyState3D::get_contact_count() const {
+	return body->get_contact_count();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_local_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).position;
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_local_normal(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).normal;
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).impulse;
+}
+
+int JoltPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), 0);
+	return body->get_contact(p_contact_idx).shape_index;
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_local_velocity_at_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).velocity;
+}
+
+RID JoltPhysicsDirectBodyState3D::get_contact_collider(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), RID());
+	return body->get_contact(p_contact_idx).collider_rid;
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_collider_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).collider_position;
+}
+
+ObjectID JoltPhysicsDirectBodyState3D::get_contact_collider_id(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), ObjectID());
+	return body->get_contact(p_contact_idx).collider_id;
+}
+
+Object *JoltPhysicsDirectBodyState3D::get_contact_collider_object(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), nullptr);
+	return ObjectDB::get_instance(body->get_contact(p_contact_idx).collider_id);
+}
+
+int JoltPhysicsDirectBodyState3D::get_contact_collider_shape(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), 0);
+	return body->get_contact(p_contact_idx).collider_shape_index;
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_collider_velocity_at_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).collider_velocity;
+}
+
+real_t JoltPhysicsDirectBodyState3D::get_step() const {
+	return (real_t)body->get_space()->get_last_step();
+}
+
+void JoltPhysicsDirectBodyState3D::integrate_forces() {
+	const float step = (float)get_step();
+
+	Vector3 linear_velocity = get_linear_velocity();
+	Vector3 angular_velocity = get_angular_velocity();
+
+	linear_velocity *= MAX(1.0f - (float)get_total_linear_damp() * step, 0.0f);
+	angular_velocity *= MAX(1.0f - (float)get_total_angular_damp() * step, 0.0f);
+
+	linear_velocity += get_total_gravity() * step;
+
+	set_linear_velocity(linear_velocity);
+	set_angular_velocity(angular_velocity);
+}
+
+PhysicsDirectSpaceState3D *JoltPhysicsDirectBodyState3D::get_space_state() {
+	return body->get_space()->get_direct_state();
+}

modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.h

@@ -0,0 +1,119 @@
+/**************************************************************************/
+/*  jolt_physics_direct_body_state_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 "servers/physics_server_3d.h"
+
+class JoltBody3D;
+
+class JoltPhysicsDirectBodyState3D final : public PhysicsDirectBodyState3D {
+	GDCLASS(JoltPhysicsDirectBodyState3D, PhysicsDirectBodyState3D)
+
+	JoltBody3D *body = nullptr;
+
+	static void _bind_methods() {}
+
+public:
+	JoltPhysicsDirectBodyState3D() = default;
+
+	explicit JoltPhysicsDirectBodyState3D(JoltBody3D *p_body);
+
+	virtual Vector3 get_total_gravity() const override;
+	virtual real_t get_total_linear_damp() const override;
+	virtual real_t get_total_angular_damp() const override;
+
+	virtual Vector3 get_center_of_mass() const override;
+	virtual Vector3 get_center_of_mass_local() const override;
+	virtual Basis get_principal_inertia_axes() const override;
+
+	virtual real_t get_inverse_mass() const override;
+	virtual Vector3 get_inverse_inertia() const override;
+	virtual Basis get_inverse_inertia_tensor() const override;
+
+	virtual void set_linear_velocity(const Vector3 &p_velocity) override;
+	virtual Vector3 get_linear_velocity() const override;
+
+	virtual void set_angular_velocity(const Vector3 &p_velocity) override;
+	virtual Vector3 get_angular_velocity() const override;
+
+	virtual void set_transform(const Transform3D &p_transform) override;
+	virtual Transform3D get_transform() const override;
+
+	virtual Vector3 get_velocity_at_local_position(const Vector3 &p_local_position) const override;
+
+	virtual void apply_central_impulse(const Vector3 &p_impulse) override;
+	virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) override;
+	virtual void apply_torque_impulse(const Vector3 &p_impulse) override;
+
+	virtual void apply_central_force(const Vector3 &p_force) override;
+	virtual void apply_force(const Vector3 &p_force, const Vector3 &p_position) override;
+	virtual void apply_torque(const Vector3 &p_torque) override;
+
+	virtual void add_constant_central_force(const Vector3 &p_force) override;
+	virtual void add_constant_force(const Vector3 &p_force, const Vector3 &p_position) override;
+	virtual void add_constant_torque(const Vector3 &p_torque) override;
+
+	virtual void set_constant_force(const Vector3 &p_force) override;
+	virtual Vector3 get_constant_force() const override;
+
+	virtual void set_constant_torque(const Vector3 &p_torque) override;
+	virtual Vector3 get_constant_torque() const override;
+
+	virtual void set_sleep_state(bool p_enabled) override;
+	virtual bool is_sleeping() const override;
+
+	virtual void set_collision_layer(uint32_t p_layer) override;
+	virtual uint32_t get_collision_layer() const override;
+
+	virtual void set_collision_mask(uint32_t p_mask) override;
+	virtual uint32_t get_collision_mask() const override;
+
+	virtual int get_contact_count() const override;
+
+	virtual Vector3 get_contact_local_position(int p_contact_idx) const override;
+	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;
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const override;
+	virtual Object *get_contact_collider_object(int p_contact_idx) const override;
+	virtual int get_contact_collider_shape(int p_contact_idx) const override;
+	virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override;
+
+	virtual real_t get_step() const override;
+
+	virtual void integrate_forces() override;
+
+	virtual PhysicsDirectSpaceState3D *get_space_state() override;
+};

modules/jolt_physics/objects/jolt_shaped_object_3d.cpp

@@ -0,0 +1,459 @@
+/**************************************************************************/
+/*  jolt_shaped_object_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_shaped_object_3d.h"
+
+#include "../misc/jolt_math_funcs.h"
+#include "../misc/jolt_type_conversions.h"
+#include "../shapes/jolt_custom_double_sided_shape.h"
+#include "../shapes/jolt_shape_3d.h"
+#include "../spaces/jolt_space_3d.h"
+
+#include "Jolt/Physics/Collision/Shape/EmptyShape.h"
+#include "Jolt/Physics/Collision/Shape/MutableCompoundShape.h"
+#include "Jolt/Physics/Collision/Shape/StaticCompoundShape.h"
+
+bool JoltShapedObject3D::_is_big() const {
+	// This number is completely arbitrary, and mostly just needs to capture `WorldBoundaryShape3D`, which needs to be kept out of the normal broadphase layers.
+	return get_aabb().get_longest_axis_size() >= 1000.0f;
+}
+
+JPH::ShapeRefC JoltShapedObject3D::_try_build_shape(bool p_optimize_compound) {
+	int built_shapes = 0;
+
+	for (JoltShapeInstance3D &shape : shapes) {
+		if (shape.is_enabled() && shape.try_build()) {
+			built_shapes += 1;
+		}
+	}
+
+	if (unlikely(built_shapes == 0)) {
+		return nullptr;
+	}
+
+	JPH::ShapeRefC result = built_shapes == 1 ? _try_build_single_shape() : _try_build_compound_shape(p_optimize_compound);
+	if (unlikely(result == nullptr)) {
+		return nullptr;
+	}
+
+	if (has_custom_center_of_mass()) {
+		result = JoltShape3D::with_center_of_mass(result, get_center_of_mass_custom());
+	}
+
+	if (scale != Vector3(1, 1, 1)) {
+		Vector3 actual_scale = scale;
+		JOLT_ENSURE_SCALE_VALID(result, actual_scale, vformat("Failed to correctly scale body '%s'.", to_string()));
+		result = JoltShape3D::with_scale(result, actual_scale);
+	}
+
+	if (is_area()) {
+		result = JoltShape3D::with_double_sided(result, true);
+	}
+
+	return result;
+}
+
+JPH::ShapeRefC JoltShapedObject3D::_try_build_single_shape() {
+	for (int shape_index = 0; shape_index < (int)shapes.size(); ++shape_index) {
+		const JoltShapeInstance3D &sub_shape = shapes[shape_index];
+
+		if (!sub_shape.is_enabled() || !sub_shape.is_built()) {
+			continue;
+		}
+
+		JPH::ShapeRefC jolt_sub_shape = sub_shape.get_jolt_ref();
+
+		Vector3 sub_shape_scale = sub_shape.get_scale();
+		const Transform3D sub_shape_transform = sub_shape.get_transform_unscaled();
+
+		if (sub_shape_scale != Vector3(1, 1, 1)) {
+			JOLT_ENSURE_SCALE_VALID(jolt_sub_shape, sub_shape_scale, vformat("Failed to correctly scale shape at index %d in body '%s'.", shape_index, to_string()));
+			jolt_sub_shape = JoltShape3D::with_scale(jolt_sub_shape, sub_shape_scale);
+		}
+
+		if (sub_shape_transform != Transform3D()) {
+			jolt_sub_shape = JoltShape3D::with_basis_origin(jolt_sub_shape, sub_shape_transform.basis, sub_shape_transform.origin);
+		}
+
+		return jolt_sub_shape;
+	}
+
+	return nullptr;
+}
+
+JPH::ShapeRefC JoltShapedObject3D::_try_build_compound_shape(bool p_optimize) {
+	JPH::StaticCompoundShapeSettings static_compound_shape_settings;
+	JPH::MutableCompoundShapeSettings mutable_compound_shape_settings;
+	JPH::CompoundShapeSettings *compound_shape_settings = p_optimize ? static_cast<JPH::CompoundShapeSettings *>(&static_compound_shape_settings) : static_cast<JPH::CompoundShapeSettings *>(&mutable_compound_shape_settings);
+
+	compound_shape_settings->mSubShapes.reserve((size_t)shapes.size());
+
+	for (int shape_index = 0; shape_index < (int)shapes.size(); ++shape_index) {
+		const JoltShapeInstance3D &sub_shape = shapes[shape_index];
+
+		if (!sub_shape.is_enabled() || !sub_shape.is_built()) {
+			continue;
+		}
+
+		JPH::ShapeRefC jolt_sub_shape = sub_shape.get_jolt_ref();
+
+		Vector3 sub_shape_scale = sub_shape.get_scale();
+		const Transform3D sub_shape_transform = sub_shape.get_transform_unscaled();
+
+		if (sub_shape_scale != Vector3(1, 1, 1)) {
+			JOLT_ENSURE_SCALE_VALID(jolt_sub_shape, sub_shape_scale, vformat("Failed to correctly scale shape at index %d for body '%s'.", shape_index, to_string()));
+			jolt_sub_shape = JoltShape3D::with_scale(jolt_sub_shape, sub_shape_scale);
+		}
+
+		compound_shape_settings->AddShape(to_jolt(sub_shape_transform.origin), to_jolt(sub_shape_transform.basis), jolt_sub_shape);
+	}
+
+	const JPH::ShapeSettings::ShapeResult shape_result = p_optimize ? static_compound_shape_settings.Create(space->get_temp_allocator()) : mutable_compound_shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to create compound shape for body '%s'. It returned the following error: '%s'.", to_string(), to_godot(shape_result.GetError())));
+
+	return shape_result.Get();
+}
+
+void JoltShapedObject3D::_enqueue_shapes_changed() {
+	if (space != nullptr) {
+		space->enqueue_shapes_changed(&shapes_changed_element);
+	}
+}
+
+void JoltShapedObject3D::_dequeue_shapes_changed() {
+	if (space != nullptr) {
+		space->dequeue_shapes_changed(&shapes_changed_element);
+	}
+}
+
+void JoltShapedObject3D::_enqueue_needs_optimization() {
+	if (space != nullptr) {
+		space->enqueue_needs_optimization(&needs_optimization_element);
+	}
+}
+
+void JoltShapedObject3D::_dequeue_needs_optimization() {
+	if (space != nullptr) {
+		space->dequeue_needs_optimization(&needs_optimization_element);
+	}
+}
+
+void JoltShapedObject3D::_shapes_changed() {
+	commit_shapes(false);
+}
+
+void JoltShapedObject3D::_shapes_committed() {
+	_update_object_layer();
+}
+
+void JoltShapedObject3D::_space_changing() {
+	JoltObject3D::_space_changing();
+
+	_dequeue_shapes_changed();
+	_dequeue_needs_optimization();
+
+	previous_jolt_shape = nullptr;
+
+	if (in_space()) {
+		jolt_settings = new JPH::BodyCreationSettings(jolt_body->GetBodyCreationSettings());
+	}
+}
+
+JoltShapedObject3D::JoltShapedObject3D(ObjectType p_object_type) :
+		JoltObject3D(p_object_type),
+		shapes_changed_element(this),
+		needs_optimization_element(this) {
+	jolt_settings->mAllowSleeping = true;
+	jolt_settings->mFriction = 1.0f;
+	jolt_settings->mRestitution = 0.0f;
+	jolt_settings->mLinearDamping = 0.0f;
+	jolt_settings->mAngularDamping = 0.0f;
+	jolt_settings->mGravityFactor = 0.0f;
+}
+
+JoltShapedObject3D::~JoltShapedObject3D() {
+	if (jolt_settings != nullptr) {
+		delete jolt_settings;
+		jolt_settings = nullptr;
+	}
+}
+
+Transform3D JoltShapedObject3D::get_transform_unscaled() const {
+	if (!in_space()) {
+		return Transform3D(to_godot(jolt_settings->mRotation), to_godot(jolt_settings->mPosition));
+	} else {
+		return Transform3D(to_godot(jolt_body->GetRotation()), to_godot(jolt_body->GetPosition()));
+	}
+}
+
+Transform3D JoltShapedObject3D::get_transform_scaled() const {
+	return get_transform_unscaled().scaled_local(scale);
+}
+
+Basis JoltShapedObject3D::get_basis() const {
+	if (!in_space()) {
+		return to_godot(jolt_settings->mRotation);
+	} else {
+		return to_godot(jolt_body->GetRotation());
+	}
+}
+
+Vector3 JoltShapedObject3D::get_position() const {
+	if (!in_space()) {
+		return to_godot(jolt_settings->mPosition);
+	} else {
+		return to_godot(jolt_body->GetPosition());
+	}
+}
+
+Vector3 JoltShapedObject3D::get_center_of_mass() const {
+	ERR_FAIL_COND_V_MSG(!in_space(), Vector3(), vformat("Failed to retrieve center-of-mass of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+	return to_godot(jolt_body->GetCenterOfMassPosition());
+}
+
+Vector3 JoltShapedObject3D::get_center_of_mass_relative() const {
+	return get_center_of_mass() - get_position();
+}
+
+Vector3 JoltShapedObject3D::get_center_of_mass_local() const {
+	ERR_FAIL_NULL_V_MSG(space, Vector3(), vformat("Failed to retrieve local center-of-mass of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	return get_transform_scaled().xform_inv(get_center_of_mass());
+}
+
+Vector3 JoltShapedObject3D::get_linear_velocity() const {
+	if (!in_space()) {
+		return to_godot(jolt_settings->mLinearVelocity);
+	} else {
+		return to_godot(jolt_body->GetLinearVelocity());
+	}
+}
+
+Vector3 JoltShapedObject3D::get_angular_velocity() const {
+	if (!in_space()) {
+		return to_godot(jolt_settings->mAngularVelocity);
+	} else {
+		return to_godot(jolt_body->GetAngularVelocity());
+	}
+}
+
+AABB JoltShapedObject3D::get_aabb() const {
+	AABB result;
+
+	for (const JoltShapeInstance3D &shape : shapes) {
+		if (shape.is_disabled()) {
+			continue;
+		}
+
+		if (result == AABB()) {
+			result = shape.get_aabb();
+		} else {
+			result.merge_with(shape.get_aabb());
+		}
+	}
+
+	return get_transform_scaled().xform(result);
+}
+
+JPH::ShapeRefC JoltShapedObject3D::build_shapes(bool p_optimize_compound) {
+	JPH::ShapeRefC new_shape = _try_build_shape(p_optimize_compound);
+
+	if (new_shape == nullptr) {
+		if (has_custom_center_of_mass()) {
+			new_shape = JPH::EmptyShapeSettings(to_jolt(get_center_of_mass_custom())).Create().Get();
+		} else {
+			new_shape = new JPH::EmptyShape();
+		}
+	}
+
+	return new_shape;
+}
+
+void JoltShapedObject3D::commit_shapes(bool p_optimize_compound) {
+	if (!in_space()) {
+		_shapes_committed();
+		return;
+	}
+
+	JPH::ShapeRefC new_shape = build_shapes(p_optimize_compound);
+	if (new_shape == jolt_shape) {
+		return;
+	}
+
+	if (previous_jolt_shape == nullptr) {
+		previous_jolt_shape = jolt_shape;
+	}
+
+	jolt_shape = new_shape;
+
+	space->get_body_iface().SetShape(jolt_body->GetID(), jolt_shape, false, JPH::EActivation::DontActivate);
+
+	_enqueue_shapes_changed();
+
+	if (!p_optimize_compound && jolt_shape->GetType() == JPH::EShapeType::Compound) {
+		_enqueue_needs_optimization();
+	} else {
+		_dequeue_needs_optimization();
+	}
+
+	_shapes_committed();
+}
+
+void JoltShapedObject3D::add_shape(JoltShape3D *p_shape, Transform3D p_transform, bool p_disabled) {
+	JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed when adding shape at index %d to physics body '%s'.", shapes.size(), to_string()));
+
+	Vector3 shape_scale;
+	JoltMath::decompose(p_transform, shape_scale);
+
+	shapes.push_back(JoltShapeInstance3D(this, p_shape, p_transform, shape_scale, p_disabled));
+
+	_shapes_changed();
+}
+
+void JoltShapedObject3D::remove_shape(const JoltShape3D *p_shape) {
+	for (int i = shapes.size() - 1; i >= 0; i--) {
+		if (shapes[i].get_shape() == p_shape) {
+			shapes.remove_at(i);
+		}
+	}
+
+	_shapes_changed();
+}
+
+void JoltShapedObject3D::remove_shape(int p_index) {
+	ERR_FAIL_INDEX(p_index, (int)shapes.size());
+	shapes.remove_at(p_index);
+
+	_shapes_changed();
+}
+
+JoltShape3D *JoltShapedObject3D::get_shape(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), nullptr);
+	return shapes[p_index].get_shape();
+}
+
+void JoltShapedObject3D::set_shape(int p_index, JoltShape3D *p_shape) {
+	ERR_FAIL_INDEX(p_index, (int)shapes.size());
+	shapes[p_index] = JoltShapeInstance3D(this, p_shape);
+
+	_shapes_changed();
+}
+
+void JoltShapedObject3D::clear_shapes() {
+	shapes.clear();
+
+	_shapes_changed();
+}
+
+void JoltShapedObject3D::clear_previous_shape() {
+	previous_jolt_shape = nullptr;
+}
+
+int JoltShapedObject3D::find_shape_index(uint32_t p_shape_instance_id) const {
+	for (int i = 0; i < (int)shapes.size(); ++i) {
+		if (shapes[i].get_id() == p_shape_instance_id) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+int JoltShapedObject3D::find_shape_index(const JPH::SubShapeID &p_sub_shape_id) const {
+	ERR_FAIL_NULL_V(jolt_shape, -1);
+	return find_shape_index((uint32_t)jolt_shape->GetSubShapeUserData(p_sub_shape_id));
+}
+
+JoltShape3D *JoltShapedObject3D::find_shape(uint32_t p_shape_instance_id) const {
+	const int shape_index = find_shape_index(p_shape_instance_id);
+	return shape_index != -1 ? shapes[shape_index].get_shape() : nullptr;
+}
+
+JoltShape3D *JoltShapedObject3D::find_shape(const JPH::SubShapeID &p_sub_shape_id) const {
+	const int shape_index = find_shape_index(p_sub_shape_id);
+	return shape_index != -1 ? shapes[shape_index].get_shape() : nullptr;
+}
+
+Transform3D JoltShapedObject3D::get_shape_transform_unscaled(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), Transform3D());
+	return shapes[p_index].get_transform_unscaled();
+}
+
+Transform3D JoltShapedObject3D::get_shape_transform_scaled(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), Transform3D());
+	return shapes[p_index].get_transform_scaled();
+}
+
+void JoltShapedObject3D::set_shape_transform(int p_index, Transform3D p_transform) {
+	ERR_FAIL_INDEX(p_index, (int)shapes.size());
+	JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, "Failed to correctly set transform for shape at index %d in body '%s'.");
+
+	Vector3 new_scale;
+	JoltMath::decompose(p_transform, new_scale);
+
+	JoltShapeInstance3D &shape = shapes[p_index];
+
+	if (shape.get_transform_unscaled() == p_transform && shape.get_scale() == new_scale) {
+		return;
+	}
+
+	shape.set_transform(p_transform);
+	shape.set_scale(new_scale);
+
+	_shapes_changed();
+}
+
+Vector3 JoltShapedObject3D::get_shape_scale(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), Vector3());
+	return shapes[p_index].get_scale();
+}
+
+bool JoltShapedObject3D::is_shape_disabled(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), false);
+	return shapes[p_index].is_disabled();
+}
+
+void JoltShapedObject3D::set_shape_disabled(int p_index, bool p_disabled) {
+	ERR_FAIL_INDEX(p_index, (int)shapes.size());
+
+	JoltShapeInstance3D &shape = shapes[p_index];
+
+	if (shape.is_disabled() == p_disabled) {
+		return;
+	}
+
+	if (p_disabled) {
+		shape.disable();
+	} else {
+		shape.enable();
+	}
+
+	_shapes_changed();
+}

modules/jolt_physics/objects/jolt_shaped_object_3d.h

@@ -0,0 +1,130 @@
+/**************************************************************************/
+/*  jolt_shaped_object_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_object_3d.h"
+
+#include "core/templates/self_list.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Body/BodyCreationSettings.h"
+
+class JoltShapedObject3D : public JoltObject3D {
+	friend class JoltShape3D;
+
+protected:
+	SelfList<JoltShapedObject3D> shapes_changed_element;
+	SelfList<JoltShapedObject3D> needs_optimization_element;
+
+	Vector3 scale = Vector3(1, 1, 1);
+
+	JPH::ShapeRefC jolt_shape;
+	JPH::ShapeRefC previous_jolt_shape;
+
+	JPH::BodyCreationSettings *jolt_settings = new JPH::BodyCreationSettings();
+
+	virtual JPH::EMotionType _get_motion_type() const = 0;
+
+	bool _is_big() const;
+
+	JPH::ShapeRefC _try_build_shape(bool p_optimize_compound);
+	JPH::ShapeRefC _try_build_single_shape();
+	JPH::ShapeRefC _try_build_compound_shape(bool p_optimize);
+
+	void _enqueue_shapes_changed();
+	void _dequeue_shapes_changed();
+
+	void _enqueue_needs_optimization();
+	void _dequeue_needs_optimization();
+
+	virtual void _shapes_changed();
+	virtual void _shapes_committed();
+	virtual void _space_changing() override;
+
+public:
+	explicit JoltShapedObject3D(ObjectType p_object_type);
+	virtual ~JoltShapedObject3D() override;
+
+	Transform3D get_transform_unscaled() const;
+	Transform3D get_transform_scaled() const;
+
+	Vector3 get_scale() const { return scale; }
+	Basis get_basis() const;
+	Vector3 get_position() const;
+
+	Vector3 get_center_of_mass() const;
+	Vector3 get_center_of_mass_relative() const;
+	Vector3 get_center_of_mass_local() const;
+
+	Vector3 get_linear_velocity() const;
+	Vector3 get_angular_velocity() const;
+
+	AABB get_aabb() const;
+
+	virtual bool has_custom_center_of_mass() const = 0;
+	virtual Vector3 get_center_of_mass_custom() const = 0;
+
+	JPH::ShapeRefC build_shapes(bool p_optimize_compound);
+
+	void commit_shapes(bool p_optimize_compound);
+
+	const JPH::Shape *get_jolt_shape() const { return jolt_shape; }
+	const JPH::Shape *get_previous_jolt_shape() const { return previous_jolt_shape; }
+
+	void add_shape(JoltShape3D *p_shape, Transform3D p_transform, bool p_disabled);
+	void remove_shape(const JoltShape3D *p_shape);
+	void remove_shape(int p_index);
+
+	JoltShape3D *get_shape(int p_index) const;
+	void set_shape(int p_index, JoltShape3D *p_shape);
+
+	void clear_shapes();
+	void clear_previous_shape();
+
+	int get_shape_count() const { return shapes.size(); }
+
+	int find_shape_index(uint32_t p_shape_instance_id) const;
+	int find_shape_index(const JPH::SubShapeID &p_sub_shape_id) const;
+
+	JoltShape3D *find_shape(uint32_t p_shape_instance_id) const;
+	JoltShape3D *find_shape(const JPH::SubShapeID &p_sub_shape_id) const;
+
+	Transform3D get_shape_transform_unscaled(int p_index) const;
+	Transform3D get_shape_transform_scaled(int p_index) const;
+	void set_shape_transform(int p_index, Transform3D p_transform);
+
+	Vector3 get_shape_scale(int p_index) const;
+
+	bool is_shape_disabled(int p_index) const;
+	void set_shape_disabled(int p_index, bool p_disabled);
+};

modules/jolt_physics/objects/jolt_soft_body_3d.cpp

@@ -0,0 +1,750 @@
+/**************************************************************************/
+/*  jolt_soft_body_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_soft_body_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+#include "../spaces/jolt_broad_phase_layer.h"
+#include "../spaces/jolt_space_3d.h"
+#include "jolt_area_3d.h"
+#include "jolt_body_3d.h"
+#include "jolt_group_filter.h"
+
+#include "servers/rendering_server.h"
+
+#include "Jolt/Physics/SoftBody/SoftBodyMotionProperties.h"
+
+namespace {
+
+template <typename TJoltVertex>
+void pin_vertices(const JoltSoftBody3D &p_body, const HashSet<int> &p_pinned_vertices, const LocalVector<int> &p_mesh_to_physics, JPH::Array<TJoltVertex> &r_physics_vertices) {
+	const int mesh_vertex_count = p_mesh_to_physics.size();
+	const int physics_vertex_count = (int)r_physics_vertices.size();
+
+	for (int mesh_index : p_pinned_vertices) {
+		ERR_CONTINUE_MSG(mesh_index < 0 || mesh_index >= mesh_vertex_count, vformat("Index %d of pinned vertex in soft body '%s' is out of bounds. There are only %d vertices in the current mesh.", mesh_index, p_body.to_string(), mesh_vertex_count));
+
+		const int physics_index = p_mesh_to_physics[mesh_index];
+		ERR_CONTINUE_MSG(physics_index < 0 || physics_index >= physics_vertex_count, vformat("Index %d of pinned vertex in soft body '%s' is out of bounds. There are only %d vertices in the current mesh. This should not happen. Please report this.", physics_index, p_body.to_string(), physics_vertex_count));
+
+		r_physics_vertices[physics_index].mInvMass = 0.0f;
+	}
+}
+
+} // namespace
+
+JPH::BroadPhaseLayer JoltSoftBody3D::_get_broad_phase_layer() const {
+	return JoltBroadPhaseLayer::BODY_DYNAMIC;
+}
+
+JPH::ObjectLayer JoltSoftBody3D::_get_object_layer() const {
+	ERR_FAIL_NULL_V(space, 0);
+
+	return space->map_to_object_layer(_get_broad_phase_layer(), collision_layer, collision_mask);
+}
+
+void JoltSoftBody3D::_space_changing() {
+	JoltObject3D::_space_changing();
+
+	// Note that we should not use `in_space()` as the condition here, since we could have cleared the mesh at this point.
+	if (jolt_body != nullptr) {
+		jolt_settings = new JPH::SoftBodyCreationSettings(jolt_body->GetSoftBodyCreationSettings());
+		jolt_settings->mSettings = nullptr;
+	}
+
+	_deref_shared_data();
+}
+
+void JoltSoftBody3D::_space_changed() {
+	JoltObject3D::_space_changed();
+
+	_update_mass();
+	_update_pressure();
+	_update_damping();
+	_update_simulation_precision();
+	_update_group_filter();
+}
+
+void JoltSoftBody3D::_add_to_space() {
+	if (unlikely(space == nullptr || !mesh.is_valid())) {
+		return;
+	}
+
+	const bool has_valid_shared = _ref_shared_data();
+	ERR_FAIL_COND(!has_valid_shared);
+
+	JPH::CollisionGroup::GroupID group_id = 0;
+	JPH::CollisionGroup::SubGroupID sub_group_id = 0;
+	JoltGroupFilter::encode_object(this, group_id, sub_group_id);
+
+	jolt_settings->mSettings = shared->settings;
+	jolt_settings->mUserData = reinterpret_cast<JPH::uint64>(this);
+	jolt_settings->mObjectLayer = _get_object_layer();
+	jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id);
+	jolt_settings->mMaxLinearVelocity = JoltProjectSettings::max_linear_velocity;
+
+	JPH::Body *new_jolt_body = space->add_object(*this, *jolt_settings);
+	if (new_jolt_body == nullptr) {
+		return;
+	}
+
+	jolt_body = new_jolt_body;
+
+	delete jolt_settings;
+	jolt_settings = nullptr;
+}
+
+bool JoltSoftBody3D::_ref_shared_data() {
+	HashMap<RID, Shared>::Iterator iter_shared_data = mesh_to_shared.find(mesh);
+
+	if (iter_shared_data == mesh_to_shared.end()) {
+		RenderingServer *rendering = RenderingServer::get_singleton();
+
+		// TODO: calling RenderingServer::mesh_surface_get_arrays() from the physics thread
+		// is not safe and can deadlock when physics/3d/run_on_separate_thread is enabled.
+		// This method blocks on the main thread to return data, but the main thread may be
+		// blocked waiting on us in PhysicsServer3D::sync().
+		const Array mesh_data = rendering->mesh_surface_get_arrays(mesh, 0);
+		ERR_FAIL_COND_V(mesh_data.is_empty(), false);
+
+		const PackedInt32Array mesh_indices = mesh_data[RenderingServer::ARRAY_INDEX];
+		ERR_FAIL_COND_V(mesh_indices.is_empty(), false);
+
+		const PackedVector3Array mesh_vertices = mesh_data[RenderingServer::ARRAY_VERTEX];
+		ERR_FAIL_COND_V(mesh_vertices.is_empty(), false);
+
+		iter_shared_data = mesh_to_shared.insert(mesh, Shared());
+
+		LocalVector<int> &mesh_to_physics = iter_shared_data->value.mesh_to_physics;
+
+		JPH::SoftBodySharedSettings &settings = *iter_shared_data->value.settings;
+		settings.mVertexRadius = JoltProjectSettings::soft_body_point_radius;
+
+		JPH::Array<JPH::SoftBodySharedSettings::Vertex> &physics_vertices = settings.mVertices;
+		JPH::Array<JPH::SoftBodySharedSettings::Face> &physics_faces = settings.mFaces;
+
+		HashMap<Vector3, int> vertex_to_physics;
+
+		const int mesh_vertex_count = mesh_vertices.size();
+		const int mesh_index_count = mesh_indices.size();
+
+		mesh_to_physics.resize(mesh_vertex_count);
+		for (int &index : mesh_to_physics) {
+			index = -1;
+		}
+		physics_vertices.reserve(mesh_vertex_count);
+		vertex_to_physics.reserve(mesh_vertex_count);
+
+		int physics_index_count = 0;
+
+		for (int i = 0; i < mesh_index_count; i += 3) {
+			int physics_face[3];
+
+			for (int j = 0; j < 3; ++j) {
+				const int mesh_index = mesh_indices[i + j];
+				const Vector3 vertex = mesh_vertices[mesh_index];
+
+				HashMap<Vector3, int>::Iterator iter_physics_index = vertex_to_physics.find(vertex);
+
+				if (iter_physics_index == vertex_to_physics.end()) {
+					physics_vertices.emplace_back(JPH::Float3((float)vertex.x, (float)vertex.y, (float)vertex.z), JPH::Float3(0.0f, 0.0f, 0.0f), 1.0f);
+					iter_physics_index = vertex_to_physics.insert(vertex, physics_index_count++);
+				}
+
+				physics_face[j] = iter_physics_index->value;
+				mesh_to_physics[mesh_index] = iter_physics_index->value;
+			}
+
+			if (physics_face[0] == physics_face[1] || physics_face[0] == physics_face[2] || physics_face[1] == physics_face[2]) {
+				continue; // We skip degenerate faces, since they're problematic, and Jolt will assert about it anyway.
+			}
+
+			// Jolt uses a different winding order, so we swap the indices to account for that.
+			physics_faces.emplace_back((JPH::uint32)physics_face[2], (JPH::uint32)physics_face[1], (JPH::uint32)physics_face[0]);
+		}
+
+		// Pin whatever pinned vertices we have currently. This is used during the `Optimize` call below to order the
+		// constraints. Note that it's fine if the pinned vertices change later, but that will reduce the effectiveness
+		// of the constraints a bit.
+		pin_vertices(*this, pinned_vertices, mesh_to_physics, physics_vertices);
+
+		// Since Godot's stiffness is input as a coefficient between 0 and 1, and Jolt uses actual stiffness for its
+		// edge constraints, we crudely map one to the other with an arbitrary constant.
+		const float stiffness = MAX(Math::pow(stiffness_coefficient, 3.0f) * 100000.0f, 0.000001f);
+		const float inverse_stiffness = 1.0f / stiffness;
+
+		JPH::SoftBodySharedSettings::VertexAttributes vertex_attrib;
+		vertex_attrib.mCompliance = vertex_attrib.mShearCompliance = inverse_stiffness;
+
+		settings.CreateConstraints(&vertex_attrib, 1, JPH::SoftBodySharedSettings::EBendType::None);
+		float multiplier = 1.0f - shrinking_factor;
+		for (JPH::SoftBodySharedSettings::Edge &e : settings.mEdgeConstraints) {
+			e.mRestLength *= multiplier;
+		}
+		settings.Optimize();
+	} else {
+		iter_shared_data->value.ref_count++;
+	}
+
+	shared = &iter_shared_data->value;
+
+	return true;
+}
+
+void JoltSoftBody3D::_deref_shared_data() {
+	if (unlikely(shared == nullptr)) {
+		return;
+	}
+
+	HashMap<RID, Shared>::Iterator iter = mesh_to_shared.find(mesh);
+	if (unlikely(iter == mesh_to_shared.end())) {
+		return;
+	}
+
+	if (--iter->value.ref_count == 0) {
+		mesh_to_shared.remove(iter);
+	}
+
+	shared = nullptr;
+}
+
+void JoltSoftBody3D::_update_mass() {
+	if (!in_space()) {
+		return;
+	}
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
+	JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+
+	const float inverse_vertex_mass = mass == 0.0f ? 1.0f : (float)physics_vertices.size() / mass;
+
+	for (JPH::SoftBodyVertex &vertex : physics_vertices) {
+		vertex.mInvMass = inverse_vertex_mass;
+	}
+
+	pin_vertices(*this, pinned_vertices, shared->mesh_to_physics, physics_vertices);
+}
+
+void JoltSoftBody3D::_update_pressure() {
+	if (!in_space()) {
+		jolt_settings->mPressure = pressure;
+		return;
+	}
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
+	motion_properties.SetPressure(pressure);
+}
+
+void JoltSoftBody3D::_update_damping() {
+	if (!in_space()) {
+		jolt_settings->mLinearDamping = linear_damping;
+		return;
+	}
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
+	motion_properties.SetLinearDamping(linear_damping);
+}
+
+void JoltSoftBody3D::_update_simulation_precision() {
+	if (!in_space()) {
+		jolt_settings->mNumIterations = (JPH::uint32)simulation_precision;
+		return;
+	}
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
+	motion_properties.SetNumIterations((JPH::uint32)simulation_precision);
+}
+
+void JoltSoftBody3D::_update_group_filter() {
+	JPH::GroupFilter *group_filter = !exceptions.is_empty() ? JoltGroupFilter::instance : nullptr;
+
+	if (!in_space()) {
+		jolt_settings->mCollisionGroup.SetGroupFilter(group_filter);
+	} else {
+		jolt_body->GetCollisionGroup().SetGroupFilter(group_filter);
+	}
+}
+
+void JoltSoftBody3D::_try_rebuild() {
+	if (space != nullptr) {
+		_reset_space();
+	}
+}
+
+void JoltSoftBody3D::_mesh_changed() {
+	_try_rebuild();
+}
+
+void JoltSoftBody3D::_simulation_precision_changed() {
+	wake_up();
+}
+
+void JoltSoftBody3D::_mass_changed() {
+	wake_up();
+}
+
+void JoltSoftBody3D::_pressure_changed() {
+	_update_pressure();
+	wake_up();
+}
+
+void JoltSoftBody3D::_damping_changed() {
+	_update_damping();
+	wake_up();
+}
+
+void JoltSoftBody3D::_pins_changed() {
+	_update_mass();
+	wake_up();
+}
+
+void JoltSoftBody3D::_vertices_changed() {
+	wake_up();
+}
+
+void JoltSoftBody3D::_exceptions_changed() {
+	_update_group_filter();
+}
+
+void JoltSoftBody3D::_motion_changed() {
+	wake_up();
+}
+
+JoltSoftBody3D::JoltSoftBody3D() :
+		JoltObject3D(OBJECT_TYPE_SOFT_BODY) {
+	jolt_settings->mRestitution = 0.0f;
+	jolt_settings->mFriction = 1.0f;
+	jolt_settings->mUpdatePosition = false;
+	jolt_settings->mMakeRotationIdentity = false;
+}
+
+JoltSoftBody3D::~JoltSoftBody3D() {
+	if (jolt_settings != nullptr) {
+		delete jolt_settings;
+		jolt_settings = nullptr;
+	}
+}
+
+bool JoltSoftBody3D::in_space() const {
+	return JoltObject3D::in_space() && shared != nullptr;
+}
+
+void JoltSoftBody3D::add_collision_exception(const RID &p_excepted_body) {
+	exceptions.push_back(p_excepted_body);
+
+	_exceptions_changed();
+}
+
+void JoltSoftBody3D::remove_collision_exception(const RID &p_excepted_body) {
+	exceptions.erase(p_excepted_body);
+
+	_exceptions_changed();
+}
+
+bool JoltSoftBody3D::has_collision_exception(const RID &p_excepted_body) const {
+	return exceptions.find(p_excepted_body) >= 0;
+}
+
+bool JoltSoftBody3D::can_interact_with(const JoltBody3D &p_other) const {
+	return (can_collide_with(p_other) || p_other.can_collide_with(*this)) && !has_collision_exception(p_other.get_rid()) && !p_other.has_collision_exception(rid);
+}
+
+bool JoltSoftBody3D::can_interact_with(const JoltSoftBody3D &p_other) const {
+	return (can_collide_with(p_other) || p_other.can_collide_with(*this)) && !has_collision_exception(p_other.get_rid()) && !p_other.has_collision_exception(rid);
+}
+
+bool JoltSoftBody3D::can_interact_with(const JoltArea3D &p_other) const {
+	return p_other.can_interact_with(*this);
+}
+
+Vector3 JoltSoftBody3D::get_velocity_at_position(const Vector3 &p_position) const {
+	return Vector3();
+}
+
+void JoltSoftBody3D::set_mesh(const RID &p_mesh) {
+	if (unlikely(mesh == p_mesh)) {
+		return;
+	}
+
+	_deref_shared_data();
+	mesh = p_mesh;
+	_mesh_changed();
+}
+
+bool JoltSoftBody3D::is_sleeping() const {
+	if (!in_space()) {
+		return false;
+	} else {
+		return !jolt_body->IsActive();
+	}
+}
+
+void JoltSoftBody3D::apply_vertex_impulse(int p_index, const Vector3 &p_impulse) {
+	ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	ERR_FAIL_NULL(shared);
+	ERR_FAIL_INDEX(p_index, (int)shared->mesh_to_physics.size());
+	const int physics_index = shared->mesh_to_physics[p_index];
+	ERR_FAIL_COND_MSG(physics_index < 0, vformat("Soft body vertex %d was not used by a face and has been omitted for '%s'. No impulse can be applied.", p_index, to_string()));
+	ERR_FAIL_COND_MSG(pinned_vertices.has(physics_index), vformat("Failed to apply impulse to point at index %d for '%s'. Point was found to be pinned.", static_cast<int>(physics_index), to_string()));
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
+
+	JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+	JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index];
+
+	physics_vertex.mVelocity += to_jolt(p_impulse) * physics_vertex.mInvMass;
+
+	_motion_changed();
+}
+
+void JoltSoftBody3D::apply_vertex_force(int p_index, const Vector3 &p_force) {
+	ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	apply_vertex_impulse(p_index, p_force * space->get_last_step());
+}
+
+void JoltSoftBody3D::apply_central_impulse(const Vector3 &p_impulse) {
+	ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply central impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
+	JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+
+	const JPH::Vec3 impulse = to_jolt(p_impulse) / physics_vertices.size();
+
+	for (JPH::SoftBodyVertex &physics_vertex : physics_vertices) {
+		if (physics_vertex.mInvMass > 0.0f) {
+			physics_vertex.mVelocity += impulse * physics_vertex.mInvMass;
+		}
+	}
+
+	_motion_changed();
+}
+
+void JoltSoftBody3D::apply_central_force(const Vector3 &p_force) {
+	ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply central force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	jolt_body->AddForce(to_jolt(p_force));
+
+	_motion_changed();
+}
+
+void JoltSoftBody3D::set_is_sleeping(bool p_enabled) {
+	if (!in_space()) {
+		return;
+	}
+
+	space->set_is_object_sleeping(jolt_body->GetID(), p_enabled);
+}
+
+bool JoltSoftBody3D::is_sleep_allowed() const {
+	if (!in_space()) {
+		return jolt_settings->mAllowSleeping;
+	} else {
+		return jolt_body->GetAllowSleeping();
+	}
+}
+
+void JoltSoftBody3D::set_is_sleep_allowed(bool p_enabled) {
+	if (!in_space()) {
+		jolt_settings->mAllowSleeping = p_enabled;
+	} else {
+		jolt_body->SetAllowSleeping(p_enabled);
+	}
+}
+
+void JoltSoftBody3D::set_simulation_precision(int p_precision) {
+	if (unlikely(simulation_precision == p_precision)) {
+		return;
+	}
+
+	simulation_precision = MAX(p_precision, 0);
+
+	_simulation_precision_changed();
+}
+
+void JoltSoftBody3D::set_mass(float p_mass) {
+	if (unlikely(mass == p_mass)) {
+		return;
+	}
+
+	mass = MAX(p_mass, 0.0f);
+
+	_mass_changed();
+}
+
+float JoltSoftBody3D::get_stiffness_coefficient() const {
+	return stiffness_coefficient;
+}
+
+void JoltSoftBody3D::set_stiffness_coefficient(float p_coefficient) {
+	stiffness_coefficient = CLAMP(p_coefficient, 0.0f, 1.0f);
+}
+
+float JoltSoftBody3D::get_shrinking_factor() const {
+	return shrinking_factor;
+}
+
+void JoltSoftBody3D::set_shrinking_factor(float p_shrinking_factor) {
+	shrinking_factor = p_shrinking_factor;
+}
+
+void JoltSoftBody3D::set_pressure(float p_pressure) {
+	if (unlikely(pressure == p_pressure)) {
+		return;
+	}
+
+	pressure = MAX(p_pressure, 0.0f);
+
+	_pressure_changed();
+}
+
+void JoltSoftBody3D::set_linear_damping(float p_damping) {
+	if (unlikely(linear_damping == p_damping)) {
+		return;
+	}
+
+	linear_damping = MAX(p_damping, 0.0f);
+
+	_damping_changed();
+}
+
+float JoltSoftBody3D::get_drag() const {
+	// Drag is not a thing in Jolt, and not supported by Godot Physics either.
+	return 0.0f;
+}
+
+void JoltSoftBody3D::set_drag(float p_drag) {
+	// Drag is not a thing in Jolt, and not supported by Godot Physics either.
+}
+
+Variant JoltSoftBody3D::get_state(PhysicsServer3D::BodyState p_state) const {
+	switch (p_state) {
+		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
+			return get_transform();
+		}
+		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
+			ERR_FAIL_V_MSG(Variant(), "Linear velocity is not supported for soft bodies.");
+		}
+		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
+			ERR_FAIL_V_MSG(Variant(), "Angular velocity is not supported for soft bodies.");
+		}
+		case PhysicsServer3D::BODY_STATE_SLEEPING: {
+			return is_sleeping();
+		}
+		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
+			return is_sleep_allowed();
+		}
+		default: {
+			ERR_FAIL_V_MSG(Variant(), vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state));
+		}
+	}
+}
+
+void JoltSoftBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value) {
+	switch (p_state) {
+		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
+			set_transform(p_value);
+		} break;
+		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
+			ERR_FAIL_MSG("Linear velocity is not supported for soft bodies.");
+		} break;
+		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
+			ERR_FAIL_MSG("Angular velocity is not supported for soft bodies.");
+		} break;
+		case PhysicsServer3D::BODY_STATE_SLEEPING: {
+			set_is_sleeping(p_value);
+		} break;
+		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
+			set_is_sleep_allowed(p_value);
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state));
+		} break;
+	}
+}
+
+Transform3D JoltSoftBody3D::get_transform() const {
+	// Since any transform gets baked into the vertices anyway we can just return identity here.
+	return Transform3D();
+}
+
+void JoltSoftBody3D::set_transform(const Transform3D &p_transform) {
+	ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to set transform for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	// For whatever reason this has to be interpreted as a relative global-space transform rather than an absolute one,
+	// because `SoftBody3D` will immediately upon entering the scene tree set itself to be top-level and also set its
+	// transform to be identity, while still expecting to stay in its original position.
+	//
+	// We also discard any scaling, since we have no way of scaling the actual edge lengths.
+	const JPH::Mat44 relative_transform = to_jolt(p_transform.orthonormalized());
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
+	JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+
+	for (JPH::SoftBodyVertex &vertex : physics_vertices) {
+		vertex.mPosition = vertex.mPreviousPosition = relative_transform * vertex.mPosition;
+		vertex.mVelocity = JPH::Vec3::sZero();
+	}
+	wake_up();
+}
+
+AABB JoltSoftBody3D::get_bounds() const {
+	ERR_FAIL_COND_V_MSG(!in_space(), AABB(), vformat("Failed to retrieve world bounds of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+	return to_godot(jolt_body->GetWorldSpaceBounds());
+}
+
+void JoltSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) {
+	// Ideally we would emit an actual error here, but that would spam the logs to the point where the actual cause will be drowned out.
+	if (unlikely(!in_space())) {
+		return;
+	}
+
+	const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
+
+	typedef JPH::SoftBodyMotionProperties::Vertex SoftBodyVertex;
+	typedef JPH::SoftBodyMotionProperties::Face SoftBodyFace;
+
+	const JPH::Array<SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+	const JPH::Array<SoftBodyFace> &physics_faces = motion_properties.GetFaces();
+
+	const int physics_vertex_count = (int)physics_vertices.size();
+
+	normals.clear();
+	normals.resize(physics_vertex_count);
+
+	// Compute vertex normals using smooth-shading:
+	// Each vertex should use the average normal of all faces it is a part of.
+	// Iterate over each face, and add the face normal to each of the face vertices.
+	// By the end of the loop, each vertex normal will be the sum of all face normals it belongs to.
+	for (const SoftBodyFace &physics_face : physics_faces) {
+		// Jolt uses a different winding order, so we swap the indices to account for that.
+
+		const uint32_t i0 = physics_face.mVertex[2];
+		const uint32_t i1 = physics_face.mVertex[1];
+		const uint32_t i2 = physics_face.mVertex[0];
+
+		const Vector3 v0 = to_godot(physics_vertices[i0].mPosition);
+		const Vector3 v1 = to_godot(physics_vertices[i1].mPosition);
+		const Vector3 v2 = to_godot(physics_vertices[i2].mPosition);
+
+		const Vector3 normal = (v2 - v0).cross(v1 - v0).normalized();
+
+		normals[i0] += normal;
+		normals[i1] += normal;
+		normals[i2] += normal;
+	}
+	// Normalize the vertex normals to have length 1.0
+	for (Vector3 &n : normals) {
+		real_t len = n.length();
+		// Some normals may have length 0 if the face was degenerate,
+		// so don't divide by zero.
+		if (len > CMP_EPSILON) {
+			n /= len;
+		}
+	}
+
+	const int mesh_vertex_count = shared->mesh_to_physics.size();
+
+	for (int i = 0; i < mesh_vertex_count; ++i) {
+		const int physics_index = shared->mesh_to_physics[i];
+		if (physics_index >= 0) {
+			const Vector3 vertex = to_godot(physics_vertices[(size_t)physics_index].mPosition);
+			const Vector3 normal = normals[(uint32_t)physics_index];
+
+			p_rendering_server_handler->set_vertex(i, vertex);
+			p_rendering_server_handler->set_normal(i, normal);
+		}
+	}
+
+	p_rendering_server_handler->set_aabb(get_bounds());
+}
+
+Vector3 JoltSoftBody3D::get_vertex_position(int p_index) {
+	ERR_FAIL_COND_V_MSG(!in_space(), Vector3(), vformat("Failed to retrieve point position for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	ERR_FAIL_NULL_V(shared, Vector3());
+	ERR_FAIL_INDEX_V(p_index, (int)shared->mesh_to_physics.size(), Vector3());
+	const int physics_index = shared->mesh_to_physics[p_index];
+	ERR_FAIL_COND_V_MSG(physics_index < 0, Vector3(), vformat("Soft body vertex %d was not used by a face and has been omitted for '%s'. Position cannot be returned.", p_index, to_string()));
+
+	const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
+	const JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+	const JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index];
+
+	return to_godot(jolt_body->GetCenterOfMassPosition() + physics_vertex.mPosition);
+}
+
+void JoltSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position) {
+	ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to set point position for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	ERR_FAIL_NULL(shared);
+	ERR_FAIL_INDEX(p_index, (int)shared->mesh_to_physics.size());
+	const int physics_index = shared->mesh_to_physics[p_index];
+	ERR_FAIL_COND_MSG(physics_index < 0, vformat("Soft body vertex %d was not used by a face and has been omitted for '%s'. Position cannot be set.", p_index, to_string()));
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
+	JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+	JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index];
+
+	const JPH::RVec3 center_of_mass = jolt_body->GetCenterOfMassPosition();
+	physics_vertex.mPosition = JPH::Vec3(to_jolt_r(p_position) - center_of_mass);
+
+	_vertices_changed();
+}
+
+void JoltSoftBody3D::pin_vertex(int p_index) {
+	pinned_vertices.insert(p_index);
+
+	_pins_changed();
+}
+
+void JoltSoftBody3D::unpin_vertex(int p_index) {
+	pinned_vertices.erase(p_index);
+
+	_pins_changed();
+}
+
+void JoltSoftBody3D::unpin_all_vertices() {
+	pinned_vertices.clear();
+
+	_pins_changed();
+}
+
+bool JoltSoftBody3D::is_vertex_pinned(int p_index) const {
+	ERR_FAIL_COND_V_MSG(!in_space(), false, vformat("Failed retrieve pin status of point for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	ERR_FAIL_NULL_V(shared, false);
+	ERR_FAIL_INDEX_V(p_index, (int)shared->mesh_to_physics.size(), false);
+	const int physics_index = shared->mesh_to_physics[p_index];
+
+	return pinned_vertices.has(physics_index);
+}

modules/jolt_physics/objects/jolt_soft_body_3d.h

@@ -0,0 +1,179 @@
+/**************************************************************************/
+/*  jolt_soft_body_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_object_3d.h"
+
+#include "servers/physics_server_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/SoftBody/SoftBodyCreationSettings.h"
+#include "Jolt/Physics/SoftBody/SoftBodySharedSettings.h"
+
+class JoltSpace3D;
+
+class JoltSoftBody3D final : public JoltObject3D {
+	struct Shared {
+		LocalVector<int> mesh_to_physics;
+		JPH::Ref<JPH::SoftBodySharedSettings> settings = new JPH::SoftBodySharedSettings();
+		int ref_count = 1;
+	};
+
+	inline static HashMap<RID, Shared> mesh_to_shared;
+
+	HashSet<int> pinned_vertices;
+	LocalVector<RID> exceptions;
+	LocalVector<Vector3> normals;
+
+	const Shared *shared = nullptr;
+
+	RID mesh;
+
+	JPH::SoftBodyCreationSettings *jolt_settings = new JPH::SoftBodyCreationSettings();
+
+	float mass = 0.0f;
+	float pressure = 0.0f;
+	float linear_damping = 0.01f;
+	float stiffness_coefficient = 0.5f;
+	float shrinking_factor = 0.0f;
+
+	int simulation_precision = 5;
+
+	virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const override;
+	virtual JPH::ObjectLayer _get_object_layer() const override;
+
+	virtual void _space_changing() override;
+	virtual void _space_changed() override;
+
+	virtual void _add_to_space() override;
+
+	bool _ref_shared_data();
+	void _deref_shared_data();
+
+	void _update_mass();
+	void _update_pressure();
+	void _update_damping();
+	void _update_simulation_precision();
+	void _update_group_filter();
+
+	void _try_rebuild();
+
+	void _mesh_changed();
+	void _simulation_precision_changed();
+	void _mass_changed();
+	void _pressure_changed();
+	void _damping_changed();
+	void _pins_changed();
+	void _vertices_changed();
+	void _exceptions_changed();
+	void _motion_changed();
+
+public:
+	JoltSoftBody3D();
+	virtual ~JoltSoftBody3D() override;
+
+	bool in_space() const;
+
+	void add_collision_exception(const RID &p_excepted_body);
+	void remove_collision_exception(const RID &p_excepted_body);
+	bool has_collision_exception(const RID &p_excepted_body) const;
+
+	const LocalVector<RID> &get_collision_exceptions() const { return exceptions; }
+
+	virtual bool can_interact_with(const JoltBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltSoftBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltArea3D &p_other) const override;
+
+	virtual bool reports_contacts() const override { return false; }
+
+	virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const override;
+
+	void set_mesh(const RID &p_mesh);
+
+	bool is_pickable() const { return pickable; }
+	void set_pickable(bool p_enabled) { pickable = p_enabled; }
+
+	bool is_sleeping() const;
+	void set_is_sleeping(bool p_enabled);
+
+	bool is_sleep_allowed() const;
+	void set_is_sleep_allowed(bool p_enabled);
+
+	void put_to_sleep() { set_is_sleeping(true); }
+	void wake_up() { set_is_sleeping(false); }
+
+	int get_simulation_precision() const { return simulation_precision; }
+	void set_simulation_precision(int p_precision);
+
+	float get_mass() const { return mass; }
+	void set_mass(float p_mass);
+
+	float get_stiffness_coefficient() const;
+	void set_stiffness_coefficient(float p_coefficient);
+
+	float get_shrinking_factor() const;
+	void set_shrinking_factor(float p_shrinking_factor);
+
+	float get_pressure() const { return pressure; }
+	void set_pressure(float p_pressure);
+
+	float get_linear_damping() const { return linear_damping; }
+	void set_linear_damping(float p_damping);
+
+	float get_drag() const;
+	void set_drag(float p_drag);
+
+	Variant get_state(PhysicsServer3D::BodyState p_state) const;
+	void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value);
+
+	Transform3D get_transform() const;
+	void set_transform(const Transform3D &p_transform);
+
+	AABB get_bounds() const;
+
+	void update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler);
+
+	Vector3 get_vertex_position(int p_index);
+	void set_vertex_position(int p_index, const Vector3 &p_position);
+
+	void pin_vertex(int p_index);
+	void unpin_vertex(int p_index);
+
+	void unpin_all_vertices();
+
+	bool is_vertex_pinned(int p_index) const;
+
+	void apply_vertex_impulse(int p_index, const Vector3 &p_impulse);
+	void apply_vertex_force(int p_index, const Vector3 &p_force);
+	void apply_central_impulse(const Vector3 &p_impulse);
+	void apply_central_force(const Vector3 &p_force);
+};