[WIP] Iterate on making stiffness calculation more stable

Author: Marvin Ewald

doc_classes/RopeStiffnessParameters.xml

@@ -12,7 +12,7 @@
 			Stiffness forces the rope to return to its resting position. The resting direction is downwards and affected by the the node's rotation.
 		</member>
 		<member name="stiffness_bend_curve" type="Curve" setter="set_stiffness_bend_curve" getter="get_stiffness_bend_curve">
-			(Optional) Scale the stiffness individually per segment according to how far each segment is bent.[br]The [code]y[/code] value of the curve is multiplied with the stiffness. [code]x = 0[/code] corresponds to no bending and [code]x = 1[/code] corresponds to 180° bending in regard to the previous segment.[br]If unset, defaults to a quadratic curve.
+                        (Optional) Scale the stiffness individually per segment according to how far each segment is bent.[br]The [code]y[/code] value of the curve is multiplied with the stiffness. [code]x = 0[/code] corresponds to no bending and [code]x = 1[/code] corresponds to 180° bending in regard to the previous segment.[br]If unset, defaults to factor [code]1.0[/code].[br]Physically, there is a material-dependent limit how far an object can bend before bending properties (stiffness) change, e.g. a material might be less inclined to snap back into place at smaller bend angles, or a material might stop bending at some point, i.e. when it breaks. While this curve intends to accomodate these physical properties, it will never work fully realistically.
 		</member>
 		<member name="stiffness_curve" type="Curve" setter="set_stiffness_curve" getter="get_stiffness_curve">
 			(Optional) Scale the stiffness along the rope. Useful to make the rope less erratic towards the end.
@@ -22,11 +22,11 @@
 		</member>
 	</members>
 	<constants>
-		<constant name="Bidirectional" value="0" enum="StiffnessMethod">
-			Computes a forward and backward pass.[br]Produces overall better and more stable results than [enum Legacy]. Acts more sensitive to [member stiffness_bend_curve].
+		<constant name="Forward" value="0" enum="StiffnessMethod">
+			Computes a single forward pass from front to back. Produces stable results but may look unusual on "free" ropes ([member Rope.fixate_begin] disabled).
 		</constant>
-		<constant name="Legacy" value="1" enum="StiffnessMethod">
-			Computes a single forward pass from front to back.[br]Produces decent results but may look unusual without [member Rope.fixate_begin] enabled. It could also behave more unstable in certain scenarios, i.e. jittery or erratic movement.[br]This was the default behavior before [code]v1.9[/code].
+		<constant name="Bidirectional" value="1" enum="StiffnessMethod">
+			(Experimental) Computes a forward and backward pass. Produces better looking results with "free" ropes ([member Rope.fixate_begin] disabled). Acts much more sensitive to [member stiffness] and tends to produce more unstable results. Reducing gravity may help against erratic, unnatural or perpetual movement.
 		</constant>
 	</constants>
 </class>

src/NativeRopeContext.cpp

@@ -15,9 +15,14 @@ static float get_point_perc(int64_t index, const PackedVector2Array& points)
     return points.size() > 0 ? static_cast<float>(index) / static_cast<float>(points.size() - 1) : 0;
 }
 
+static Vector2 exp_decay(Vector2 value, Vector2 target, float damping_factor, double delta)
+{
+    return value.lerp(target, (float)(1.0 - exp(-damping_factor * delta)));
+}
+
 static Vector2 damp_vec(Vector2 value, float damping_factor, double delta)
 {
-    return value.lerp(VECTOR_ZERO, (float)(1.0 - exp(-damping_factor * delta)));
+    return exp_decay(value, VECTOR_ZERO, damping_factor, delta);
 }
 
 
@@ -67,6 +72,9 @@ void NativeRopeContext::load_context(Node2D* rope)
     collision_damping = rope->get("collision_damping");
     report_contact_points = rope->get("report_contact_points");
     resolve_collisions_while_constraining = rope->get("resolve_collisions_while_constraining");
