diff --git a/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp b/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp index 05b241eee2f..96081636c22 100644 --- a/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp +++ b/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] = { - body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(), - 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(jolt_bodies[0]); - JPH::Body *jolt_body_b = static_cast(jolt_bodies[1]); - + JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr; + JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr; ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr); Transform3D shifted_ref_a; diff --git a/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp index 200491ac497..07041d075fc 100644 --- a/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp +++ b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp @@ -656,16 +656,8 @@ void JoltGeneric6DOFJoint3D::rebuild() { return; } - const JPH::BodyID body_ids[2] = { - body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(), - 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(jolt_bodies[0]); - JPH::Body *jolt_body_b = static_cast(jolt_bodies[1]); - + JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr; + JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr; ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr); Transform3D shifted_ref_a; diff --git a/modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp b/modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp index de5118f25cf..9fe2de0401e 100644 --- a/modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp +++ b/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] = { - body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(), - 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(jolt_bodies[0]); - JPH::Body *jolt_body_b = static_cast(jolt_bodies[1]); - + JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr; + JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr; ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr); float ref_shift = 0.0f; diff --git a/modules/jolt_physics/joints/jolt_pin_joint_3d.cpp b/modules/jolt_physics/joints/jolt_pin_joint_3d.cpp index 42ede3bd5bd..4a50e5453af 100644 --- a/modules/jolt_physics/joints/jolt_pin_joint_3d.cpp +++ b/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] = { - body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(), - 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(jolt_bodies[0]); - JPH::Body *jolt_body_b = static_cast(jolt_bodies[1]); - + JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr; + JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr; ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr); Transform3D shifted_ref_a; diff --git a/modules/jolt_physics/joints/jolt_slider_joint_3d.cpp b/modules/jolt_physics/joints/jolt_slider_joint_3d.cpp index 335e6576211..00ed11944d0 100644 --- a/modules/jolt_physics/joints/jolt_slider_joint_3d.cpp +++ b/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] = { - body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(), - 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(jolt_bodies[0]); - JPH::Body *jolt_body_b = static_cast(jolt_bodies[1]); - + JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr; + JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr; ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr); float ref_shift = 0.0f; diff --git a/modules/jolt_physics/objects/jolt_area_3d.cpp b/modules/jolt_physics/objects/jolt_area_3d.cpp index 19fd7a090da..dea95d1520f 100644 --- a/modules/jolt_physics/objects/jolt_area_3d.cpp +++ b/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); - if (new_jolt_id.IsInvalid()) { + JPH::Body *new_jolt_body = space->add_rigid_body(*this, *jolt_settings); + 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 = other_jolt_body.as_shaped(); + const JoltShapedObject3D *other_object = space->try_get_shaped(p_body_id); 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); - - JoltBody3D *body = jolt_body.as_body(); - if (unlikely(body == nullptr)) { - return; + if (JoltBody3D *other_body = space->try_get_body(p_body_id)) { + other_body->add_area(this); } - - 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); - - JoltBody3D *body = jolt_body.as_body(); - if (unlikely(body == nullptr)) { - return; + if (JoltBody3D *other_body = space->try_get_body(p_body_id)) { + other_body->remove_area(this); } - - 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); - ERR_FAIL_COND(body.is_invalid()); - - body->GetCollisionGroup().SetGroupFilter(JoltGroupFilter::instance); + jolt_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); } } diff --git a/modules/jolt_physics/objects/jolt_body_3d.cpp b/modules/jolt_physics/objects/jolt_body_3d.cpp index 9e5c437f972..1a4459f77f6 100644 --- a/modules/jolt_physics/objects/jolt_body_3d.cpp +++ b/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); - if (new_jolt_id.IsInvalid()) { + JPH::Body *new_jolt_body = space->add_rigid_body(*this, *jolt_settings, sleep_initially); + 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()) { - return; + if (in_space()) { + 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); - ERR_FAIL_COND(body.is_invalid()); - - body->SetCollideKinematicVsNonDynamic(value); + jolt_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()) { - _motion_changed(); - return; + if (in_space()) { + jolt_body->ResetForce(); + 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); - ERR_FAIL_COND_V(body.is_invalid(), Basis()); - - return to_godot(body->GetRotation() * body->GetMotionProperties()->GetInertiaRotation()); + return to_godot(jolt_body->GetRotation() * jolt_body->GetMotionPropertiesUnchecked()->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); - ERR_FAIL_COND_V(body.is_invalid(), Vector3()); - - const JPH::MotionProperties &motion_properties = *body->GetMotionPropertiesUnchecked(); - - return to_godot(motion_properties.GetLocalSpaceInverseInertia().GetDiagonal3()); + return to_godot(jolt_body->GetMotionPropertiesUnchecked()->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); - ERR_FAIL_COND_V(body.is_invalid(), Basis()); - - return to_godot(body->GetInverseInertia()).basis; + return to_godot(jolt_body->GetInverseInertia()).basis; } void JoltBody3D::set_linear_velocity(const Vector3 &p_velocity) { if (is_static() || is_kinematic()) { linear_surface_velocity = p_velocity; - _motion_changed(); - return; + } else { + 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(); - return; + } else { + 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); - ERR_FAIL_COND_V(body.is_invalid(), Vector3()); - - const JPH::MotionProperties &motion_properties = *body->GetMotionPropertiesUnchecked(); - + const JPH::MotionProperties &motion_properties = *jolt_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(); - return; + } else { + 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()) { - 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)); + jolt_body->AddForce(to_jolt(p_force), jolt_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()) { - return; - } - - const JoltWritableBody3D body = space->write_body(jolt_id); - ERR_FAIL_COND(body.is_invalid()); - - body->AddForce(to_jolt(p_force)); + jolt_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()) { - 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)); + jolt_body->AddImpulse(to_jolt(p_impulse), jolt_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()) { - return; - } - - const JoltWritableBody3D body = space->write_body(jolt_id); - ERR_FAIL_COND(body.is_invalid()); - - body->AddImpulse(to_jolt(p_impulse)); + jolt_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()) { - return; - } - - const JoltWritableBody3D body = space->write_body(jolt_id); - ERR_FAIL_COND(body.is_invalid()); - - body->AddTorque(to_jolt(p_torque)); + jolt_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()) { - return; - } - - const JoltWritableBody3D body = space->write_body(jolt_id); - ERR_FAIL_COND(body.is_invalid()); - - body->AddAngularImpulse(to_jolt(p_impulse)); + jolt_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()); - body->SetAngularVelocity(JPH::Vec3::sZero()); + jolt_body->SetLinearVelocity(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) { diff --git a/modules/jolt_physics/objects/jolt_object_3d.cpp b/modules/jolt_physics/objects/jolt_object_3d.cpp index e9e6d504e0e..d737cb3b2dd 100644 --- a/modules/jolt_physics/objects/jolt_object_3d.cpp +++ b/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); - - jolt_id = JPH::BodyID(); + space->remove_body(jolt_body->GetID()); + jolt_body = nullptr; } 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() { diff --git a/modules/jolt_physics/objects/jolt_object_3d.h b/modules/jolt_physics/objects/jolt_object_3d.h index 624e506f21f..fb338f4e14b 100644 --- a/modules/jolt_physics/objects/jolt_object_3d.h +++ b/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); diff --git a/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp index f32ddad0d46..5f40c325245 100644 --- a/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp +++ b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp @@ -178,11 +178,8 @@ void JoltShapedObject3D::_space_changing() { previous_jolt_shape = nullptr; - if (space != nullptr) { - const JoltWritableBody3D body = space->write_body(jolt_id); - ERR_FAIL_COND(body.is_invalid()); - - jolt_settings = new JPH::BodyCreationSettings(body->GetBodyCreationSettings()); + if (in_space()) { + jolt_settings = new JPH::BodyCreationSettings(jolt_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())); - - const JoltReadableBody3D body = space->read_body(jolt_id); - ERR_FAIL_COND_V(body.is_invalid(), Vector3()); - - return to_godot(body->GetCenterOfMassPosition()); + 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 { @@ -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(); diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp index 3e6a42feead..67c752748a4 100644 --- a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp +++ b/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 (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()); + if (in_space()) { + jolt_settings = new JPH::SoftBodyCreationSettings(jolt_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); - if (new_jolt_id.IsInvalid()) { + JPH::Body *new_jolt_body = space->add_soft_body(*this, *jolt_settings); + 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); - ERR_FAIL_COND(body.is_invalid()); - - JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); - + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*jolt_body->GetMotionPropertiesUnchecked()); JPH::Array &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); - ERR_FAIL_COND(body.is_invalid()); - - JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); - + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*jolt_body->GetMotionPropertiesUnchecked()); motion_properties.SetPressure(pressure); } @@ -273,11 +262,7 @@ void JoltSoftBody3D::_update_damping() { return; } - JoltWritableBody3D body = space->write_body(jolt_id); - ERR_FAIL_COND(body.is_invalid()); - - JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); - + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*jolt_body->GetMotionPropertiesUnchecked()); motion_properties.SetLinearDamping(linear_damping); } @@ -287,11 +272,7 @@ void JoltSoftBody3D::_update_simulation_precision() { return; } - JoltWritableBody3D body = space->write_body(jolt_id); - ERR_FAIL_COND(body.is_invalid()); - - JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); - + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*jolt_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(*body->GetMotionPropertiesUnchecked()); + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*jolt_body->GetMotionPropertiesUnchecked()); JPH::Array &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())); - - const JoltReadableBody3D body = space->read_body(jolt_id); - ERR_FAIL_COND_V(body.is_invalid(), AABB()); - - return to_godot(body->GetWorldSpaceBounds()); + return to_godot(jolt_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); - ERR_FAIL_COND(body.is_invalid()); - - const JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); + const JPH::SoftBodyMotionProperties &motion_properties = static_cast(*jolt_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); - ERR_FAIL_COND_V(body.is_invalid(), Vector3()); - - const JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); + const JPH::SoftBodyMotionProperties &motion_properties = static_cast(*jolt_body->GetMotionPropertiesUnchecked()); const JPH::Array &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); - ERR_FAIL_COND(body.is_invalid()); - - JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); - + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*jolt_body->GetMotionPropertiesUnchecked()); JPH::Array &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; diff --git a/modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp b/modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp deleted file mode 100644 index e9d40dd2ee0..00000000000 --- a/modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp +++ /dev/null @@ -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 -struct VariantVisitors : TTypes... { - using TTypes::operator()...; -}; - -template -VariantVisitors(TTypes...) -> VariantVisitors; - -} // 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(&ids); - - if (vector == nullptr) { - ids = JPH::BodyIDVector(); - vector = std::get_if(&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); -} diff --git a/modules/jolt_physics/spaces/jolt_body_accessor_3d.h b/modules/jolt_physics/spaces/jolt_body_accessor_3d.h deleted file mode 100644 index 6e79c4639d3..00000000000 --- a/modules/jolt_physics/spaces/jolt_body_accessor_3d.h +++ /dev/null @@ -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 - -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 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 -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 -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(body->GetUserData()); - } else { - return nullptr; - } - } - - JoltShapedObject3D *as_shaped() const { - if (JoltObject3D *object = as_object(); object != nullptr && object->is_shaped()) { - return reinterpret_cast(body->GetUserData()); - } else { - return nullptr; - } - } - - JoltBody3D *as_body() const { - if (JoltObject3D *object = as_object(); object != nullptr && object->is_body()) { - return reinterpret_cast(body->GetUserData()); - } else { - return nullptr; - } - } - - JoltArea3D *as_area() const { - if (JoltObject3D *object = as_object(); object != nullptr && object->is_area()) { - return reinterpret_cast(body->GetUserData()); - } else { - return nullptr; - } - } - - TBody *operator->() const { return body; } - TBody &operator*() const { return *body; } - - explicit operator TBody *() const { return body; } -}; - -template -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 operator[](int p_index) const { - const JPH::BodyID &body_id = p_index < accessor.get_count() ? accessor.get_at(p_index) : JPH::BodyID(); - return JoltAccessibleBody3D(accessor.get_space(), body_id); - } -}; - -typedef JoltScopedBodyAccessor3D JoltScopedBodyReader3D; -typedef JoltScopedBodyAccessor3D JoltScopedBodyWriter3D; - -typedef JoltAccessibleBody3D JoltReadableBody3D; -typedef JoltAccessibleBody3D JoltWritableBody3D; - -typedef JoltAccessibleBodies3D JoltReadableBodies3D; -typedef JoltAccessibleBodies3D JoltWritableBodies3D; diff --git a/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp b/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp index 680b341d44e..226244b9840 100644 --- a/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp +++ b/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(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() }; - const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2); - - JoltBody3D *body1 = jolt_bodies[0].as_body(); + JoltBody3D *body1 = space->try_get_body(shape_pair.GetBody1ID()); 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(); - const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2(); + JoltObject3D *object1 = space->try_get_object(body_id1); + JoltObject3D *object2 = space->try_get_object(body_id2); - 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]; - - if (jolt_body1.is_invalid() || jolt_body2.is_invalid()) { + if (object1 == nullptr || object2 == nullptr) { continue; } - JoltArea3D *area1 = jolt_body1.as_area(); - JoltArea3D *area2 = jolt_body2.as_area(); + JoltArea3D *area1 = object1->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 = jolt_body.as_shaped(); + const JoltShapedObject3D *object = space->try_get_shaped(p_body_id); 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) { diff --git a/modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp b/modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp index f6bd42ac44e..db60046f228 100644 --- a/modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp +++ b/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 jolt_body_self->GetCollisionGroup().CanCollide(p_jolt_body.GetCollisionGroup()); + return body_self.get_jolt_body()->GetCollisionGroup().CanCollide(p_jolt_body.GetCollisionGroup()); } bool JoltMotionFilter3D::ShouldCollide(const JPH::Shape *p_jolt_shape, const JPH::SubShapeID &p_jolt_shape_id) const { diff --git a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp index e0fb463e895..ca18ced8929 100644 --- a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp +++ b/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 JoltReadableBody3D other_jolt_body = space->read_body(hit.mBodyID2); - const JoltBody3D *other_body = other_jolt_body.as_body(); + const JoltBody3D *other_body = space->try_get_body(hit.mBodyID2); 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 = other_jolt_body.as_body(); + const JoltBody3D *other_body = space->try_get_body(hit.mBodyID2); 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 = collider_jolt_body.as_shaped(); + const JoltShapedObject3D *collider = space->try_get_shaped(hit.mBodyID2); 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 = body.as_object(); + const JoltObject3D *object = space->try_get_object(body_id); 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 JoltReadableBody3D body = space->read_body(hit.mBodyID); - const JoltObject3D *object = body.as_object(); + const JoltObject3D *object = space->try_get_object(hit.mBodyID); 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 JoltReadableBody3D body = space->read_body(hit.mBodyID2); - const JoltObject3D *object = body.as_object(); + const JoltObject3D *object = space->try_get_object(hit.mBodyID2); 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 = body.as_object(); + const JoltObject3D *object = space->try_get_object(hit.mBodyID2); 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 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::RVec3 &shape_pos_com = shape_transformed.mShapePositionCOM; - const JPH::RMat44 shape_3x3 = JPH::RMat44::sRotation(shape_rotation); - const JPH::Vec3 shape_com_local = shape.GetCenterOfMass(); - const JPH::Vec3 shape_com = shape_3x3.Multiply3x3(shape_com_local); - 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(); + const JPH::RMat44 shape_rotation = JPH::RMat44::sRotation(shape_transformed.mShapeRotation); + const JPH::Vec3 shape_com = shape_rotation.Multiply3x3(shape.GetCenterOfMass()); + const JPH::RVec3 shape_pos = shape_transformed.mShapePositionCOM - JPH::RVec3(shape_com); + const JPH::RMat44 shape_xform = shape_rotation.PostTranslated(shape_pos); + const JPH::RMat44 shape_xform_inv = shape_xform.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()); } } diff --git a/modules/jolt_physics/spaces/jolt_space_3d.cpp b/modules/jolt_physics/spaces/jolt_space_3d.cpp index ec959972bc1..aaa5a239947 100644 --- a/modules/jolt_physics/spaces/jolt_space_3d.cpp +++ b/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 { - return JoltReadableBody3D(*this, p_body_id); +JPH::Body *JoltSpace3D::try_get_jolt_body(const JPH::BodyID &p_body_id) const { + return get_lock_iface().TryGetBody(p_body_id); } -JoltReadableBody3D JoltSpace3D::read_body(const JoltObject3D &p_object) const { - return read_body(p_object.get_jolt_id()); +JoltObject3D *JoltSpace3D::try_get_object(const JPH::BodyID &p_body_id) const { + const JPH::Body *jolt_body = try_get_jolt_body(p_body_id); + if (unlikely(jolt_body == nullptr)) { + return nullptr; + } + + return reinterpret_cast(jolt_body->GetUserData()); } -JoltWritableBody3D JoltSpace3D::write_body(const JPH::BodyID &p_body_id) const { - return JoltWritableBody3D(*this, p_body_id); +JoltShapedObject3D *JoltSpace3D::try_get_shaped(const JPH::BodyID &p_body_id) const { + 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 { - return write_body(p_object.get_jolt_id()); +JoltBody3D *JoltSpace3D::try_get_body(const JPH::BodyID &p_body_id) const { + 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 { - return JoltReadableBodies3D(*this, p_body_ids, p_body_count); +JoltArea3D *JoltSpace3D::try_get_area(const JPH::BodyID &p_body_id) const { + 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 { - return JoltWritableBodies3D(*this, p_body_ids, p_body_count); +JoltSoftBody3D *JoltSpace3D::try_get_soft_body(const JPH::BodyID &p_body_id) const { + 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) { - const JPH::BodyID body_id = get_body_iface().CreateAndAddBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate); - - if (unlikely(body_id.IsInvalid())) { +JPH::Body *JoltSpace3D::add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) { + JPH::BodyInterface &body_iface = get_body_iface(); + JPH::Body *jolt_body = body_iface.CreateBody(p_settings); + 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) { - const JPH::BodyID body_id = get_body_iface().CreateAndAddSoftBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate); - - if (unlikely(body_id.IsInvalid())) { +JPH::Body *JoltSpace3D::add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) { + JPH::BodyInterface &body_iface = get_body_iface(); + JPH::Body *jolt_body = body_iface.CreateSoftBody(p_settings); + 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) { diff --git a/modules/jolt_physics/spaces/jolt_space_3d.h b/modules/jolt_physics/spaces/jolt_space_3d.h index 5edc6c0c051..22601338120 100644 --- a/modules/jolt_physics/spaces/jolt_space_3d.h +++ b/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::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; - JoltReadableBody3D read_body(const JoltObject3D &p_object) const; - - JoltWritableBody3D write_body(const JPH::BodyID &p_body_id) const; - JoltWritableBody3D write_body(const JoltObject3D &p_object) 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; + JPH::Body *try_get_jolt_body(const JPH::BodyID &p_body_id) const; + JoltObject3D *try_get_object(const JPH::BodyID &p_body_id) const; + JoltShapedObject3D *try_get_shaped(const JPH::BodyID &p_body_id) const; + JoltBody3D *try_get_body(const JPH::BodyID &p_body_id) const; + JoltArea3D *try_get_area(const JPH::BodyID &p_body_id) const; + JoltSoftBody3D *try_get_soft_body(const JPH::BodyID &p_body_id) 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::BodyID add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &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::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);