Implement collisions with physics objects

Ropes can now collide with Godot physics objects.

Closes #2

Author: mphe

.clang-tidy

@@ -8,6 +8,7 @@ Checks:
   - -cppcoreguidelines-init-variables  # Produces false positves when initialization happens later
   - -cppcoreguidelines-avoid-magic-numbers  # This also triggers with literals like 3.1415
   - -cppcoreguidelines-avoid-non-const-global-variables
+  - -cppcoreguidelines-owning-memory
   - misc-*
   - -misc-non-private-member-variables-in-classes
   - -misc-use-anonymous-namespace
@@ -21,3 +22,4 @@ Checks:
   - -readability-redundant-access-specifiers
   - -readability-implicit-bool-conversion
   - -readability-math-missing-parentheses
+  - -readability-avoid-unconditional-preprocessor-if

.gitignore

@@ -10,6 +10,7 @@ compile_commands.json
 *.so
 *.exe
 *.dylib
+*.o
 
 # Godot-specific ignores
 .godot/

demo/addons/ropesim/Rope.gd

@@ -72,7 +72,7 @@ signal on_point_count_changed()
 ## Whether to fixate the first point at the rope's node position.
 @export var fixate_begin: bool = true
 
-## Render rope points for debugging purposes.
+## Render rope points and collision radius for debugging purposes.
 @export var render_debug: bool = false: set = _set_draw_debug
 
 ## Render the rope as line.
@@ -87,6 +87,33 @@ signal on_point_count_changed()
 ## Color gradient along the rendered line.
 @export var color_gradient: Gradient: set = _set_gradient
 
+@export_group("Collisions")
+
+## Enable collisions with physics bodies.
+## Collision checks are only performed for rope points, not whole segments.[br]
+## The performance impact is relatively cheap.[br]
+## [br]
+## If more precision is required to prevent tunneling or stretching between points, the number of
+## segments, the collision radius and the amount of constraint iterations can be tweaked.
+## There is also the [member resolve_collisions_while_constraining] option as last resort.
+@export var enable_collisions: bool = false
+
+## Collision radius around each rope point.
+@export var collision_radius: float = 1.0
+
+## Velocity damping after collisions. Gets applied in addition to regular damping.
+## Allows to make the rope appear smoother or rougher, i.e. make it slide more or less.
+@export var collision_damping: float = 0.0
+
+## Resolves collisions after each constraint iteration instead of once after the constraint step.
+## Increases precision but also drastically increases computation time.
+## Negligible with a single rope but expensive with many ropes.
+@export var resolve_collisions_while_constraining: bool = false
+
+## Physics layers to collide with.
+@export_flags_2d_physics var collision_mask: int = 1
+
+
 var _registered: bool = false
 var _colors := PackedColorArray()
 var _seg_lengths := PackedFloat32Array()
@@ -127,6 +154,9 @@ func _draw() -> void:
         for i in _points.size():
             draw_circle(_points[i], line_width / 2, Color.RED)
 
+            if enable_collisions:
+                draw_circle(_points[i], collision_radius, Color.CYAN, false)
+
 
 # Logic
 

demo/rope_examples/benchmark.tscn

@@ -11,7 +11,7 @@ offset_top = -1.0
 offset_right = 509.0
 offset_bottom = 22.0
 text = "384 Ropes
-3.99 ms"
+8.30 ms"
 script = ExtResource("2_ms00a")
 
 [node name="Node2D9" type="Node2D" parent="."]

src/NativeRopeContext.cpp

@@ -1,4 +1,10 @@
 #include "NativeRopeContext.hpp"
+#include "godot_cpp/classes/physics_direct_space_state2d.hpp"
+#include "godot_cpp/classes/physics_server2d.hpp"
+#include "godot_cpp/classes/physics_shape_query_parameters2d.hpp"
+#include "godot_cpp/classes/world2d.hpp"
+#include <godot_cpp/classes/window.hpp>
+#include <godot_cpp/variant/utility_functions.hpp>
 
 using namespace godot;
 
@@ -16,6 +22,19 @@ static Vector2 damp_vec(Vector2 value, float damping_factor, double delta)
     return value.lerp(VECTOR_ZERO, (float)(1.0 - exp(-damping_factor * delta)));
 }
 
+NativeRopeContext::NativeRopeContext() :
+    shape_query(memnew(PhysicsShapeQueryParameters2D))
+{
+    PhysicsServer2D* physics_server = PhysicsServer2D::get_singleton();
+    cast_shape_rid = physics_server->circle_shape_create();
+    shape_query->set_shape_rid(cast_shape_rid);
+}
+
+NativeRopeContext::~NativeRopeContext()
+{
+    PhysicsServer2D* physics_server = PhysicsServer2D::get_singleton();
+    physics_server->free_rid(cast_shape_rid);
+}
 
 bool NativeRopeContext::validate() const
 {
@@ -42,6 +61,11 @@ void NativeRopeContext::load_context(Node2D* rope)
     fixate_begin = rope->get("fixate_begin");
     resolve_to_begin = rope->get("resolve_to_begin");
     resolve_to_end = rope->get("resolve_to_end");
+    enable_collisions = rope->get("enable_collisions");
+    collision_radius = rope->get("collision_radius");
+    collision_mask = rope->get("collision_mask");
+    collision_damping = rope->get("collision_damping");
+    resolve_collisions_while_constraining = rope->get("resolve_collisions_while_constraining");
 }
 
 void NativeRopeContext::simulate(double delta)
