Added function to control 6DOF precision

8 files changed, 58 insertions, 1 deletions

diff --git a/modules/bullet/bullet_physics_server.cpp b/modules/bullet/bullet_physics_server.cpp
index 315afe3d72..7bc731e75e 100644
--- a/modules/bullet/bullet_physics_server.cpp
+++ b/modules/bullet/bullet_physics_server.cpp
@@ -1471,6 +1471,22 @@ bool BulletPhysicsServer::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis
 	return generic_6dof_joint->get_flag(p_axis, p_flag);
 }
 
+void BulletPhysicsServer::generic_6dof_joint_set_precision(RID p_joint, int p_precision) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_6DOF);
+	Generic6DOFJointBullet *generic_6dof_joint = static_cast<Generic6DOFJointBullet *>(joint);
+	generic_6dof_joint->set_precision(p_precision);
+}
+
+int BulletPhysicsServer::generic_6dof_joint_get_precision(RID p_joint) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0);
+	Generic6DOFJointBullet *generic_6dof_joint = static_cast<Generic6DOFJointBullet *>(joint);
+	return generic_6dof_joint->get_precision();
+}
+
 void BulletPhysicsServer::free(RID p_rid) {
 	if (shape_owner.owns(p_rid)) {
 
diff --git a/modules/bullet/bullet_physics_server.h b/modules/bullet/bullet_physics_server.h
index c8c782267e..0cea3f5ba6 100644
--- a/modules/bullet/bullet_physics_server.h
+++ b/modules/bullet/bullet_physics_server.h
@@ -375,6 +375,9 @@ public:
 	virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable);
 	virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag);
 
+	virtual void generic_6dof_joint_set_precision(RID p_joint, int precision);
+	virtual int generic_6dof_joint_get_precision(RID p_joint);
+
 	/* MISC */
 
 	virtual void free(RID p_rid);
diff --git a/modules/bullet/generic_6dof_joint_bullet.cpp b/modules/bullet/generic_6dof_joint_bullet.cpp
index a94b88d566..812dcd2d56 100644
--- a/modules/bullet/generic_6dof_joint_bullet.cpp
+++ b/modules/bullet/generic_6dof_joint_bullet.cpp
@@ -265,3 +265,11 @@ bool Generic6DOFJointBullet::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOF
 	ERR_FAIL_INDEX_V(p_axis, 3, false);
 	return flags[p_axis][p_flag];
 }
+
+void Generic6DOFJointBullet::set_precision(int p_precision) {
+	sixDOFConstraint->setOverrideNumSolverIterations(MAX(1, p_precision));
+}
+
+int Generic6DOFJointBullet::get_precision() const {
+	return sixDOFConstraint->getOverrideNumSolverIterations();
+}
diff --git a/modules/bullet/generic_6dof_joint_bullet.h b/modules/bullet/generic_6dof_joint_bullet.h
index 176127ed6c..848c3a10cd 100644
--- a/modules/bullet/generic_6dof_joint_bullet.h
+++ b/modules/bullet/generic_6dof_joint_bullet.h
@@ -68,6 +68,9 @@ public:
 
 	void set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value);
 	bool get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const;
+
+	void set_precision(int p_precision);
+	int get_precision() const;
 };
 
 #endif
diff --git a/scene/3d/physics_joint.cpp b/scene/3d/physics_joint.cpp
index 02c6b1d969..8fd86c940c 100644
--- a/scene/3d/physics_joint.cpp
+++ b/scene/3d/physics_joint.cpp
@@ -707,6 +707,9 @@ void Generic6DOFJoint::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_flag_z", "flag", "value"), &Generic6DOFJoint::set_flag_z);
 	ClassDB::bind_method(D_METHOD("get_flag_z", "flag"), &Generic6DOFJoint::get_flag_z);
 