+
+    if (data_buffer.size() < points.size())
+        data_buffer.resize(points.size() * 2);
 }
 
 void NativeRopeContext::simulate(double delta)
@@ -82,7 +90,16 @@ void NativeRopeContext::simulate(double delta)
         points[0] = rope->get_global_position();
     }
 
+    // Calculate new velocities and swap point buffers
     _simulate_velocities(delta);
+    std::swap(oldpoints, points);
+
+    // Compute new point positions
+    for (int64_t i = 0; i < points.size(); ++i)
+        points[i] = oldpoints[i] + simulation_weights[i] * data_buffer[i];
+
+    // Constraint step
+    _simulate_stiffness(delta);
     _constraint(delta);
 
     if (!resolve_collisions_while_constraining)
@@ -107,41 +124,22 @@ void NativeRopeContext::_writeback()
 
 void NativeRopeContext::_simulate_velocities(double delta)
 {
-    const int first_idx = fixate_begin ? 1 : 0;
-    const int size = (int)points.size();
-
-    // NOTE: The following is a little ugly but it reduces memory usage by reusing existing buffers.
-    // It is also faster than allocating a new temporary buffer and also likely more cache efficient.
-
-    // oldpoints is no longer needed after initial velocity computation, so we reuse it to store velocities.
-    PackedVector2Array& velocities = oldpoints;
-    // Afterwards we use oldpoints to store the new points and finally swap points and oldpoints
-    PackedVector2Array& new_points = oldpoints;
-
-    // Compute velocities and apply damping + gravity
     const bool use_damping_curve = damping_curve.is_valid() && damping_curve->get_point_count() > 0;
     const Vector2 frame_gravity = gravity_direction * static_cast<float>(gravity * delta);
 
-    for (int i = first_idx; i < size; ++i)
+    // Init velocities using delta + damping + gravity
+    for (int64_t i = 0; i < points.size(); ++i)
     {
         const Vector2 vel = points[i] - oldpoints[i];
         const float dampmult = use_damping_curve ? damping_curve->sample_baked(get_point_perc(i, points)) : 1.0f;
-        velocities[i] = damp_vec(vel, damping * dampmult, delta);
-        velocities[i] += frame_gravity;
+        data_buffer[i] = damp_vec(vel, damping * dampmult, delta) + frame_gravity;
     }
 
     // Apply other forces
-    _simulate_wind(&velocities);
-    _simulate_stiffness(delta, &velocities);
-
-    // Move points according to velocity
-    for (int i = first_idx; i < size; ++i)
-        new_points[i] = points[i] + simulation_weights[i] * velocities[i];
-
-    std::swap(oldpoints, points);
+    _simulate_wind();
 }
 
-void NativeRopeContext::_simulate_wind(PackedVector2Array* velocities) const
+void NativeRopeContext::_simulate_wind()
 {
     if (wind.is_null() || !wind->get_enable_wind() || wind->get_noise().is_null())
         return;
@@ -154,43 +152,54 @@ void NativeRopeContext::_simulate_wind(PackedVector2Array* velocities) const
     {
         const float noise = wind->get_noise()->get_noise_3d(points[i].x, points[i].y, time);
         const Vector2 noise_velocity = orth_strength * noise;
-        (*velocities)[i] += wind_velocity + noise_velocity;
+        data_buffer[i] += wind_velocity + noise_velocity;
     }
 }
 
