From 5b25e6d6e97f461e220a9660f0efbf68350eddb9 Mon Sep 17 00:00:00 2001
From: Dragos Daian <daian.dragos@yahoo.com>
Date: Sun, 12 Nov 2023 13:52:24 +0100
Subject: [PATCH] also add spring joint impl

---
 src/box2d-wrapper/box2d_wrapper.cpp         | 30 +++++++++++++++++++++
 src/box2d-wrapper/box2d_wrapper.h           | 15 +++++++++++
 src/joints/box2d_damped_spring_joint_2d.cpp | 17 ++++++++----
 3 files changed, 57 insertions(+), 5 deletions(-)

diff --git a/src/box2d-wrapper/box2d_wrapper.cpp b/src/box2d-wrapper/box2d_wrapper.cpp
index 519492c..3efd431 100644
--- a/src/box2d-wrapper/box2d_wrapper.cpp
+++ b/src/box2d-wrapper/box2d_wrapper.cpp
@@ -831,6 +831,36 @@ b2Joint *box2d::joint_create_revolute(b2World *world_handle,
 	return world_handle->CreateJoint(&joint_def);
 }
 
+void box2d::joint_change_distance_joint(b2World *world_handle,
+		b2Joint *joint_handle,
+		real_t rest_length,
+		real_t stiffness,
+		real_t damping) {
+	b2DistanceJoint *joint = (b2DistanceJoint *)joint_handle;
+	joint->SetDamping(damping);
+	joint->SetStiffness(stiffness);
+	joint->SetLength(rest_length);
+}
+
+b2Joint *box2d::joint_create_distance_joint(b2World *world_handle,
+		b2Body *body_handle_1,
+		b2Body *body_handle_2,
+		const b2Vec2 anchor_1,
+		const b2Vec2 anchor_2,
+		real_t rest_length,
+		real_t stiffness,
+		real_t damping) {
+	b2DistanceJointDef joint_def;
+	joint_def.bodyA = body_handle_1;
+	joint_def.bodyB = body_handle_2;
+	joint_def.localAnchorA = anchor_1;
+	joint_def.localAnchorB = anchor_2;
+	joint_def.length = rest_length;
+	joint_def.stiffness = stiffness;
+	joint_def.damping = damping;
+	return world_handle->CreateJoint(&joint_def);
+}
+
 void box2d::joint_destroy(b2World *world_handle, b2Joint *joint_handle) {
 	world_handle->DestroyJoint(joint_handle);
 }
diff --git a/src/box2d-wrapper/box2d_wrapper.h b/src/box2d-wrapper/box2d_wrapper.h
index abb14ea..397c253 100644
--- a/src/box2d-wrapper/box2d_wrapper.h
+++ b/src/box2d-wrapper/box2d_wrapper.h
@@ -416,6 +416,21 @@ b2Joint *joint_create_revolute(b2World *world_handle,
 		real_t motor_target_velocity,
 		bool motor_enabled);
 
+b2Joint *joint_create_distance_joint(b2World *world_handle,
+		b2Body *body_handle_1,
+		b2Body *body_handle_2,
+		const b2Vec2 anchor_1,
+		const b2Vec2 anchor_2,
+		real_t rest_length,
+		real_t stiffness,
+		real_t damping);
+
+void joint_change_distance_joint(b2World *world_handle,
+		b2Joint *joint_handle,
+		real_t rest_length,
+		real_t stiffness,
+		real_t damping);
+
 void joint_destroy(b2World *world_handle, b2Joint *joint_handle);
 
 ShapeCastResult shape_casting(b2World *world_handle,
diff --git a/src/joints/box2d_damped_spring_joint_2d.cpp b/src/joints/box2d_damped_spring_joint_2d.cpp
index 191971c..035f0e2 100644
--- a/src/joints/box2d_damped_spring_joint_2d.cpp
+++ b/src/joints/box2d_damped_spring_joint_2d.cpp
@@ -1,4 +1,5 @@
 #include "box2d_damped_spring_joint_2d.h"
+#include "../spaces/box2d_space_2d.h"
 
 void Box2DDampedSpringJoint2D::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) {
 	switch (p_param) {
@@ -12,6 +13,9 @@ void Box2DDampedSpringJoint2D::set_param(PhysicsServer2D::DampedSpringParam p_pa
 			stiffness = p_value;
 		} break;
 	}
+	ERR_FAIL_COND(!box2d::is_handle_valid(space_handle));
+	ERR_FAIL_COND(!box2d::is_handle_valid(handle));
+	box2d::joint_change_distance_joint(space_handle, handle, rest_length, stiffness, damping);
 }
 
 real_t Box2DDampedSpringJoint2D::get_param(PhysicsServer2D::DampedSpringParam p_param) const {
@@ -37,10 +41,13 @@ Box2DDampedSpringJoint2D::Box2DDampedSpringJoint2D(const Vector2 &p_anchor_a, co
 
 	rest_length = p_anchor_a.distance_to(p_anchor_b);
 
-	// TODO: create rapier joint when available
-	// See https://github.com/dimforge/rapier/issues/241
-	ERR_FAIL_MSG("Spring joints not supported for now");
+	b2Vec2 box2d_anchor_A = { anchor_A.x, anchor_A.y };
+	b2Vec2 box2d_anchor_B = { anchor_B.x, anchor_B.y };
 
-	//A->add_constraint(this, 0);
-	//B->add_constraint(this, 1);
+	ERR_FAIL_COND(!p_body_a->get_space());
+	ERR_FAIL_COND(p_body_a->get_space() != p_body_b->get_space());
+	space_handle = p_body_a->get_space()->get_handle();
+	ERR_FAIL_COND(!box2d::is_handle_valid(space_handle));
+	handle = box2d::joint_create_distance_joint(space_handle, p_body_a->get_body_handle(), p_body_b->get_body_handle(), box2d_anchor_A, box2d_anchor_B, rest_length, stiffness, damping);
+	ERR_FAIL_COND(!box2d::is_handle_valid(handle));
 }