Fix 6DOF Physical Bone joint

Adding angular and linear springs param to PhysicalBone joint type JOINT_TYPE_6DOF,
using new 6DOF feautres implemented in sdfgeoff/godot@e149327.

Typo correction lenear_equilibrium_point to linear_equilibrium_point.

2 files changed, 89 insertions, 1 deletions

diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index 9148b436a0..bcfcf33e57 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -1907,6 +1907,26 @@ bool PhysicalBone::SixDOFJointData::_set(const StringName &p_name, const Variant
 		if (j.is_valid())
 			PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j, axis, PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS, axis_data[axis].linear_limit_softness);
 
+	} else if ("linear_spring_enabled" == var_name) {
+		axis_data[axis].linear_spring_enabled = p_value;
+		if (j.is_valid())
+			PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(j, axis, PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING, axis_data[axis].linear_spring_enabled);
+
+	} else if ("linear_spring_stiffness" == var_name) {
+		axis_data[axis].linear_spring_stiffness = p_value;
+		if (j.is_valid())
+			PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j, axis, PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS, axis_data[axis].linear_spring_stiffness);
+
+	} else if ("linear_spring_damping" == var_name) {
+		axis_data[axis].linear_spring_damping = p_value;
+		if (j.is_valid())
+			PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j, axis, PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING, axis_data[axis].linear_spring_damping);
+
+	} else if ("linear_equilibrium_point" == var_name) {
+		axis_data[axis].linear_equilibrium_point = p_value;
+		if (j.is_valid())
+			PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j, axis, PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT, axis_data[axis].linear_equilibrium_point);
+
 	} else if ("linear_restitution" == var_name) {
 		axis_data[axis].linear_restitution = p_value;
 		if (j.is_valid())
@@ -1952,6 +1972,26 @@ bool PhysicalBone::SixDOFJointData::_set(const StringName &p_name, const Variant
 		if (j.is_valid())
 			PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j, axis, PhysicsServer::G6DOF_JOINT_ANGULAR_ERP, axis_data[axis].erp);
 
+	} else if ("angular_spring_enabled" == var_name) {
+		axis_data[axis].angular_spring_enabled = p_value;
+		if (j.is_valid())
+			PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(j, axis, PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING, axis_data[axis].angular_spring_enabled);
+
+	} else if ("angular_spring_stiffness" == var_name) {
+		axis_data[axis].angular_spring_stiffness = p_value;
+		if (j.is_valid())
+			PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j, axis, PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS, axis_data[axis].angular_spring_stiffness);
+
+	} else if ("angular_spring_damping" == var_name) {
+		axis_data[axis].angular_spring_damping = p_value;
+		if (j.is_valid())
+			PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j, axis, PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING, axis_data[axis].angular_spring_damping);
+
+	} else if ("angular_equilibrium_point" == var_name) {
+		axis_data[axis].angular_equilibrium_point = p_value;
+		if (j.is_valid())
+			PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j, axis, PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT, axis_data[axis].angular_equilibrium_point);
+
 	} else {
 		return false;
 	}
@@ -1990,6 +2030,14 @@ bool PhysicalBone::SixDOFJointData::_get(const StringName &p_name, Variant &r_re
 		r_ret = axis_data[axis].linear_limit_lower;
 	} else if ("linear_limit_softness" == var_name) {
 		r_ret = axis_data[axis].linear_limit_softness;
+	} else if ("linear_spring_enabled" == var_name) {
+		r_ret = axis_data[axis].linear_spring_enabled;
+	} else if ("linear_spring_stiffness" == var_name) {
+		r_ret = axis_data[axis].linear_spring_stiffness;
+	} else if ("linear_spring_damping" == var_name) {
+		r_ret = axis_data[axis].linear_spring_damping;
+	} else if ("linear_equilibrium_point" == var_name) {
+		r_ret = axis_data[axis].linear_equilibrium_point;
 	} else if ("linear_restitution" == var_name) {
 		r_ret = axis_data[axis].linear_restitution;
 	} else if ("linear_damping" == var_name) {
@@ -2008,6 +2056,14 @@ bool PhysicalBone::SixDOFJointData::_get(const StringName &p_name, Variant &r_re
 		r_ret = axis_data[axis].angular_damping;
 	} else if ("erp" == var_name) {
 		r_ret = axis_data[axis].erp;
+	} else if ("angular_spring_enabled" == var_name) {
+		r_ret = axis_data[axis].angular_spring_enabled;
+	} else if ("angular_spring_stiffness" == var_name) {
+		r_ret = axis_data[axis].angular_spring_stiffness;
+	} else if ("angular_spring_damping" == var_name) {
+		r_ret = axis_data[axis].angular_spring_damping;
+	} else if ("angular_equilibrium_point" == var_name) {
+		r_ret = axis_data[axis].angular_equilibrium_point;
 	} else {
 		return false;
 	}
@@ -2022,6 +2078,10 @@ void PhysicalBone::SixDOFJointData::_get_property_list(List<PropertyInfo> *p_lis
 		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/linear_limit_upper"));
 		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/linear_limit_lower"));
 		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/linear_limit_softness", PROPERTY_HINT_RANGE, "0.01,16,0.01"));
+		p_list->push_back(PropertyInfo(Variant::BOOL, "joint_constraints/" + axis_names[i] + "/linear_spring_enabled"));
+		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/linear_spring_stiffness"));
+		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/linear_spring_damping"));
+		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/linear_equilibrium_point"));
 		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/linear_restitution", PROPERTY_HINT_RANGE, "0.01,16,0.01"));
 		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/linear_damping", PROPERTY_HINT_RANGE, "0.01,16,0.01"));
 		p_list->push_back(PropertyInfo(Variant::BOOL, "joint_constraints/" + axis_names[i] + "/angular_limit_enabled"));
@@ -2031,6 +2091,10 @@ void PhysicalBone::SixDOFJointData::_get_property_list(List<PropertyInfo> *p_lis
 		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/angular_restitution", PROPERTY_HINT_RANGE, "0.01,16,0.01"));
 		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/angular_damping", PROPERTY_HINT_RANGE, "0.01,16,0.01"));
 		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/erp"));
+		p_list->push_back(PropertyInfo(Variant::BOOL, "joint_constraints/" + axis_names[i] + "/angular_spring_enabled"));
+		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/angular_spring_stiffness"));
+		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/angular_spring_damping"));
+		p_list->push_back(PropertyInfo(Variant::REAL, "joint_constraints/" + axis_names[i] + "/angular_equilibrium_point"));
 	}
 }
 
@@ -2294,6 +2358,10 @@ void PhysicalBone::_reload_joint() {
 				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT, g6dofjd->axis_data[axis].linear_limit_upper);
 				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT, g6dofjd->axis_data[axis].linear_limit_lower);
 				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS, g6dofjd->axis_data[axis].linear_limit_softness);
