diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp index 67e7ebb306..7abf4c7918 100644 --- a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp +++ b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp @@ -188,9 +188,32 @@ JPH::SoftBodySharedSettings *JoltSoftBody3D::_create_shared_settings() { pin_vertices(*this, pinned_vertices, mesh_to_physics, physics_vertices); // Since Godot's stiffness is input as a coefficient between 0 and 1, and Jolt uses actual stiffness for its - // edge constraints, we crudely map one to the other with an arbitrary constant. - const float stiffness = MAX(Math::pow(stiffness_coefficient, 3.0f) * 100000.0f, 0.000001f); - const float inverse_stiffness = 1.0f / stiffness; + // edge constraints, we must map one to the other. + // + // Godot uses classic PBD edge constraints, which have a stiffness parameter k that is used in the position correction formula as follows: + // delta_x1 = -k * w1 / (w1 + w2) * (l - l0) / l * (x2 - x1) + // where k is the stiffness, w1 and w2 are the inverse masses of the two vertices, l is the current length of the edge = |x2 - x1|, l0 is the rest length of the edge, and x1 and x2 are the vertex positions. + // + // Note that the actual formula used in Godot physics seems to use an approximation of this which avoids calculating the square root: + // delta_x1 = -k * w1 / (w1 + w2) * (l^2 - l0^2) / (l^2 + l0^2) * (x2 - x1) + // + // Jolt uses XPBD which goes as follows: + // delta_x1 = -w1 / (w1 + w2 + compliance / dt^2) * (l - l0) / l * (x2 - x1) + // where compliance is the inverse of stiffness and dt is the timestep. + // + // We can derive Jolt's compliance from Godot's stiffness by evaluating: + // k * w1 / (w1 + w2) = w1 / (w1 + w2 + compliance / dt^2) + // which simplifies to: + // compliance = dt^2 * (1 / k - 1) * (w1 + w2) + + // Assuming that the vertices have the same mass: + const float w1_plus_w2 = 2.0f * physics_vertices.size() / mass; + + // Calculate time step of a single XPBD iteration + const float dt = 1.0f / Engine::get_singleton()->get_user_physics_ticks_per_second() / simulation_precision; + + // Now calculate the compliance + const float inverse_stiffness = dt * dt * (1.0f / stiffness_coefficient - 1.0f) * w1_plus_w2; JPH::SoftBodySharedSettings::VertexAttributes vertex_attrib; vertex_attrib.mCompliance = vertex_attrib.mShearCompliance = inverse_stiffness; @@ -213,7 +236,7 @@ void JoltSoftBody3D::_update_mass() { 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; + const float inverse_vertex_mass = (float)physics_vertices.size() / mass; for (JPH::SoftBodyVertex &vertex : physics_vertices) { vertex.mInvMass = inverse_vertex_mass; @@ -277,6 +300,7 @@ void JoltSoftBody3D::_simulation_precision_changed() { } void JoltSoftBody3D::_mass_changed() { + _update_mass(); wake_up(); } @@ -455,11 +479,13 @@ void JoltSoftBody3D::set_simulation_precision(int p_precision) { } void JoltSoftBody3D::set_mass(float p_mass) { + ERR_FAIL_COND(p_mass <= 0.0); // A mass of zero would result in infinite inverse mass. + if (unlikely(mass == p_mass)) { return; } - mass = MAX(p_mass, 0.0f); + mass = p_mass; _mass_changed(); } diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.h b/modules/jolt_physics/objects/jolt_soft_body_3d.h index eb347899a9..ec2b4d1eab 100644 --- a/modules/jolt_physics/objects/jolt_soft_body_3d.h +++ b/modules/jolt_physics/objects/jolt_soft_body_3d.h @@ -51,7 +51,7 @@ class JoltSoftBody3D final : public JoltObject3D { JPH::SoftBodyCreationSettings *jolt_settings = new JPH::SoftBodyCreationSettings(); - float mass = 0.0f; + float mass = 1.0f; float pressure = 0.0f; float linear_damping = 0.01f; float stiffness_coefficient = 0.5f;