-void NativeRopeContext::_simulate_stiffness(double delta, PackedVector2Array* velocities) const
+void NativeRopeContext::_simulate_stiffness(double delta)
 {
-    if (stiffness_params->get_stiffness() <= 0)
+    if (stiffness_params.is_null() || stiffness_params->get_stiffness() <= 0)
         return;
 
     const auto num_points = points.size();
+    uint8_t num_passes = 1;  // Averaging the force in bidirectional stiffness seems to make the rope more stable
 
-    if (stiffness_params->get_stiffness_method() == RopeStiffnessParameters::Legacy)
+    // Clear buffer as it is used for accumulating stiffness forces.
+    for (int64_t i = 0; i < num_points; ++i)
+        data_buffer[i] = VECTOR_ZERO;
+
+    if (stiffness_params->get_stiffness_method() == RopeStiffnessParameters::Forward)
     {
         const Vector2 parent_seg_dir = rope->get_global_transform().basis_xform(VECTOR_DOWN).normalized();
-        _simulate_stiffness_segment(delta, velocities, 1, num_points, parent_seg_dir);  // Start at second and stop at last point
+        _simulate_stiffness_segment(delta, 1, num_points, parent_seg_dir);  // Start at second and stop at last point
     }
     else
     {
         const Vector2 end_parent = (points[num_points - 2] - points[num_points - 1]).normalized();
+        num_passes = 2;
 
         if (fixate_begin)
         {
             // TODO: Make direction configurable, e.g. an enum to select the mode ("transform", "gravity", "custom").
             const Vector2 parent_seg_dir = rope->get_global_transform().basis_xform(VECTOR_DOWN).normalized();
-            _simulate_stiffness_segment(delta, velocities, 1, num_points, parent_seg_dir);  // Start at second and stop at last point
-            _simulate_stiffness_segment(delta, velocities, num_points - 2, 0, end_parent);  // Start at second last and stop before first point (because it's fixed)
+            _simulate_stiffness_segment(delta, 1, num_points, parent_seg_dir);  // Start at second and stop at last point
+            _simulate_stiffness_segment(delta, num_points - 2, 0, end_parent);  // Start at second last and stop before first point (because it's fixed)
         }
         else
         {
             const Vector2 begin_parent = (points[1] - points[0]).normalized();
-            _simulate_stiffness_segment(delta, velocities, 1, num_points, begin_parent);  // Start at second and stop at last point
-            _simulate_stiffness_segment(delta, velocities, num_points - 2, -1, end_parent);  // Start at second last and stop at first point
+            _simulate_stiffness_segment(delta, 1, num_points, begin_parent);  // Start at second and stop at last point
+            _simulate_stiffness_segment(delta, num_points - 2, -1, end_parent);  // Start at second last and stop at first point
         }
     }
+
+    const float stiffness_damping = stiffness_params->get_stiffness_damping();
+
+    for (int64_t i = 0; i < num_points; ++i)
+        points[i] += damp_vec(data_buffer[i] / num_passes, stiffness_damping, delta);
 }
 