+				PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING, g6dofjd->axis_data[axis].linear_spring_enabled);
+				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS, g6dofjd->axis_data[axis].linear_spring_stiffness);
+				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING, g6dofjd->axis_data[axis].linear_spring_damping);
+				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT, g6dofjd->axis_data[axis].linear_equilibrium_point);
 				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION, g6dofjd->axis_data[axis].linear_restitution);
 				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING, g6dofjd->axis_data[axis].linear_damping);
 				PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT, g6dofjd->axis_data[axis].angular_limit_enabled);
@@ -2303,6 +2371,10 @@ void PhysicalBone::_reload_joint() {
 				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION, g6dofjd->axis_data[axis].angular_restitution);
 				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING, g6dofjd->axis_data[axis].angular_damping);
 				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_ANGULAR_ERP, g6dofjd->axis_data[axis].erp);
+				PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING, g6dofjd->axis_data[axis].angular_spring_enabled);
+				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS, g6dofjd->axis_data[axis].angular_spring_stiffness);
+				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING, g6dofjd->axis_data[axis].angular_spring_damping);
+				PhysicsServer::get_singleton()->generic_6dof_joint_set_param(joint, static_cast<Vector3::Axis>(axis), PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT, g6dofjd->axis_data[axis].angular_equilibrium_point);
 			}
 
 		} break;
diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h
index 20d948b6eb..5474290c07 100644
--- a/scene/3d/physics_body.h
+++ b/scene/3d/physics_body.h
@@ -494,6 +494,10 @@ public:
 			real_t linear_limit_softness;
 			real_t linear_restitution;
 			real_t linear_damping;
+			bool linear_spring_enabled;
+			real_t linear_spring_stiffness;
+			real_t linear_spring_damping;
+			real_t linear_equilibrium_point;
 			bool angular_limit_enabled;
 			real_t angular_limit_upper;
 			real_t angular_limit_lower;
@@ -501,6 +505,10 @@ public:
 			real_t angular_restitution;
 			real_t angular_damping;
 			real_t erp;
+			bool angular_spring_enabled;
+			real_t angular_spring_stiffness;
+			real_t angular_spring_damping;
+			real_t angular_equilibrium_point;
 
 			SixDOFAxisData() :
 					linear_limit_enabled(true),
@@ -509,13 +517,21 @@ public:
 					linear_limit_softness(0.7),
 					linear_restitution(0.5),
 					linear_damping(1.),
+					linear_spring_enabled(false),
+					linear_spring_stiffness(0),
+					linear_spring_damping(0),
+					linear_equilibrium_point(0),
 					angular_limit_enabled(true),
 					angular_limit_upper(0),
 					angular_limit_lower(0),
 					angular_limit_softness(0.5),
 					angular_restitution(0),
 					angular_damping(1.),
-					erp(0.5) {}
+					erp(0.5),
+					angular_spring_enabled(false),
+					angular_spring_stiffness(0),
+					angular_spring_damping(0.),
+					angular_equilibrium_point(0) {}
 		};
 
 		virtual JointType get_joint_type() { return JOINT_TYPE_6DOF; }