+	ClassDB::bind_method(D_METHOD("set_precision", "precision"), &Generic6DOFJoint::set_precision);
+	ClassDB::bind_method(D_METHOD("get_precision"), &Generic6DOFJoint::get_precision);
+
 	ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "linear_limit_x/enabled"), "set_flag_x", "get_flag_x", FLAG_ENABLE_LINEAR_LIMIT);
 	ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_x/upper_distance"), "set_param_x", "get_param_x", PARAM_LINEAR_UPPER_LIMIT);
 	ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_x/lower_distance"), "set_param_x", "get_param_x", PARAM_LINEAR_LOWER_LIMIT);
@@ -795,6 +798,8 @@ void Generic6DOFJoint::_bind_methods() {
 	ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_spring_z/damping"), "set_param_z", "get_param_z", PARAM_ANGULAR_SPRING_DAMPING);
 	ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_spring_z/equilibrium_point"), "set_param_z", "get_param_z", PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT);
 
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "precision", PROPERTY_HINT_RANGE, "1,99999,1"), "set_precision", "get_precision");
+
 	BIND_ENUM_CONSTANT(PARAM_LINEAR_LOWER_LIMIT);
 	BIND_ENUM_CONSTANT(PARAM_LINEAR_UPPER_LIMIT);
 	BIND_ENUM_CONSTANT(PARAM_LINEAR_LIMIT_SOFTNESS);
@@ -907,6 +912,14 @@ bool Generic6DOFJoint::get_flag_z(Flag p_flag) const {
 	return flags_z[p_flag];
 }
 
+void Generic6DOFJoint::set_precision(int p_precision) {
+	precision = p_precision;
+
+	PhysicsServer::get_singleton()->generic_6dof_joint_set_precision(
+			get_joint(),
+			precision);
+}
+
 RID Generic6DOFJoint::_configure_joint(PhysicsBody *body_a, PhysicsBody *body_b) {
 
 	Transform gt = get_global_transform();
@@ -941,7 +954,8 @@ RID Generic6DOFJoint::_configure_joint(PhysicsBody *body_a, PhysicsBody *body_b)
 	return j;
 }
 
-Generic6DOFJoint::Generic6DOFJoint() {
+Generic6DOFJoint::Generic6DOFJoint() :
+		precision(1) {
 
 	set_param_x(PARAM_LINEAR_LOWER_LIMIT, 0);
 	set_param_x(PARAM_LINEAR_UPPER_LIMIT, 0);
diff --git a/scene/3d/physics_joint.h b/scene/3d/physics_joint.h
index ee4ca28658..753795da90 100644
--- a/scene/3d/physics_joint.h
+++ b/scene/3d/physics_joint.h
@@ -305,6 +305,8 @@ protected:
 	float params_z[PARAM_MAX];
 	bool flags_z[FLAG_MAX];
 
+	int precision;
+
 	virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b);
 	static void _bind_methods();
 
@@ -327,6 +329,11 @@ public:
 	void set_flag_z(Flag p_flag, bool p_enabled);
 	bool get_flag_z(Flag p_flag) const;
 
+	void set_precision(int p_precision);
+	int get_precision() const {
+		return precision;
+	}
+
 	Generic6DOFJoint();
 };
 
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index b3c61403aa..c361d00fcc 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -348,6 +348,9 @@ public:
 	virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable);
 	virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag);
 
+	virtual void generic_6dof_joint_set_precision(RID p_joint, int precision) {}
+	virtual int generic_6dof_joint_get_precision(RID p_joint) { return 0; }
+
 	virtual JointType joint_get_type(RID p_joint) const;
 
 	virtual void joint_set_solver_priority(RID p_joint, int p_priority);
diff --git a/servers/physics_server.h b/servers/physics_server.h
index 15b353f768..9fb5e958c3 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -734,6 +734,9 @@ public:
 	virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) = 0;
 	virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) = 0;
 
+	virtual void generic_6dof_joint_set_precision(RID p_joint, int precision) = 0;
+	virtual int generic_6dof_joint_get_precision(RID p_joint) = 0;
+
 	/* QUERY API */
 
 	enum AreaBodyStatus {