-void NativeRopeContext::_simulate_stiffness_segment(double  /*delta*/, PackedVector2Array* velocities, int64_t idx_from, int64_t idx_stop, Vector2 last_stiffness_dir) const
+void NativeRopeContext::_simulate_stiffness_segment(double delta, int64_t idx_from, int64_t idx_stop, Vector2 last_stiffness_dir)
 {
     // NOTE: oldpoints should not be used here, see comments in simulate_velocities().
 
@@ -200,16 +209,12 @@ void NativeRopeContext::_simulate_stiffness_segment(double  /*delta*/, PackedVec
     const auto stiffness_bend_curve = stiffness_params->get_stiffness_bend_curve();
     const bool use_stiffness_curve = stiffness_curve.is_valid() && stiffness_curve->get_point_count() > 0;
     const bool use_bend_curve = stiffness_bend_curve.is_valid() && stiffness_bend_curve->get_point_count() > 0;
-    const int64_t idx_step = idx_from <= idx_stop ? 1 : -1;
+    const bool iterating_forward = idx_from <= idx_stop;
+    const int64_t idx_step = iterating_forward ? 1 : -1;
+    const int64_t seglength_idx_offset = iterating_forward ? -1 : 0;
 
     for (int64_t i = idx_from; i != idx_stop; i += idx_step)
     {
-        // NOTE: Asked a physicist to confirm this computation is physically accurate.
-        // He mentioned that, while it is technically correct, there is a material-dependent limit
-        // how far an object can bend before bending properties (stiffness) changes.
-        // E.g. a material might be less inclined to snap back into place at smaller bend angles, or
-        // a material might stop bending at some point, i.e. when it breaks.
-        //
         //  |  parent_seg_dir     --->  parent_seg_dir.orthogonal()
         //  |                     \
         //  V                      \   seg_dir
@@ -218,22 +223,21 @@ void NativeRopeContext::_simulate_stiffness_segment(double  /*delta*/, PackedVec
         //    V                   V
         //   / force_dir
         //  V
-        //
-        const auto last_idx = i - idx_step;
+
+        const int64_t last_idx = i - idx_step;
         const Vector2 seg_dir = (points[i] - points[last_idx]).normalized();
         const float angle = seg_dir.angle_to(last_stiffness_dir);  // angle is signed and can be used to determine the force direction
-        const float bend_factor = Math::absf(angle / 3.1415f);
+        const float bend_factor = Math::absf(angle / (float)Math_PI);
         const float scale_curve_pos = get_point_perc(idx_step >= 0 ? i : (points.size() - 1 - i), points);
         const float scale = use_stiffness_curve ? stiffness_curve->sample_baked(scale_curve_pos) : 1.0f;
-        // If no curve specified, use quadratic scale by default
-        const float bend_scale = (use_bend_curve ? stiffness_bend_curve->sample_baked(bend_factor) : bend_factor * bend_factor);
+        const float bend_scale = use_bend_curve ? stiffness_bend_curve->sample_baked(bend_factor) : 1.0f;
 
-        // The force directs orthogonal to the current segment
-        const Vector2 force_dir = Math::sign(-angle) * seg_dir.orthogonal();
-        force += force_dir * stiffness * scale * bend_scale;
+        const Vector2 target_pos = points[last_idx] + last_stiffness_dir * seg_lengths[i + seglength_idx_offset];
+        const Vector2 diff = target_pos - points[i];
+        force += exp_decay(VECTOR_ZERO, diff, stiffness * scale * bend_scale, delta);
 
-        // `simulation_weights` will be applied later in `_simulate_velocities()`
-        (*velocities)[i] += force;
+        // In bidirectional mode points need to be buffered to prevent the first pass affecting the second
+        data_buffer[i] += force * simulation_weights[i];
 
         // Carry velocity over to the next segment. `simulation_weights` must be applied now to not carry over a wrong value.
         force *= simulation_weights[i];

src/NativeRopeContext.hpp

@@ -21,11 +21,20 @@ class NativeRopeContext  // NOLINT(cppcoreguidelines-special-member-functions)
     bool validate() const;
 
 protected:
+    /// Reads point data from `points` and `oldpoints` and writes velocities to `data_buffer`.
     void _simulate_velocities(double delta);
-    void _simulate_wind(PackedVector2Array* velocities) const;
-    void _simulate_stiffness(double delta, PackedVector2Array* velocities) const;
-    void _simulate_stiffness_segment(double delta, PackedVector2Array* velocities, int64_t idx_from, int64_t idx_stop, Vector2 last_stiffness_dir) const;
+
+    /// Reads point data from `points` and writes velocities to `data_buffer`.
+    void _simulate_wind();
+
+    /// Reads point data from `data_buffer` and writes new points to `points`.
+    void _simulate_stiffness(double delta);
+    void _simulate_stiffness_segment(double delta, int64_t idx_from, int64_t idx_stop, Vector2 last_stiffness_dir);
+
+    /// Resolves collisions. Reads and writes to `points`.
     void _resolve_collisions(double delta, bool disable_contact_reporting);
+
+    /// Constrains points iteratively. Reads and writes to `points`.
     void _constraint(double delta);
     void _writeback();
 
@@ -36,6 +45,12 @@ class NativeRopeContext  // NOLINT(cppcoreguidelines-special-member-functions)
     PackedVector2Array _contact_points;
 
 public:
+    /// Internal buffer for point and velocity calculations. Will be resized on demand to be at
+    /// least as large as the current points vector. It will not be cleared between different ropes.
+    /// It is needed mostly for storing intermediate results during bidirectional stiffness
+    /// calculation without overwriting `points` and `oldpoints`.
+    PackedVector2Array data_buffer;
+
     Node2D* rope = nullptr;
     PackedVector2Array points;
     PackedVector2Array oldpoints;

src/RopeStiffnessParameters.cpp

@@ -47,6 +47,17 @@ void RopeStiffnessParameters::set_stiffness_bend_curve(Ref<Curve> curve)
     emit_changed();
 }
 
+float RopeStiffnessParameters::get_stiffness_damping() const
+{
+    return stiffness_damping;
+}
+
+void RopeStiffnessParameters::set_stiffness_damping(float value)
+{
+    stiffness_damping = value;
+    emit_changed();
+}
+
 
 void RopeStiffnessParameters::_bind_methods()
 {
@@ -58,12 +69,15 @@ void RopeStiffnessParameters::_bind_methods()
     ClassDB::bind_method(D_METHOD("set_stiffness_curve", "curve"), &RopeStiffnessParameters::set_stiffness_curve);
     ClassDB::bind_method(D_METHOD("get_stiffness_bend_curve"), &RopeStiffnessParameters::get_stiffness_bend_curve);
     ClassDB::bind_method(D_METHOD("set_stiffness_bend_curve", "curve"), &RopeStiffnessParameters::set_stiffness_bend_curve);
+    ClassDB::bind_method(D_METHOD("get_stiffness_damping"), &RopeStiffnessParameters::get_stiffness_damping);
+    ClassDB::bind_method(D_METHOD("set_stiffness_damping", "value"), &RopeStiffnessParameters::set_stiffness_damping);
 
     ADD_PROPERTY(PropertyInfo(Variant::INT, "stiffness_method", PROPERTY_HINT_ENUM, stiffness_method_strings), "set_stiffness_method", "get_stiffness_method");
     ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "stiffness"), "set_stiffness", "get_stiffness");
     ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "stiffness_curve", PROPERTY_HINT_RESOURCE_TYPE, "Curve"), "set_stiffness_curve", "get_stiffness_curve");
     ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "stiffness_bend_curve", PROPERTY_HINT_RESOURCE_TYPE, "Curve"), "set_stiffness_bend_curve", "get_stiffness_bend_curve");