@@ -58,7 +82,10 @@ void NativeRopeContext::simulate(double delta)
     }
 
     _simulate_velocities(delta);
-    _constraint();
+    _constraint(delta);
+
+    if (!resolve_collisions_while_constraining)
+        _resolve_collisions(delta);
 
     if (fixate_begin)
         simulation_weights[0] = backup_multiplier_begin;
@@ -168,7 +195,7 @@ static void constraint_segment(Vector2* point_a, Vector2* point_b, float weight_
     *point_b += (weight_b + weight_b * (1.0 - weight_a)) * dir;
 }
 
-void NativeRopeContext::_constraint()
+void NativeRopeContext::_constraint(double delta)
 {
     const bool use_euclid_constraint = max_endpoint_distance > 0;
 
@@ -205,10 +232,44 @@ void NativeRopeContext::_constraint()
         }
     }
 
-
     for (int _ = 0; _ < num_constraint_iterations; ++_)
     {
         for (int i = 0; i < points.size() - 1; ++i)
             constraint_segment(&points[i], &points[i + 1], simulation_weights[i], simulation_weights[i + 1], seg_lengths[i]);
+
+        if (resolve_collisions_while_constraining)
+            _resolve_collisions(delta);
     }
 }
+
+void NativeRopeContext::_resolve_collisions(double delta)
+{
+    if (!enable_collisions)
+        return;
+
+    PhysicsDirectSpaceState2D* space = rope->get_world_2d()->get_direct_space_state();
+    PhysicsServer2D* physics_server = PhysicsServer2D::get_singleton();
+    Transform2D transform;
+
+    physics_server->shape_set_data(cast_shape_rid, collision_radius);
+    shape_query->set_collision_mask(collision_mask);
+
+    for (int i = 0; i < points.size(); ++i)
+    {
+        transform.set_origin(points[i]);
+        shape_query->set_transform(transform);
+        const Dictionary rest_info = space->get_rest_info(shape_query);
+
+        if (rest_info.is_empty())
+            continue;
+
+        const Vector2 intersect_point = rest_info["point"];
+        const Vector2 intersect_normal = rest_info["normal"];
+        const Vector2 safe_point = intersect_point + intersect_normal * (collision_radius + CMP_EPSILON);
+        const Vector2 safe_motion = (safe_point - points[i]) * simulation_weights[i];
+        const Vector2 new_point = points[i] + safe_motion;
+
+        points[i] = oldpoints[i] + damp_vec(new_point - oldpoints[i], collision_damping, delta);
+    }
+}
+

src/NativeRopeContext.hpp

@@ -1,8 +1,8 @@
-#ifndef NATIVE_ROPE_CONTEXT_HPP
-#define NATIVE_ROPE_CONTEXT_HPP
+#pragma once
 
 #include "godot_cpp/classes/curve.hpp"
 #include "godot_cpp/classes/node2d.hpp"
+#include "godot_cpp/classes/physics_shape_query_parameters2d.hpp"
 
 namespace godot
 {
@@ -11,14 +11,18 @@ namespace godot
     class NativeRopeContext
     {
         public:
-            void load_context(Node2D* rope);
+            NativeRopeContext();
+            ~NativeRopeContext();
+
+            void load_context(Node2D *rope);
             void simulate(double delta);
             bool validate() const;
 
         protected:
             void _simulate_velocities(double delta);
             void _simulate_stiffness(PackedVector2Array* velocities) const;
-            void _constraint();
+            void _resolve_collisions(double delta);
+            void _constraint(double delta);
 
         public:
             Node2D* rope = nullptr;
@@ -31,12 +35,17 @@ namespace godot
             float damping = 0.0;
             float stiffness = 0.0;
             float max_endpoint_distance = 0.0;
+            float collision_radius = 1.0;
+            float collision_damping = 0.0;
             int num_constraint_iterations = 0;
+            int collision_mask = 0;
             Ref<Curve> damping_curve;
+            Ref<PhysicsShapeQueryParameters2D> shape_query;
+            RID cast_shape_rid;
             bool fixate_begin = true;
             bool resolve_to_begin = false;
             bool resolve_to_end = false;
+            bool enable_collisions = false;
+            bool resolve_collisions_while_constraining = false;
     };
 }
-
-#endif