Merge pull request #105748 from mihe/jolt/body-pointer

Remove no-op locking in Jolt Physics module

modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp

@@ -348,21 +348,12 @@ void JoltConeTwistJoint3D::rebuild() {
 	destroy();
 
 	JoltSpace3D *space = get_space();
-
 	if (space == nullptr) {
 		return;
 	}
 
-	const JPH::BodyID body_ids[2] = {
+	JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr;
-		body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
+	JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr;
-		body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
-	};
-
-	const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
-
-	JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
-	JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
-
 	ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
 
 	Transform3D shifted_ref_a;

modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp

@@ -656,16 +656,8 @@ void JoltGeneric6DOFJoint3D::rebuild() {
 		return;
 	}
 
-	const JPH::BodyID body_ids[2] = {
+	JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr;
-		body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
+	JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr;
-		body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
-	};
-
-	const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
-
-	JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
-	JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
-
 	ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
 
 	Transform3D shifted_ref_a;

modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp

@@ -381,21 +381,12 @@ void JoltHingeJoint3D::rebuild() {
 	destroy();
 
 	JoltSpace3D *space = get_space();
-
 	if (space == nullptr) {
 		return;
 	}
 
-	const JPH::BodyID body_ids[2] = {
+	JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr;
-		body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
+	JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr;
-		body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
-	};
-
-	const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
-
-	JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
-	JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
-
 	ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
 
 	float ref_shift = 0.0f;

modules/jolt_physics/joints/jolt_pin_joint_3d.cpp

@@ -138,21 +138,12 @@ void JoltPinJoint3D::rebuild() {
 	destroy();
 
 	JoltSpace3D *space = get_space();
-
 	if (space == nullptr) {
 		return;
 	}
 
-	const JPH::BodyID body_ids[2] = {
+	JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr;
-		body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
+	JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr;
-		body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
-	};
-
-	const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
-
-	JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
-	JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
-
 	ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
 
 	Transform3D shifted_ref_a;

modules/jolt_physics/joints/jolt_slider_joint_3d.cpp

@@ -494,21 +494,12 @@ void JoltSliderJoint3D::rebuild() {
 	destroy();
 
 	JoltSpace3D *space = get_space();
-
 	if (space == nullptr) {
 		return;
 	}
 
-	const JPH::BodyID body_ids[2] = {
+	JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr;
-		body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
+	JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr;
-		body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
-	};
-
-	const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
-
-	JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
-	JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
-
 	ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
 
 	float ref_shift = 0.0f;

modules/jolt_physics/objects/jolt_area_3d.cpp

@@ -83,12 +83,12 @@ void JoltArea3D::_add_to_space() {
 
 	jolt_settings->SetShape(jolt_shape);
 
-	const JPH::BodyID new_jolt_id = space->add_rigid_body(*this, *jolt_settings);
+	JPH::Body *new_jolt_body = space->add_rigid_body(*this, *jolt_settings);
-	if (new_jolt_id.IsInvalid()) {
+	if (new_jolt_body == nullptr) {
 		return;
 	}
 
-	jolt_id = new_jolt_id;
+	jolt_body = new_jolt_body;
 
 	delete jolt_settings;
 	jolt_settings = nullptr;
@@ -107,8 +107,7 @@ void JoltArea3D::_dequeue_call_queries() {
 }
 
 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 JoltReadableBody3D other_jolt_body = space->read_body(p_body_id);
+	const JoltShapedObject3D *other_object = space->try_get_shaped(p_body_id);
-	const JoltShapedObject3D *other_object = other_jolt_body.as_shaped();
 	ERR_FAIL_NULL(other_object);
 
 	p_overlap.rid = other_object->get_rid();
@@ -187,25 +186,15 @@ void JoltArea3D::_report_event(const Callable &p_callback, PhysicsServer3D::Area
 }
 
 void JoltArea3D::_notify_body_entered(const JPH::BodyID &p_body_id) {
-	const JoltReadableBody3D jolt_body = space->read_body(p_body_id);
+	if (JoltBody3D *other_body = space->try_get_body(p_body_id)) {
-
+		other_body->add_area(this);
-	JoltBody3D *body = jolt_body.as_body();
-	if (unlikely(body == nullptr)) {
-		return;
 	}
-
-	body->add_area(this);
 }
 
 void JoltArea3D::_notify_body_exited(const JPH::BodyID &p_body_id) {
-	const JoltReadableBody3D jolt_body = space->read_body(p_body_id);
+	if (JoltBody3D *other_body = space->try_get_body(p_body_id)) {
-
+		other_body->remove_area(this);
-	JoltBody3D *body = jolt_body.as_body();
-	if (unlikely(body == nullptr)) {
-		return;
 	}
-
-	body->remove_area(this);
 }
 
 void JoltArea3D::_force_bodies_entered() {
@@ -291,10 +280,7 @@ void JoltArea3D::_update_group_filter() {
 		return;
 	}
 
-	const JoltWritableBody3D body = space->write_body(jolt_id);
+	jolt_body->GetCollisionGroup().SetGroupFilter(JoltGroupFilter::instance);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->GetCollisionGroup().SetGroupFilter(JoltGroupFilter::instance);
 }
 
 void JoltArea3D::_update_default_gravity() {
@@ -384,7 +370,7 @@ void JoltArea3D::set_transform(Transform3D p_transform) {
 		jolt_settings->mPosition = to_jolt_r(p_transform.origin);
 		jolt_settings->mRotation = to_jolt(p_transform.basis);
 	} else {
-		space->get_body_iface().SetPositionAndRotation(jolt_id, to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate);
+		space->get_body_iface().SetPositionAndRotation(jolt_body->GetID(), to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate);
 	}
 }
 

modules/jolt_physics/objects/jolt_body_3d.cpp

@@ -144,12 +144,12 @@ void JoltBody3D::_add_to_space() {
 
 	jolt_settings->SetShape(jolt_shape);
 
-	const JPH::BodyID new_jolt_id = space->add_rigid_body(*this, *jolt_settings, sleep_initially);
+	JPH::Body *new_jolt_body = space->add_rigid_body(*this, *jolt_settings, sleep_initially);
-	if (new_jolt_id.IsInvalid()) {
+	if (new_jolt_body == nullptr) {
 		return;
 	}
 
-	jolt_id = new_jolt_id;
+	jolt_body = new_jolt_body;
 
 	delete jolt_settings;
 	jolt_settings = nullptr;
@@ -295,14 +295,9 @@ JPH::MassProperties JoltBody3D::_calculate_mass_properties() const {
 }
 
 void JoltBody3D::_update_mass_properties() {
-	if (!in_space()) {
+	if (in_space()) {
-		return;
+		jolt_body->GetMotionPropertiesUnchecked()->SetMassProperties(_calculate_allowed_dofs(), _calculate_mass_properties());
 	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->GetMotionPropertiesUnchecked()->SetMassProperties(_calculate_allowed_dofs(), _calculate_mass_properties());
 }
 
 void JoltBody3D::_update_gravity(JPH::Body &p_jolt_body) {
@@ -407,10 +402,7 @@ void JoltBody3D::_update_possible_kinematic_contacts() {
 	if (!in_space()) {
 		jolt_settings->mCollideKinematicVsNonDynamic = value;
 	} else {
-		const JoltWritableBody3D body = space->write_body(jolt_id);
+		jolt_body->SetCollideKinematicVsNonDynamic(value);
-		ERR_FAIL_COND(body.is_invalid());
-
-		body->SetCollideKinematicVsNonDynamic(value);
 	}
 }
 
@@ -419,13 +411,9 @@ void JoltBody3D::_update_sleep_allowed() {
 
 	if (!in_space()) {
 		jolt_settings->mAllowSleeping = value;
-		return;
+	} else {
+		jolt_body->SetAllowSleeping(value);
 	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->SetAllowSleeping(value);
 }
 
 void JoltBody3D::_destroy_joint_constraints() {
@@ -435,8 +423,10 @@ void JoltBody3D::_destroy_joint_constraints() {
 }
 
 void JoltBody3D::_exit_all_areas() {
+	ERR_FAIL_COND(!in_space());
+
 	for (JoltArea3D *area : areas) {
-		area->body_exited(jolt_id, false);
+		area->body_exited(jolt_body->GetID(), false);
 	}
 
 	areas.clear();
@@ -447,13 +437,9 @@ void JoltBody3D::_update_group_filter() {
 
 	if (!in_space()) {
 		jolt_settings->mCollisionGroup.SetGroupFilter(group_filter);
-		return;
+	} else {
+		jolt_body->GetCollisionGroup().SetGroupFilter(group_filter);
 	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->GetCollisionGroup().SetGroupFilter(group_filter);
 }
 
 void JoltBody3D::_mode_changed() {
@@ -559,7 +545,7 @@ void JoltBody3D::set_transform(Transform3D p_transform) {
 	} else if (is_kinematic()) {
 		kinematic_transform = p_transform;
 	} else {
-		space->get_body_iface().SetPositionAndRotation(jolt_id, to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate);
+		space->get_body_iface().SetPositionAndRotation(jolt_body->GetID(), to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate);
 	}
 
 	_transform_changed();
@@ -694,29 +680,20 @@ void JoltBody3D::set_custom_integrator(bool p_enabled) {
 
 	custom_integrator = p_enabled;
 
-	if (!in_space()) {
+	if (in_space()) {
-		_motion_changed();
+		jolt_body->ResetForce();
-		return;
+		jolt_body->ResetTorque();
 	}
 
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->ResetForce();
-	body->ResetTorque();
-
 	_motion_changed();
 }
 
 bool JoltBody3D::is_sleeping() const {
 	if (!in_space()) {
 		return sleep_initially;
+	} else {
+		return !jolt_body->IsActive();
 	}
-
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), false);
-
-	return !body->IsActive();
 }
 
 bool JoltBody3D::is_sleep_actually_allowed() const {
@@ -726,15 +703,12 @@ bool JoltBody3D::is_sleep_actually_allowed() const {
 void JoltBody3D::set_is_sleeping(bool p_enabled) {
 	if (!in_space()) {
 		sleep_initially = p_enabled;
-		return;
-	}
-
-	JPH::BodyInterface &body_iface = space->get_body_iface();
-
-	if (p_enabled) {
-		body_iface.DeactivateBody(jolt_id);
 	} else {
-		body_iface.ActivateBody(jolt_id);
+		if (p_enabled) {
+			space->get_body_iface().DeactivateBody(jolt_body->GetID());
+		} else {
+			space->get_body_iface().ActivateBody(jolt_body->GetID());
+		}
 	}
 }
 
@@ -749,85 +723,60 @@ void JoltBody3D::set_is_sleep_allowed(bool p_enabled) {
 }
 
 Basis JoltBody3D::get_principal_inertia_axes() const {
-	ERR_FAIL_NULL_V_MSG(space, Basis(), vformat("Failed to retrieve principal inertia axes 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()));
+	ERR_FAIL_COND_V_MSG(!in_space(), Basis(), vformat("Failed to retrieve principal inertia axes 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()));
 
 	if (unlikely(is_static() || is_kinematic())) {
 		return Basis();
 	}
 
-	const JoltReadableBody3D body = space->read_body(jolt_id);
+	return to_godot(jolt_body->GetRotation() * jolt_body->GetMotionPropertiesUnchecked()->GetInertiaRotation());
-	ERR_FAIL_COND_V(body.is_invalid(), Basis());
-
-	return to_godot(body->GetRotation() * body->GetMotionProperties()->GetInertiaRotation());
 }
 
 Vector3 JoltBody3D::get_inverse_inertia() const {
-	ERR_FAIL_NULL_V_MSG(space, Vector3(), vformat("Failed to retrieve inverse inertia 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()));
+	ERR_FAIL_COND_V_MSG(!in_space(), Vector3(), vformat("Failed to retrieve inverse inertia 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()));
 
 	if (unlikely(is_static() || is_kinematic())) {
 		return Vector3();
 	}
 
-	const JoltReadableBody3D body = space->read_body(jolt_id);
+	return to_godot(jolt_body->GetMotionPropertiesUnchecked()->GetLocalSpaceInverseInertia().GetDiagonal3());
-	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
-
-	const JPH::MotionProperties &motion_properties = *body->GetMotionPropertiesUnchecked();
-
-	return to_godot(motion_properties.GetLocalSpaceInverseInertia().GetDiagonal3());
 }
 
 Basis JoltBody3D::get_inverse_inertia_tensor() const {
-	ERR_FAIL_NULL_V_MSG(space, Basis(), vformat("Failed to retrieve inverse inertia tensor 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()));
+	ERR_FAIL_COND_V_MSG(!in_space(), Basis(), vformat("Failed to retrieve inverse inertia tensor 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()));
 
 	if (unlikely(is_static() || is_kinematic())) {
 		return Basis();
 	}
 
-	const JoltReadableBody3D body = space->read_body(jolt_id);
+	return to_godot(jolt_body->GetInverseInertia()).basis;
-	ERR_FAIL_COND_V(body.is_invalid(), Basis());
-
-	return to_godot(body->GetInverseInertia()).basis;
 }
 
 void JoltBody3D::set_linear_velocity(const Vector3 &p_velocity) {
 	if (is_static() || is_kinematic()) {
 		linear_surface_velocity = p_velocity;
-		_motion_changed();
+	} else {
-		return;
+		if (!in_space()) {
+			jolt_settings->mLinearVelocity = to_jolt(p_velocity);
+		} else {
+			jolt_body->GetMotionPropertiesUnchecked()->SetLinearVelocityClamped(to_jolt(p_velocity));
+		}
 	}
 
-	if (!in_space()) {
-		jolt_settings->mLinearVelocity = to_jolt(p_velocity);
-		_motion_changed();
-		return;
-	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->GetMotionPropertiesUnchecked()->SetLinearVelocityClamped(to_jolt(p_velocity));
-
 	_motion_changed();
 }
 
 void JoltBody3D::set_angular_velocity(const Vector3 &p_velocity) {
 	if (is_static() || is_kinematic()) {
 		angular_surface_velocity = p_velocity;
-		_motion_changed();
+	} else {
-		return;
+		if (!in_space()) {
+			jolt_settings->mAngularVelocity = to_jolt(p_velocity);
+		} else {
+			jolt_body->GetMotionPropertiesUnchecked()->SetAngularVelocityClamped(to_jolt(p_velocity));
+		}
 	}
 
-	if (!in_space()) {
-		jolt_settings->mAngularVelocity = to_jolt(p_velocity);
-		_motion_changed();
-		return;
-	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->GetMotionPropertiesUnchecked()->SetAngularVelocityClamped(to_jolt(p_velocity));
-
 	_motion_changed();
 }
 
@@ -840,9 +789,6 @@ void JoltBody3D::set_axis_velocity(const Vector3 &p_axis_velocity) {
 		linear_velocity += p_axis_velocity;
 		jolt_settings->mLinearVelocity = to_jolt(linear_velocity);
 	} else {
-		const JoltWritableBody3D body = space->write_body(jolt_id);
-		ERR_FAIL_COND(body.is_invalid());
-
 		Vector3 linear_velocity = get_linear_velocity();
 		linear_velocity -= axis * axis.dot(linear_velocity);
 		linear_velocity += p_axis_velocity;
@@ -857,14 +803,10 @@ Vector3 JoltBody3D::get_velocity_at_position(const Vector3 &p_position) const {
 		return Vector3();
 	}
 
-	const JoltReadableBody3D body = space->read_body(jolt_id);
+	const JPH::MotionProperties &motion_properties = *jolt_body->GetMotionPropertiesUnchecked();
-	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
-
-	const JPH::MotionProperties &motion_properties = *body->GetMotionPropertiesUnchecked();
-
 	const Vector3 total_linear_velocity = to_godot(motion_properties.GetLinearVelocity()) + linear_surface_velocity;
 	const Vector3 total_angular_velocity = to_godot(motion_properties.GetAngularVelocity()) + angular_surface_velocity;
-	const Vector3 com_to_pos = p_position - to_godot(body->GetCenterOfMassPosition());
+	const Vector3 com_to_pos = p_position - to_godot(jolt_body->GetCenterOfMassPosition());
 
 	return total_linear_velocity + total_angular_velocity.cross(com_to_pos);
 }
@@ -894,14 +836,10 @@ void JoltBody3D::set_max_contacts_reported(int p_count) {
 
 	if (!in_space()) {
 		jolt_settings->mUseManifoldReduction = use_manifold_reduction;
-		_contact_reporting_changed();
+	} else {
-		return;
+		space->get_body_iface().SetUseManifoldReduction(jolt_body->GetID(), use_manifold_reduction);
 	}
 
-	JPH::BodyInterface &body_iface = space->get_body_iface();
-
-	body_iface.SetUseManifoldReduction(jolt_id, use_manifold_reduction);
-
 	_contact_reporting_changed();
 }
 
@@ -963,115 +901,73 @@ void JoltBody3D::reset_mass_properties() {
 }
 
 void JoltBody3D::apply_force(const Vector3 &p_force, const Vector3 &p_position) {
-	ERR_FAIL_NULL_MSG(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()));
+	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()));
 
-	if (unlikely(!is_rigid())) {
+	if (unlikely(!is_rigid()) || custom_integrator || p_force == Vector3()) {
 		return;
 	}
 
-	if (custom_integrator || p_force == Vector3()) {
+	jolt_body->AddForce(to_jolt(p_force), jolt_body->GetPosition() + to_jolt(p_position));
-		return;
-	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->AddForce(to_jolt(p_force), body->GetPosition() + to_jolt(p_position));
 
 	_motion_changed();
 }
 
 void JoltBody3D::apply_central_force(const Vector3 &p_force) {
-	ERR_FAIL_NULL_MSG(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()));
+	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()));
 
-	if (unlikely(!is_rigid())) {
+	if (unlikely(!is_rigid()) || custom_integrator || p_force == Vector3()) {
 		return;
 	}
 
-	if (custom_integrator || p_force == Vector3()) {
+	jolt_body->AddForce(to_jolt(p_force));
-		return;
-	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->AddForce(to_jolt(p_force));
 
 	_motion_changed();
 }
 
 void JoltBody3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
-	ERR_FAIL_NULL_MSG(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_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()));
 
-	if (unlikely(!is_rigid())) {
+	if (unlikely(!is_rigid()) || p_impulse == Vector3()) {
 		return;
 	}
 
-	if (p_impulse == Vector3()) {
+	jolt_body->AddImpulse(to_jolt(p_impulse), jolt_body->GetPosition() + to_jolt(p_position));
-		return;
-	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->AddImpulse(to_jolt(p_impulse), body->GetPosition() + to_jolt(p_position));
 
 	_motion_changed();
 }
 
 void JoltBody3D::apply_central_impulse(const Vector3 &p_impulse) {
-	ERR_FAIL_NULL_MSG(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()));
+	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()));
 
-	if (unlikely(!is_rigid())) {
+	if (unlikely(!is_rigid()) || p_impulse == Vector3()) {
 		return;
 	}
 
-	if (p_impulse == Vector3()) {
+	jolt_body->AddImpulse(to_jolt(p_impulse));
-		return;
-	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->AddImpulse(to_jolt(p_impulse));
 
 	_motion_changed();
 }
 
 void JoltBody3D::apply_torque(const Vector3 &p_torque) {
-	ERR_FAIL_NULL_MSG(space, vformat("Failed to apply torque 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_COND_MSG(!in_space(), vformat("Failed to apply torque 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()));
 
-	if (unlikely(!is_rigid())) {
+	if (unlikely(!is_rigid()) || custom_integrator || p_torque == Vector3()) {
 		return;
 	}
 
-	if (custom_integrator || p_torque == Vector3()) {
+	jolt_body->AddTorque(to_jolt(p_torque));
-		return;
-	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->AddTorque(to_jolt(p_torque));
 
 	_motion_changed();
 }
 
 void JoltBody3D::apply_torque_impulse(const Vector3 &p_impulse) {
-	ERR_FAIL_NULL_MSG(space, vformat("Failed to apply torque 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_COND_MSG(!in_space(), vformat("Failed to apply torque 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()));
 
-	if (unlikely(!is_rigid())) {
+	if (unlikely(!is_rigid()) || p_impulse == Vector3()) {
 		return;
 	}
 
-	if (p_impulse == Vector3()) {
+	jolt_body->AddAngularImpulse(to_jolt(p_impulse));
-		return;
-	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->AddAngularImpulse(to_jolt(p_impulse));
 
 	_motion_changed();
 }
@@ -1091,9 +987,6 @@ void JoltBody3D::add_constant_force(const Vector3 &p_force, const Vector3 &p_pos
 		return;
 	}
 
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
 	constant_force += p_force;
 	constant_torque += (p_position - get_center_of_mass_relative()).cross(p_force);
 
@@ -1260,22 +1153,19 @@ void JoltBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
 
 	const JPH::EMotionType motion_type = _get_motion_type();
 
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
 	if (motion_type == JPH::EMotionType::Static) {
 		put_to_sleep();
 	}
 
-	body->SetMotionType(motion_type);
+	jolt_body->SetMotionType(motion_type);
 
 	if (motion_type != JPH::EMotionType::Static) {
 		wake_up();
 	}
 
 	if (motion_type == JPH::EMotionType::Kinematic) {
-		body->SetLinearVelocity(JPH::Vec3::sZero());
+		jolt_body->SetLinearVelocity(JPH::Vec3::sZero());
-		body->SetAngularVelocity(JPH::Vec3::sZero());
+		jolt_body->SetAngularVelocity(JPH::Vec3::sZero());
 	}
 
 	linear_surface_velocity = Vector3();
@@ -1287,11 +1177,9 @@ void JoltBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
 bool JoltBody3D::is_ccd_enabled() const {
 	if (!in_space()) {
 		return jolt_settings->mMotionQuality == JPH::EMotionQuality::LinearCast;
+	} else {
+		return space->get_body_iface().GetMotionQuality(jolt_body->GetID()) == JPH::EMotionQuality::LinearCast;
 	}
-
-	const JPH::BodyInterface &body_iface = space->get_body_iface();
-
-	return body_iface.GetMotionQuality(jolt_id) == JPH::EMotionQuality::LinearCast;
 }
 
 void JoltBody3D::set_ccd_enabled(bool p_enabled) {
@@ -1299,12 +1187,9 @@ void JoltBody3D::set_ccd_enabled(bool p_enabled) {
 
 	if (!in_space()) {
 		jolt_settings->mMotionQuality = motion_quality;
-		return;
+	} else {
+		space->get_body_iface().SetMotionQuality(jolt_body->GetID(), motion_quality);
 	}
-
-	JPH::BodyInterface &body_iface = space->get_body_iface();
-
-	body_iface.SetMotionQuality(jolt_id, motion_quality);
 }
 
 void JoltBody3D::set_mass(float p_mass) {
@@ -1324,47 +1209,33 @@ void JoltBody3D::set_inertia(const Vector3 &p_inertia) {
 float JoltBody3D::get_bounce() const {
 	if (!in_space()) {
 		return jolt_settings->mRestitution;
+	} else {
+		return jolt_body->GetRestitution();
 	}
-
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), 0.0f);
-
-	return body->GetRestitution();
 }
 
 void JoltBody3D::set_bounce(float p_bounce) {
 	if (!in_space()) {
 		jolt_settings->mRestitution = p_bounce;
-		return;
+	} else {
+		jolt_body->SetRestitution(p_bounce);
 	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->SetRestitution(p_bounce);
 }
 
 float JoltBody3D::get_friction() const {
 	if (!in_space()) {
 		return jolt_settings->mFriction;
+	} else {
+		return jolt_body->GetFriction();
 	}
-
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), 0.0f);
-
-	return body->GetFriction();
 }
 
 void JoltBody3D::set_friction(float p_friction) {
 	if (!in_space()) {
 		jolt_settings->mFriction = p_friction;
-		return;
+	} else {
+		jolt_body->SetFriction(p_friction);
 	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->SetFriction(p_friction);
 }
 
 void JoltBody3D::set_gravity_scale(float p_scale) {

modules/jolt_physics/objects/jolt_object_3d.cpp

@@ -37,13 +37,12 @@
 #include "jolt_group_filter.h"
 
 void JoltObject3D::_remove_from_space() {
-	if (unlikely(jolt_id.IsInvalid())) {
+	if (!in_space()) {
 		return;
 	}
 
-	space->remove_body(jolt_id);
+	space->remove_body(jolt_body->GetID());
-
+	jolt_body = nullptr;
-	jolt_id = JPH::BodyID();
 }
 
 void JoltObject3D::_reset_space() {
@@ -60,7 +59,7 @@ void JoltObject3D::_update_object_layer() {
 		return;
 	}
 
-	space->get_body_iface().SetObjectLayer(jolt_id, _get_object_layer());
+	space->get_body_iface().SetObjectLayer(jolt_body->GetID(), _get_object_layer());
 }
 
 void JoltObject3D::_collision_layer_changed() {

modules/jolt_physics/objects/jolt_object_3d.h

@@ -66,7 +66,7 @@ protected:
 	RID rid;
 	ObjectID instance_id;
 	JoltSpace3D *space = nullptr;
-	JPH::BodyID jolt_id;
+	JPH::Body *jolt_body = nullptr;
 
 	uint32_t collision_layer = 1;
 	uint32_t collision_mask = 1;
@@ -121,11 +121,12 @@ public:
 	void set_instance_id(ObjectID p_id) { instance_id = p_id; }
 	Object *get_instance() const;
 
-	JPH::BodyID get_jolt_id() const { return jolt_id; }
+	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_id.IsInvalid(); }
+	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);

modules/jolt_physics/objects/jolt_shaped_object_3d.cpp

@@ -178,11 +178,8 @@ void JoltShapedObject3D::_space_changing() {
 
 	previous_jolt_shape = nullptr;
 
-	if (space != nullptr) {
+	if (in_space()) {
-		const JoltWritableBody3D body = space->write_body(jolt_id);
+		jolt_settings = new JPH::BodyCreationSettings(jolt_body->GetBodyCreationSettings());
-		ERR_FAIL_COND(body.is_invalid());
-
-		jolt_settings = new JPH::BodyCreationSettings(body->GetBodyCreationSettings());
 	}
 }
 
@@ -208,12 +205,9 @@ JoltShapedObject3D::~JoltShapedObject3D() {
 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()));
 	}
-
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), Transform3D());
-
-	return Transform3D(to_godot(body->GetRotation()), to_godot(body->GetPosition()));
 }
 
 Transform3D JoltShapedObject3D::get_transform_scaled() const {
@@ -223,32 +217,22 @@ Transform3D JoltShapedObject3D::get_transform_scaled() const {
 Basis JoltShapedObject3D::get_basis() const {
 	if (!in_space()) {
 		return to_godot(jolt_settings->mRotation);
+	} else {
+		return to_godot(jolt_body->GetRotation());
 	}
-
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), Basis());
-
-	return to_godot(body->GetRotation());
 }
 
 Vector3 JoltShapedObject3D::get_position() const {
 	if (!in_space()) {
 		return to_godot(jolt_settings->mPosition);
+	} else {
+		return to_godot(jolt_body->GetPosition());
 	}
-
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
-
-	return to_godot(body->GetPosition());
 }
 
 Vector3 JoltShapedObject3D::get_center_of_mass() const {
-	ERR_FAIL_NULL_V_MSG(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()));
+	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());
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
-
-	return to_godot(body->GetCenterOfMassPosition());
 }
 
 Vector3 JoltShapedObject3D::get_center_of_mass_relative() const {
@@ -264,23 +248,17 @@ Vector3 JoltShapedObject3D::get_center_of_mass_local() const {
 Vector3 JoltShapedObject3D::get_linear_velocity() const {
 	if (!in_space()) {
 		return to_godot(jolt_settings->mLinearVelocity);
+	} else {
+		return to_godot(jolt_body->GetLinearVelocity());
 	}
-
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
-
-	return to_godot(body->GetLinearVelocity());
 }
 
 Vector3 JoltShapedObject3D::get_angular_velocity() const {
 	if (!in_space()) {
 		return to_godot(jolt_settings->mAngularVelocity);
+	} else {
+		return to_godot(jolt_body->GetAngularVelocity());
 	}
-
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
-
-	return to_godot(body->GetAngularVelocity());
 }
 
 AABB JoltShapedObject3D::get_aabb() const {
@@ -321,9 +299,6 @@ void JoltShapedObject3D::commit_shapes(bool p_optimize_compound) {
 		return;
 	}
 
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
 	JPH::ShapeRefC new_shape = build_shapes(p_optimize_compound);
 	if (new_shape == jolt_shape) {
 		return;
@@ -332,7 +307,7 @@ void JoltShapedObject3D::commit_shapes(bool p_optimize_compound) {
 	previous_jolt_shape = jolt_shape;
 	jolt_shape = new_shape;
 
-	space->get_body_iface().SetShape(jolt_id, jolt_shape, false, JPH::EActivation::DontActivate);
+	space->get_body_iface().SetShape(jolt_body->GetID(), jolt_shape, false, JPH::EActivation::DontActivate);
 
 	_enqueue_shapes_changed();
 

modules/jolt_physics/objects/jolt_soft_body_3d.cpp

@@ -78,15 +78,12 @@ JPH::ObjectLayer JoltSoftBody3D::_get_object_layer() const {
 void JoltSoftBody3D::_space_changing() {
 	JoltObject3D::_space_changing();
 
-	_deref_shared_data();
+	if (in_space()) {
-
+		jolt_settings = new JPH::SoftBodyCreationSettings(jolt_body->GetSoftBodyCreationSettings());
-	if (space != nullptr && !jolt_id.IsInvalid()) {
-		const JoltReadableBody3D body = space->read_body(jolt_id);
-		ERR_FAIL_COND(body.is_invalid());
-
-		jolt_settings = new JPH::SoftBodyCreationSettings(body->GetSoftBodyCreationSettings());
 		jolt_settings->mSettings = nullptr;
 	}
+
+	_deref_shared_data();
 }
 
 void JoltSoftBody3D::_space_changed() {
@@ -117,12 +114,12 @@ void JoltSoftBody3D::_add_to_space() {
 	jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id);
 	jolt_settings->mMaxLinearVelocity = JoltProjectSettings::max_linear_velocity;
 
-	const JPH::BodyID new_jolt_id = space->add_soft_body(*this, *jolt_settings);
+	JPH::Body *new_jolt_body = space->add_soft_body(*this, *jolt_settings);
-	if (new_jolt_id.IsInvalid()) {
+	if (new_jolt_body == nullptr) {
 		return;
 	}
 
-	jolt_id = new_jolt_id;
+	jolt_body = new_jolt_body;
 
 	delete jolt_settings;
 	jolt_settings = nullptr;
@@ -237,11 +234,7 @@ void JoltSoftBody3D::_update_mass() {
 		return;
 	}
 
-	JoltWritableBody3D body = space->write_body(jolt_id);
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
-	ERR_FAIL_COND(body.is_invalid());
-
-	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*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;
@@ -259,11 +252,7 @@ void JoltSoftBody3D::_update_pressure() {
 		return;
 	}
 
-	JoltWritableBody3D body = space->write_body(jolt_id);
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
-	ERR_FAIL_COND(body.is_invalid());
-
-	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
-
 	motion_properties.SetPressure(pressure);
 }
 
@@ -273,11 +262,7 @@ void JoltSoftBody3D::_update_damping() {
 		return;
 	}
 
-	JoltWritableBody3D body = space->write_body(jolt_id);
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
-	ERR_FAIL_COND(body.is_invalid());
-
-	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
-
 	motion_properties.SetLinearDamping(linear_damping);
 }
 
@@ -287,11 +272,7 @@ void JoltSoftBody3D::_update_simulation_precision() {
 		return;
 	}
 
-	JoltWritableBody3D body = space->write_body(jolt_id);
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
-	ERR_FAIL_COND(body.is_invalid());
-
-	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
-
 	motion_properties.SetNumIterations((JPH::uint32)simulation_precision);
 }
 
@@ -300,13 +281,9 @@ void JoltSoftBody3D::_update_group_filter() {
 
 	if (!in_space()) {
 		jolt_settings->mCollisionGroup.SetGroupFilter(group_filter);
-		return;
+	} else {
+		jolt_body->GetCollisionGroup().SetGroupFilter(group_filter);
 	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->GetCollisionGroup().SetGroupFilter(group_filter);
 }
 
 void JoltSoftBody3D::_try_rebuild() {
@@ -407,21 +384,16 @@ void JoltSoftBody3D::set_mesh(const RID &p_mesh) {
 	}
 
 	_deref_shared_data();
-
 	mesh = p_mesh;
-
 	_mesh_changed();
 }
 
 bool JoltSoftBody3D::is_sleeping() const {
 	if (!in_space()) {
 		return false;
+	} else {
+		return !jolt_body->IsActive();
 	}
-
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), false);
-
-	return !body->IsActive();
 }
 
 void JoltSoftBody3D::set_is_sleeping(bool p_enabled) {
@@ -432,32 +404,26 @@ void JoltSoftBody3D::set_is_sleeping(bool p_enabled) {
 	JPH::BodyInterface &body_iface = space->get_body_iface();
 
 	if (p_enabled) {
-		body_iface.DeactivateBody(jolt_id);
+		body_iface.DeactivateBody(jolt_body->GetID());
 	} else {
-		body_iface.ActivateBody(jolt_id);
+		body_iface.ActivateBody(jolt_body->GetID());
 	}
 }
 
 bool JoltSoftBody3D::is_sleep_allowed() const {
 	if (!in_space()) {
-		return true;
+		return jolt_settings->mAllowSleeping;
+	} else {
+		return jolt_body->GetAllowSleeping();
 	}
-
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), false);
-
-	return body->GetAllowSleeping();
 }
 
 void JoltSoftBody3D::set_is_sleep_allowed(bool p_enabled) {
 	if (!in_space()) {
-		return;
+		jolt_settings->mAllowSleeping = p_enabled;
+	} else {
+		jolt_body->SetAllowSleeping(p_enabled);
 	}
-
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
-	body->SetAllowSleeping(p_enabled);
 }
 
 void JoltSoftBody3D::set_simulation_precision(int p_precision) {
@@ -571,9 +537,6 @@ Transform3D JoltSoftBody3D::get_transform() const {
 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()));
 
-	JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
-
 	// 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.
@@ -581,7 +544,7 @@ void JoltSoftBody3D::set_transform(const Transform3D &p_transform) {
 	// 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 &>(*body->GetMotionPropertiesUnchecked());
+	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) {
@@ -592,11 +555,7 @@ void JoltSoftBody3D::set_transform(const Transform3D &p_transform) {
 
 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());
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), AABB());
-
-	return to_godot(body->GetWorldSpaceBounds());
 }
 
 void JoltSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) {
@@ -605,10 +564,7 @@ void JoltSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHandl
 		return;
 	}
 
-	const JoltReadableBody3D body = space->read_body(jolt_id);
+	const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
-	ERR_FAIL_COND(body.is_invalid());
-
-	const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
 
 	typedef JPH::SoftBodyMotionProperties::Vertex SoftBodyVertex;
 	typedef JPH::SoftBodyMotionProperties::Face SoftBodyFace;
@@ -660,14 +616,11 @@ Vector3 JoltSoftBody3D::get_vertex_position(int p_index) {
 	ERR_FAIL_INDEX_V(p_index, (int)shared->mesh_to_physics.size(), Vector3());
 	const size_t physics_index = (size_t)shared->mesh_to_physics[p_index];
 
-	const JoltReadableBody3D body = space->read_body(jolt_id);
+	const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
-	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
-
-	const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
 	const JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
 	const JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index];
 
-	return to_godot(body->GetCenterOfMassPosition() + physics_vertex.mPosition);
+	return to_godot(jolt_body->GetCenterOfMassPosition() + physics_vertex.mPosition);
 }
 
 void JoltSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position) {
@@ -682,15 +635,11 @@ void JoltSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position)
 		return;
 	}
 
-	JoltWritableBody3D body = space->write_body(jolt_id);
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
-	ERR_FAIL_COND(body.is_invalid());
-
-	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*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 = body->GetCenterOfMassPosition();
+	const JPH::RVec3 center_of_mass = jolt_body->GetCenterOfMassPosition();
 	const JPH::Vec3 local_position = JPH::Vec3(to_jolt_r(p_position) - center_of_mass);
 	const JPH::Vec3 displacement = local_position - physics_vertex.mPosition;
 	const JPH::Vec3 velocity = displacement / last_step;

modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp

@@ -1,196 +0,0 @@
-/**************************************************************************/
-/*  jolt_body_accessor_3d.cpp                                             */
-/**************************************************************************/
-/*                         This file is part of:                          */
-/*                             GODOT ENGINE                               */
-/*                        https://godotengine.org                         */
-/**************************************************************************/
-/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
-/*                                                                        */
-/* Permission is hereby granted, free of charge, to any person obtaining  */
-/* a copy of this software and associated documentation files (the        */
-/* "Software"), to deal in the Software without restriction, including    */
-/* without limitation the rights to use, copy, modify, merge, publish,    */
-/* distribute, sublicense, and/or sell copies of the Software, and to     */
-/* permit persons to whom the Software is furnished to do so, subject to  */
-/* the following conditions:                                              */
-/*                                                                        */
-/* The above copyright notice and this permission notice shall be         */
-/* included in all copies or substantial portions of the Software.        */
-/*                                                                        */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
-/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
-/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
-/**************************************************************************/
-
-#include "jolt_body_accessor_3d.h"
-
-#include "jolt_space_3d.h"
-
-namespace {
-
-template <class... TTypes>
-struct VariantVisitors : TTypes... {
-	using TTypes::operator()...;
-};
-
-template <class... TTypes>
-VariantVisitors(TTypes...) -> VariantVisitors<TTypes...>;
-
-} // namespace
-
-JoltBodyAccessor3D::JoltBodyAccessor3D(const JoltSpace3D *p_space) :
-		space(p_space) {
-}
-
-JoltBodyAccessor3D::~JoltBodyAccessor3D() = default;
-
-void JoltBodyAccessor3D::acquire(const JPH::BodyID *p_ids, int p_id_count) {
-	ERR_FAIL_NULL(space);
-
-	lock_iface = &space->get_lock_iface();
-	ids = BodyIDSpan(p_ids, p_id_count);
-	_acquire_internal(p_ids, p_id_count);
-}
-
-void JoltBodyAccessor3D::acquire(const JPH::BodyID &p_id) {
-	ERR_FAIL_NULL(space);
-
-	lock_iface = &space->get_lock_iface();
-	ids = p_id;
-	_acquire_internal(&p_id, 1);
-}
-
-void JoltBodyAccessor3D::acquire_active() {
-	const JPH::PhysicsSystem &physics_system = space->get_physics_system();
-
-	acquire(physics_system.GetActiveBodiesUnsafe(JPH::EBodyType::RigidBody), (int)physics_system.GetNumActiveBodies(JPH::EBodyType::RigidBody));
-}
-
-void JoltBodyAccessor3D::acquire_all() {
-	ERR_FAIL_NULL(space);
-
-	lock_iface = &space->get_lock_iface();
-
-	JPH::BodyIDVector *vector = std::get_if<JPH::BodyIDVector>(&ids);
-
-	if (vector == nullptr) {
-		ids = JPH::BodyIDVector();
-		vector = std::get_if<JPH::BodyIDVector>(&ids);
-	}
-
-	space->get_physics_system().GetBodies(*vector);
-
-	_acquire_internal(vector->data(), (int)vector->size());
-}
-
-void JoltBodyAccessor3D::release() {
-	_release_internal();
-	lock_iface = nullptr;
-}
-
-const JPH::BodyID *JoltBodyAccessor3D::get_ids() const {
-	ERR_FAIL_COND_V(not_acquired(), nullptr);
-
-	return std::visit(
-			VariantVisitors{
-					[](const JPH::BodyID &p_id) { return &p_id; },
-					[](const JPH::BodyIDVector &p_vector) { return p_vector.data(); },
-					[](const BodyIDSpan &p_span) { return p_span.ptr; } },
-			ids);
-}
-
-int JoltBodyAccessor3D::get_count() const {
-	ERR_FAIL_COND_V(not_acquired(), 0);
-
-	return std::visit(
-			VariantVisitors{
-					[](const JPH::BodyID &p_id) { return 1; },
-					[](const JPH::BodyIDVector &p_vector) { return (int)p_vector.size(); },
-					[](const BodyIDSpan &p_span) { return p_span.count; } },
-			ids);
-}
-
-const JPH::BodyID &JoltBodyAccessor3D::get_at(int p_index) const {
-	CRASH_BAD_INDEX(p_index, get_count());
-	return get_ids()[p_index];
-}
-
-void JoltBodyReader3D::_acquire_internal(const JPH::BodyID *p_ids, int p_id_count) {
-	mutex_mask = lock_iface->GetMutexMask(p_ids, p_id_count);
-	lock_iface->LockRead(mutex_mask);
-}
-
-void JoltBodyReader3D::_release_internal() {
-	ERR_FAIL_COND(not_acquired());
-	lock_iface->UnlockRead(mutex_mask);
-}
-
-JoltBodyReader3D::JoltBodyReader3D(const JoltSpace3D *p_space) :
-		JoltBodyAccessor3D(p_space) {
-}
-
-const JPH::Body *JoltBodyReader3D::try_get(const JPH::BodyID &p_id) const {
-	if (unlikely(p_id.IsInvalid())) {
-		return nullptr;
-	}
-
-	ERR_FAIL_COND_V(not_acquired(), nullptr);
-
-	return lock_iface->TryGetBody(p_id);
-}
-
-const JPH::Body *JoltBodyReader3D::try_get(int p_index) const {
-	const int count = get_count();
-	if (unlikely(p_index < 0 || p_index >= count)) {
-		return nullptr;
-	}
-
-	return try_get(get_at(p_index));
-}
-
-const JPH::Body *JoltBodyReader3D::try_get() const {
-	return try_get(0);
-}
-
-void JoltBodyWriter3D::_acquire_internal(const JPH::BodyID *p_ids, int p_id_count) {
-	mutex_mask = lock_iface->GetMutexMask(p_ids, p_id_count);
-	lock_iface->LockWrite(mutex_mask);
-}
-
-void JoltBodyWriter3D::_release_internal() {
-	ERR_FAIL_COND(not_acquired());
-	lock_iface->UnlockWrite(mutex_mask);
-}
-
-JoltBodyWriter3D::JoltBodyWriter3D(const JoltSpace3D *p_space) :
-		JoltBodyAccessor3D(p_space) {
-}
-
-JPH::Body *JoltBodyWriter3D::try_get(const JPH::BodyID &p_id) const {
-	if (unlikely(p_id.IsInvalid())) {
-		return nullptr;
-	}
-
-	ERR_FAIL_COND_V(not_acquired(), nullptr);
-
-	return lock_iface->TryGetBody(p_id);
-}
-
-JPH::Body *JoltBodyWriter3D::try_get(int p_index) const {
-	const int count = get_count();
-	if (unlikely(p_index < 0 || p_index >= count)) {
-		return nullptr;
-	}
-
-	return try_get(get_at(p_index));
-}
-
-JPH::Body *JoltBodyWriter3D::try_get() const {
-	return try_get(0);
-}

modules/jolt_physics/spaces/jolt_body_accessor_3d.h

@@ -1,211 +0,0 @@
-/**************************************************************************/
-/*  jolt_body_accessor_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 "../objects/jolt_object_3d.h"
-
-#include "Jolt/Jolt.h"
-
-#include "Jolt/Physics/Body/BodyLockInterface.h"
-
-#include <variant>
-
-class JoltArea3D;
-class JoltBody3D;
-class JoltShapedObject3D;
-class JoltSpace3D;
-
-class JoltBodyAccessor3D {
-protected:
-	struct BodyIDSpan {
-		BodyIDSpan(const JPH::BodyID *p_ptr, int p_count) :
-				ptr(p_ptr), count(p_count) {}
-
-		const JPH::BodyID *ptr;
-		int count;
-	};
-
-	virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) = 0;
-	virtual void _release_internal() = 0;
-
-	const JoltSpace3D *space = nullptr;
-
-	const JPH::BodyLockInterface *lock_iface = nullptr;
-
-	std::variant<JPH::BodyID, JPH::BodyIDVector, BodyIDSpan> ids;
-
-public:
-	explicit JoltBodyAccessor3D(const JoltSpace3D *p_space);
-	virtual ~JoltBodyAccessor3D() = 0;
-
-	void acquire(const JPH::BodyID *p_ids, int p_id_count);
-	void acquire(const JPH::BodyID &p_id);
-	void acquire_active();
-	void acquire_all();
-	void release();
-
-	bool is_acquired() const { return lock_iface != nullptr; }
-	bool not_acquired() const { return lock_iface == nullptr; }
-
-	const JoltSpace3D &get_space() const { return *space; }
-	const JPH::BodyID *get_ids() const;
-	int get_count() const;
-
-	const JPH::BodyID &get_at(int p_index) const;
-};
-
-class JoltBodyReader3D final : public JoltBodyAccessor3D {
-	virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) override;
-	virtual void _release_internal() override;
-
-	JPH::BodyLockInterface::MutexMask mutex_mask = 0;
-
-public:
-	explicit JoltBodyReader3D(const JoltSpace3D *p_space);
-
-	const JPH::Body *try_get(const JPH::BodyID &p_id) const;
-	const JPH::Body *try_get(int p_index) const;
-	const JPH::Body *try_get() const;
-};
-
-class JoltBodyWriter3D final : public JoltBodyAccessor3D {
-	virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) override;
-	virtual void _release_internal() override;
-
-	JPH::BodyLockInterface::MutexMask mutex_mask = 0;
-
-public:
-	explicit JoltBodyWriter3D(const JoltSpace3D *p_space);
-
-	JPH::Body *try_get(const JPH::BodyID &p_id) const;
-	JPH::Body *try_get(int p_index) const;
-	JPH::Body *try_get() const;
-};
-
-template <typename TBodyAccessor>
-class JoltScopedBodyAccessor3D {
-	TBodyAccessor inner;
-
-public:
-	JoltScopedBodyAccessor3D(const JoltSpace3D &p_space, const JPH::BodyID *p_ids, int p_id_count) :
-			inner(&p_space) { inner.acquire(p_ids, p_id_count); }
-
-	JoltScopedBodyAccessor3D(const JoltSpace3D &p_space, const JPH::BodyID &p_id) :
-			inner(&p_space) { inner.acquire(p_id); }
-
-	JoltScopedBodyAccessor3D(const JoltScopedBodyAccessor3D &p_other) = delete;
-	JoltScopedBodyAccessor3D(JoltScopedBodyAccessor3D &&p_other) = default;
-	~JoltScopedBodyAccessor3D() { inner.release(); }
-
-	const JoltSpace3D &get_space() const { return inner.get_space(); }
-	int get_count() const { return inner.get_count(); }
-	const JPH::BodyID &get_at(int p_index) const { return inner.get_at(p_index); }
-
-	JoltScopedBodyAccessor3D &operator=(const JoltScopedBodyAccessor3D &p_other) = delete;
-	JoltScopedBodyAccessor3D &operator=(JoltScopedBodyAccessor3D &&p_other) = default;
-
-	decltype(auto) try_get(const JPH::BodyID &p_id) const { return inner.try_get(p_id); }
-	decltype(auto) try_get(int p_index) const { return inner.try_get(p_index); }
-	decltype(auto) try_get() const { return inner.try_get(); }
-};
-
-template <typename TAccessor, typename TBody>
-class JoltAccessibleBody3D {
-	TAccessor accessor;
-	TBody *body = nullptr;
-
-public:
-	JoltAccessibleBody3D(const JoltSpace3D &p_space, const JPH::BodyID &p_id) :
-			accessor(p_space, p_id), body(accessor.try_get()) {}
-
-	bool is_valid() const { return body != nullptr; }
-	bool is_invalid() const { return body == nullptr; }
-
-	JoltObject3D *as_object() const {
-		if (body != nullptr) {
-			return reinterpret_cast<JoltObject3D *>(body->GetUserData());
-		} else {
-			return nullptr;
-		}
-	}
-
-	JoltShapedObject3D *as_shaped() const {
-		if (JoltObject3D *object = as_object(); object != nullptr && object->is_shaped()) {
-			return reinterpret_cast<JoltShapedObject3D *>(body->GetUserData());
-		} else {
-			return nullptr;
-		}
-	}
-
-	JoltBody3D *as_body() const {
-		if (JoltObject3D *object = as_object(); object != nullptr && object->is_body()) {
-			return reinterpret_cast<JoltBody3D *>(body->GetUserData());
-		} else {
-			return nullptr;
-		}
-	}
-
-	JoltArea3D *as_area() const {
-		if (JoltObject3D *object = as_object(); object != nullptr && object->is_area()) {
-			return reinterpret_cast<JoltArea3D *>(body->GetUserData());
-		} else {
-			return nullptr;
-		}
-	}
-
-	TBody *operator->() const { return body; }
-	TBody &operator*() const { return *body; }
-
-	explicit operator TBody *() const { return body; }
-};
-
-template <typename TAccessor, typename TBody>
-class JoltAccessibleBodies3D {
-	TAccessor accessor;
-
-public:
-	JoltAccessibleBodies3D(const JoltSpace3D &p_space, const JPH::BodyID *p_ids, int p_id_count) :
-			accessor(p_space, p_ids, p_id_count) {}
-
-	JoltAccessibleBody3D<TAccessor, TBody> operator[](int p_index) const {
-		const JPH::BodyID &body_id = p_index < accessor.get_count() ? accessor.get_at(p_index) : JPH::BodyID();
-		return JoltAccessibleBody3D<TAccessor, TBody>(accessor.get_space(), body_id);
-	}
-};
-
-typedef JoltScopedBodyAccessor3D<JoltBodyReader3D> JoltScopedBodyReader3D;
-typedef JoltScopedBodyAccessor3D<JoltBodyWriter3D> JoltScopedBodyWriter3D;
-
-typedef JoltAccessibleBody3D<JoltScopedBodyReader3D, const JPH::Body> JoltReadableBody3D;
-typedef JoltAccessibleBody3D<JoltScopedBodyWriter3D, JPH::Body> JoltWritableBody3D;
-
-typedef JoltAccessibleBodies3D<JoltScopedBodyReader3D, const JPH::Body> JoltReadableBodies3D;
-typedef JoltAccessibleBodies3D<JoltScopedBodyWriter3D, JPH::Body> JoltWritableBodies3D;

modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp

@@ -262,7 +262,6 @@ bool JoltContactListener3D::_try_evaluate_area_overlap(const JPH::Body &p_body1,
 	};
 
 	const JPH::SubShapeIDPair shape_pair1(p_body1.GetID(), p_manifold.mSubShapeID1, p_body2.GetID(), p_manifold.mSubShapeID2);
-
 	const JPH::SubShapeIDPair shape_pair2(p_body2.GetID(), p_manifold.mSubShapeID2, p_body1.GetID(), p_manifold.mSubShapeID1);
 
 	const JoltObject3D *object1 = reinterpret_cast<JoltObject3D *>(p_body1.GetUserData());
@@ -405,13 +404,10 @@ void JoltContactListener3D::_flush_contacts() {
 		const JPH::SubShapeIDPair &shape_pair = E.key;
 		Manifold &manifold = E.value;
 
-		const JPH::BodyID body_ids[2] = { shape_pair.GetBody1ID(), shape_pair.GetBody2ID() };
+		JoltBody3D *body1 = space->try_get_body(shape_pair.GetBody1ID());
-		const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
-
-		JoltBody3D *body1 = jolt_bodies[0].as_body();
 		ERR_FAIL_NULL(body1);
 
-		JoltBody3D *body2 = jolt_bodies[1].as_body();
+		JoltBody3D *body2 = space->try_get_body(shape_pair.GetBody2ID());
 		ERR_FAIL_NULL(body2);
 
 		const int shape_index1 = body1->find_shape_index(shape_pair.GetSubShapeID1());
@@ -435,21 +431,18 @@ void JoltContactListener3D::_flush_area_enters() {
 		const JPH::BodyID &body_id1 = shape_pair.GetBody1ID();
 		const JPH::BodyID &body_id2 = shape_pair.GetBody2ID();
 
-		const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
+		JoltObject3D *object1 = space->try_get_object(body_id1);
-		const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
+		JoltObject3D *object2 = space->try_get_object(body_id2);
 
-		const JPH::BodyID body_ids[2] = { body_id1, body_id2 };
+		if (object1 == nullptr || object2 == nullptr) {
-		const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
-
-		const JoltReadableBody3D jolt_body1 = jolt_bodies[0];
-		const JoltReadableBody3D jolt_body2 = jolt_bodies[1];
-
-		if (jolt_body1.is_invalid() || jolt_body2.is_invalid()) {
 			continue;
 		}
 
-		JoltArea3D *area1 = jolt_body1.as_area();
+		JoltArea3D *area1 = object1->as_area();
-		JoltArea3D *area2 = jolt_body2.as_area();
+		JoltArea3D *area2 = object2->as_area();
+
+		const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
+		const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
 
 		if (area1 != nullptr && area2 != nullptr) {
 			area1->area_shape_entered(body_id2, sub_shape_id2, sub_shape_id1);
@@ -466,8 +459,7 @@ void JoltContactListener3D::_flush_area_enters() {
 void JoltContactListener3D::_flush_area_shifts() {
 	for (const JPH::SubShapeIDPair &shape_pair : area_overlaps) {
 		auto is_shifted = [&](const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_sub_shape_id) {
-			const JoltReadableBody3D jolt_body = space->read_body(p_body_id);
+			const JoltShapedObject3D *object = space->try_get_shaped(p_body_id);
-			const JoltShapedObject3D *object = jolt_body.as_shaped();
 			ERR_FAIL_NULL_V(object, false);
 
 			if (object->get_previous_jolt_shape() == nullptr) {
@@ -495,21 +487,18 @@ void JoltContactListener3D::_flush_area_exits() {
 		const JPH::BodyID &body_id1 = shape_pair.GetBody1ID();
 		const JPH::BodyID &body_id2 = shape_pair.GetBody2ID();
 
+		JoltObject3D *object1 = space->try_get_object(body_id1);
+		JoltObject3D *object2 = space->try_get_object(body_id2);
+
+		JoltArea3D *area1 = object1 != nullptr ? object1->as_area() : nullptr;
+		JoltArea3D *area2 = object2 != nullptr ? object2->as_area() : nullptr;
+
+		const JoltBody3D *body1 = object1 != nullptr ? object1->as_body() : nullptr;
+		const JoltBody3D *body2 = object2 != nullptr ? object2->as_body() : nullptr;
+
 		const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
 		const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
 
-		const JPH::BodyID body_ids[2] = { body_id1, body_id2 };
-		const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
-
-		const JoltReadableBody3D jolt_body1 = jolt_bodies[0];
-		const JoltReadableBody3D jolt_body2 = jolt_bodies[1];
-
-		JoltArea3D *area1 = jolt_body1.as_area();
-		JoltArea3D *area2 = jolt_body2.as_area();
-
-		const JoltBody3D *body1 = jolt_body1.as_body();
-		const JoltBody3D *body2 = jolt_body2.as_body();
-
 		if (area1 != nullptr && area2 != nullptr) {
 			area1->area_shape_exited(body_id2, sub_shape_id2, sub_shape_id1);
 		} else if (area1 != nullptr && body2 != nullptr) {

modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp

@@ -90,8 +90,7 @@ bool JoltMotionFilter3D::ShouldCollideLocked(const JPH::Body &p_jolt_body) const
 		return false;
 	}
 
-	const JoltReadableBody3D jolt_body_self = space.read_body(body_self);
+	return body_self.get_jolt_body()->GetCollisionGroup().CanCollide(p_jolt_body.GetCollisionGroup());
-	return jolt_body_self->GetCollisionGroup().CanCollide(p_jolt_body.GetCollisionGroup());
 }
 
 bool JoltMotionFilter3D::ShouldCollide(const JPH::Shape *p_jolt_shape, const JPH::SubShapeID &p_jolt_shape_id) const {

modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp

@@ -121,7 +121,7 @@ bool JoltPhysicsDirectSpaceState3D::_cast_motion_impl(const JPH::Shape &p_jolt_s
 			continue;
 		}
 
-		const JoltReadableBody3D other_jolt_body = space->read_body(other_jolt_id);
+		const JPH::Body *other_jolt_body = space->try_get_jolt_body(other_jolt_id);
 		if (!p_body_filter.ShouldCollideLocked(*other_jolt_body)) {
 			continue;
 		}
@@ -202,9 +202,7 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_recover(const JoltBody3D &p_bod
 
 		for (int j = 0; j < hit_count; j++) {
 			const JPH::CollideShapeResult &hit = collector.get_hit(j);
-
+			const JoltBody3D *other_body = space->try_get_body(hit.mBodyID2);
-			const JoltReadableBody3D other_jolt_body = space->read_body(hit.mBodyID2);
-			const JoltBody3D *other_body = other_jolt_body.as_body();
 			ERR_CONTINUE(other_body == nullptr);
 
 			combined_priority += other_body->get_collision_priority();
@@ -234,8 +232,7 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_recover(const JoltBody3D &p_bod
 				continue;
 			}
 
-			const JoltReadableBody3D other_jolt_body = space->read_body(hit.mBodyID2);
+			const JoltBody3D *other_body = space->try_get_body(hit.mBodyID2);
-			const JoltBody3D *other_body = other_jolt_body.as_body();
 			ERR_CONTINUE(other_body == nullptr);
 
 			const float recovery_distance = penetration_depth * JoltProjectSettings::motion_query_recovery_amount;
@@ -356,8 +353,7 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_collide(const JoltBody3D &p_bod
 			contact_points2.push_back(hit.mContactPointOn2);
 		}
 
-		const JoltReadableBody3D collider_jolt_body = space->read_body(hit.mBodyID2);
+		const JoltShapedObject3D *collider = space->try_get_shaped(hit.mBodyID2);
-		const JoltShapedObject3D *collider = collider_jolt_body.as_shaped();
 		ERR_FAIL_NULL_V(collider, false);
 
 		const int local_shape = p_body.find_shape_index(hit.mSubShapeID1);
@@ -495,8 +491,7 @@ bool JoltPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_paramet
 	const JPH::BodyID &body_id = hit.mBodyID;
 	const JPH::SubShapeID &sub_shape_id = hit.mSubShapeID2;
 
-	const JoltReadableBody3D body = space->read_body(body_id);
+	const JoltObject3D *object = space->try_get_object(body_id);
-	const JoltObject3D *object = body.as_object();
 	ERR_FAIL_NULL_V(object, false);
 
 	const JPH::RVec3 position = ray.GetPointOnRay(hit.mFraction);
@@ -504,7 +499,7 @@ bool JoltPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_paramet
 	JPH::Vec3 normal = JPH::Vec3::sZero();
 
 	if (!p_parameters.hit_from_inside || hit.mFraction > 0.0f) {
-		normal = body->GetWorldSpaceSurfaceNormal(sub_shape_id, position);
+		normal = object->get_jolt_body()->GetWorldSpaceSurfaceNormal(sub_shape_id, position);
 
 		// If we got a back-face normal we need to flip it.
 		if (normal.Dot(vector) > 0) {
@@ -523,7 +518,7 @@ bool JoltPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_paramet
 		const int shape_index = shaped_object->find_shape_index(sub_shape_id);
 		ERR_FAIL_COND_V(shape_index == -1, false);
 		r_result.shape = shape_index;
-		r_result.face_index = _try_get_face_index(*body, sub_shape_id);
+		r_result.face_index = _try_get_face_index(*object->get_jolt_body(), sub_shape_id);
 	}
 
 	return true;
@@ -546,9 +541,7 @@ int JoltPhysicsDirectSpaceState3D::intersect_point(const PointParameters &p_para
 
 	for (int i = 0; i < hit_count; ++i) {
 		const JPH::CollidePointResult &hit = collector.get_hit(i);
-
+		const JoltObject3D *object = space->try_get_object(hit.mBodyID);
-		const JoltReadableBody3D body = space->read_body(hit.mBodyID);
-		const JoltObject3D *object = body.as_object();
 		ERR_FAIL_NULL_V(object, 0);
 
 		ShapeResult &result = *r_results++;
@@ -605,9 +598,7 @@ int JoltPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_para
 
 	for (int i = 0; i < hit_count; ++i) {
 		const JPH::CollideShapeResult &hit = collector.get_hit(i);
-
+		const JoltObject3D *object = space->try_get_object(hit.mBodyID2);
-		const JoltReadableBody3D body = space->read_body(hit.mBodyID2);
-		const JoltObject3D *object = body.as_object();
 		ERR_FAIL_NULL_V(object, 0);
 
 		ShapeResult &result = *r_results++;
@@ -769,8 +760,7 @@ bool JoltPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameter
 	}
 
 	const JPH::CollideShapeResult &hit = collector.get_hit();
-	const JoltReadableBody3D body = space->read_body(hit.mBodyID2);
+	const JoltObject3D *object = space->try_get_object(hit.mBodyID2);
-	const JoltObject3D *object = body.as_object();
 	ERR_FAIL_NULL_V(object, false);
 
 	r_info->shape = 0;
@@ -807,11 +797,9 @@ Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_
 	ERR_FAIL_NULL_V(object, Vector3());
 	ERR_FAIL_COND_V(object->get_space() != space, Vector3());
 
-	const JoltReadableBody3D body = space->read_body(*object);
-	const JPH::TransformedShape root_shape = body->GetTransformedShape();
-
 	JoltQueryCollectorAll<JPH::TransformedShapeCollector, 32> collector;
-	root_shape.CollectTransformedShapes(body->GetWorldSpaceBounds(), collector);
+	const JPH::TransformedShape root_shape = object->get_jolt_body()->GetTransformedShape();
+	root_shape.CollectTransformedShapes(object->get_jolt_body()->GetWorldSpaceBounds(), collector);
 
 	const JPH::RVec3 point = to_jolt_r(p_point);
 
@@ -835,17 +823,14 @@ Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_
 		JPH::ConvexShape::SupportBuffer shape_support_buffer;
 		const JPH::ConvexShape::Support *shape_support = shape_convex.GetSupportFunction(JPH::ConvexShape::ESupportMode::IncludeConvexRadius, shape_support_buffer, shape_transformed.GetShapeScale());
 
-		const JPH::Quat &shape_rotation = shape_transformed.mShapeRotation;
+		const JPH::RMat44 shape_rotation = JPH::RMat44::sRotation(shape_transformed.mShapeRotation);
-		const JPH::RVec3 &shape_pos_com = shape_transformed.mShapePositionCOM;
+		const JPH::Vec3 shape_com = shape_rotation.Multiply3x3(shape.GetCenterOfMass());
-		const JPH::RMat44 shape_3x3 = JPH::RMat44::sRotation(shape_rotation);
+		const JPH::RVec3 shape_pos = shape_transformed.mShapePositionCOM - JPH::RVec3(shape_com);
-		const JPH::Vec3 shape_com_local = shape.GetCenterOfMass();
+		const JPH::RMat44 shape_xform = shape_rotation.PostTranslated(shape_pos);
-		const JPH::Vec3 shape_com = shape_3x3.Multiply3x3(shape_com_local);
+		const JPH::RMat44 shape_xform_inv = shape_xform.InversedRotationTranslation();
-		const JPH::RVec3 shape_pos = shape_pos_com - JPH::RVec3(shape_com);
-		const JPH::RMat44 shape_4x4 = shape_3x3.PostTranslated(shape_pos);
-		const JPH::RMat44 shape_4x4_inv = shape_4x4.InversedRotationTranslation();
 
 		JPH::PointConvexSupport point_support;
-		point_support.mPoint = JPH::Vec3(shape_4x4_inv * point);
+		point_support.mPoint = JPH::Vec3(shape_xform_inv * point);
 
 		JPH::Vec3 separating_axis = JPH::Vec3::sAxisX();
 		JPH::Vec3 point_on_a = JPH::Vec3::sZero();
@@ -861,7 +846,7 @@ Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_
 
 		if (distance_sq < closest_distance_sq) {
 			closest_distance_sq = distance_sq;
-			closest_point = shape_4x4 * point_on_a;
+			closest_point = shape_xform * point_on_a;
 			found_point = true;
 		}
 	}
@@ -869,7 +854,7 @@ Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_
 	if (found_point) {
 		return to_godot(closest_point);
 	} else {
-		return to_godot(body->GetPosition());
+		return to_godot(object->get_jolt_body()->GetPosition());
 	}
 }
 

modules/jolt_physics/spaces/jolt_space_3d.cpp

@@ -298,28 +298,53 @@ void JoltSpace3D::map_from_object_layer(JPH::ObjectLayer p_object_layer, JPH::Br
 	layers->from_object_layer(p_object_layer, r_broad_phase_layer, r_collision_layer, r_collision_mask);
 }
 
-JoltReadableBody3D JoltSpace3D::read_body(const JPH::BodyID &p_body_id) const {
+JPH::Body *JoltSpace3D::try_get_jolt_body(const JPH::BodyID &p_body_id) const {
-	return JoltReadableBody3D(*this, p_body_id);
+	return get_lock_iface().TryGetBody(p_body_id);
 }
 
-JoltReadableBody3D JoltSpace3D::read_body(const JoltObject3D &p_object) const {
+JoltObject3D *JoltSpace3D::try_get_object(const JPH::BodyID &p_body_id) const {
-	return read_body(p_object.get_jolt_id());
+	const JPH::Body *jolt_body = try_get_jolt_body(p_body_id);
+	if (unlikely(jolt_body == nullptr)) {
+		return nullptr;
+	}
+
+	return reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
 }
 
-JoltWritableBody3D JoltSpace3D::write_body(const JPH::BodyID &p_body_id) const {
+JoltShapedObject3D *JoltSpace3D::try_get_shaped(const JPH::BodyID &p_body_id) const {
-	return JoltWritableBody3D(*this, p_body_id);
+	JoltObject3D *object = try_get_object(p_body_id);
+	if (unlikely(object == nullptr)) {
+		return nullptr;
+	}
+
+	return object->as_shaped();
 }
 
-JoltWritableBody3D JoltSpace3D::write_body(const JoltObject3D &p_object) const {
+JoltBody3D *JoltSpace3D::try_get_body(const JPH::BodyID &p_body_id) const {
-	return write_body(p_object.get_jolt_id());
+	JoltObject3D *object = try_get_object(p_body_id);
+	if (unlikely(object == nullptr)) {
+		return nullptr;
+	}
+
+	return object->as_body();
 }
 
-JoltReadableBodies3D JoltSpace3D::read_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const {
+JoltArea3D *JoltSpace3D::try_get_area(const JPH::BodyID &p_body_id) const {
-	return JoltReadableBodies3D(*this, p_body_ids, p_body_count);
+	JoltObject3D *object = try_get_object(p_body_id);
+	if (unlikely(object == nullptr)) {
+		return nullptr;
+	}
+
+	return object->as_area();
 }
 
-JoltWritableBodies3D JoltSpace3D::write_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const {
+JoltSoftBody3D *JoltSpace3D::try_get_soft_body(const JPH::BodyID &p_body_id) const {
-	return JoltWritableBodies3D(*this, p_body_ids, p_body_count);
+	JoltObject3D *object = try_get_object(p_body_id);
+	if (unlikely(object == nullptr)) {
+		return nullptr;
+	}
+
+	return object->as_soft_body();
 }
 
 JoltPhysicsDirectSpaceState3D *JoltSpace3D::get_direct_state() {
@@ -346,38 +371,40 @@ void JoltSpace3D::set_default_area(JoltArea3D *p_area) {
 	}
 }
 
-JPH::BodyID JoltSpace3D::add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) {
+JPH::Body *JoltSpace3D::add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) {
-	const JPH::BodyID body_id = get_body_iface().CreateAndAddBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
+	JPH::BodyInterface &body_iface = get_body_iface();
-
+	JPH::Body *jolt_body = body_iface.CreateBody(p_settings);
-	if (unlikely(body_id.IsInvalid())) {
+	if (unlikely(jolt_body == nullptr)) {
 		ERR_PRINT_ONCE(vformat("Failed to create underlying Jolt Physics body for '%s'. "
 							   "Consider increasing maximum number of bodies in project settings. "
 							   "Maximum number of bodies is currently set to %d.",
 				p_object.to_string(), JoltProjectSettings::max_bodies));
 
-		return JPH::BodyID();
+		return nullptr;
 	}
 
+	body_iface.AddBody(jolt_body->GetID(), p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
 	bodies_added_since_optimizing += 1;
 
-	return body_id;
+	return jolt_body;
 }
 
-JPH::BodyID JoltSpace3D::add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) {
+JPH::Body *JoltSpace3D::add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) {
-	const JPH::BodyID body_id = get_body_iface().CreateAndAddSoftBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
+	JPH::BodyInterface &body_iface = get_body_iface();
-
+	JPH::Body *jolt_body = body_iface.CreateSoftBody(p_settings);
-	if (unlikely(body_id.IsInvalid())) {
+	if (unlikely(jolt_body == nullptr)) {
 		ERR_PRINT_ONCE(vformat("Failed to create underlying Jolt Physics body for '%s'. "
 							   "Consider increasing maximum number of bodies in project settings. "
 							   "Maximum number of bodies is currently set to %d.",
 				p_object.to_string(), JoltProjectSettings::max_bodies));
 
-		return JPH::BodyID();
+		return nullptr;
 	}
 
+	body_iface.AddBody(jolt_body->GetID(), p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
 	bodies_added_since_optimizing += 1;
 
-	return body_id;
+	return jolt_body;
 }
 
 void JoltSpace3D::remove_body(const JPH::BodyID &p_body_id) {

modules/jolt_physics/spaces/jolt_space_3d.h

@@ -30,8 +30,6 @@
 
 #pragma once
 
-#include "jolt_body_accessor_3d.h"
-
 #include "servers/physics_server_3d.h"
 
 #include "Jolt/Jolt.h"
@@ -52,6 +50,7 @@ class JoltLayers;
 class JoltObject3D;
 class JoltPhysicsDirectSpaceState3D;
 class JoltShapedObject3D;
+class JoltSoftBody3D;
 
 class JoltSpace3D {
 	SelfList<JoltBody3D>::List body_call_queries_list;
@@ -112,14 +111,12 @@ public:
 	JPH::ObjectLayer map_to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask);
 	void map_from_object_layer(JPH::ObjectLayer p_object_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const;
 
-	JoltReadableBody3D read_body(const JPH::BodyID &p_body_id) const;
+	JPH::Body *try_get_jolt_body(const JPH::BodyID &p_body_id) const;
-	JoltReadableBody3D read_body(const JoltObject3D &p_object) const;
+	JoltObject3D *try_get_object(const JPH::BodyID &p_body_id) const;
-
+	JoltShapedObject3D *try_get_shaped(const JPH::BodyID &p_body_id) const;
-	JoltWritableBody3D write_body(const JPH::BodyID &p_body_id) const;
+	JoltBody3D *try_get_body(const JPH::BodyID &p_body_id) const;
-	JoltWritableBody3D write_body(const JoltObject3D &p_object) const;
+	JoltArea3D *try_get_area(const JPH::BodyID &p_body_id) const;
-
+	JoltSoftBody3D *try_get_soft_body(const JPH::BodyID &p_body_id) const;
-	JoltReadableBodies3D read_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const;
-	JoltWritableBodies3D write_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const;
 
 	JoltPhysicsDirectSpaceState3D *get_direct_state();
 
@@ -128,8 +125,8 @@ public:
 
 	float get_last_step() const { return last_step; }
 
-	JPH::BodyID add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping = false);
+	JPH::Body *add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping = false);
-	JPH::BodyID add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping = false);
+	JPH::Body *add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping = false);
 
 	void remove_body(const JPH::BodyID &p_body_id);