+    ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "stiffness_damping"), "set_stiffness_damping", "get_stiffness_damping");
 
     BIND_ENUM_CONSTANT(Bidirectional);
-    BIND_ENUM_CONSTANT(Legacy);
+    BIND_ENUM_CONSTANT(Forward);
 }

src/RopeStiffnessParameters.hpp

@@ -12,8 +12,8 @@ class RopeStiffnessParameters : public Resource  // NOLINT(cppcoreguidelines-spe
 public:
     enum StiffnessMethod : int8_t  // NOLINT(cppcoreguidelines-use-enum-class)
     {
+        Forward,
         Bidirectional,
-        Legacy,
     };
 
     static constexpr const char* stiffness_method_strings = "Bidirectional,Legacy";
@@ -31,6 +31,9 @@ class RopeStiffnessParameters : public Resource  // NOLINT(cppcoreguidelines-spe
     Ref<Curve> get_stiffness_bend_curve() const;
     void set_stiffness_bend_curve(Ref<Curve> curve);
 
+    float get_stiffness_damping() const;
+    void set_stiffness_damping(float value);
+
 protected:
     static void _bind_methods();
 
@@ -39,8 +42,8 @@ class RopeStiffnessParameters : public Resource  // NOLINT(cppcoreguidelines-spe
     float stiffness = 0.0;
     Ref<Curve> stiffness_curve;
     Ref<Curve> stiffness_bend_curve;
+    float stiffness_damping = 0.0;
     // TODO: Stiffness direction for back and front
-    // TODO: Stiffness damping
 };
 
 }