Allow double-axis lock in RigidBody and KinematicBody

6 files changed, 25 insertions, 41 deletions

diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
index f8cd6ca858..630f32cec1 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -559,32 +559,30 @@ void BodySW::integrate_velocities(real_t p_step) {
 	if (fi_callback)
 		get_space()->body_add_to_state_query_list(&direct_state_query_list);
 
-	if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {
-
-		_set_transform(new_transform, false);
-		_set_inv_transform(new_transform.affine_inverse());
-		if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3())
-			set_active(false); //stopped moving, deactivate
-
-		return;
-	}
-
 	//apply axis lock
-	if (axis_lock != PhysicsServer::BODY_AXIS_LOCK_DISABLED) {
-
-		int axis = axis_lock - 1;
+	if (locked_axis[0] || locked_axis[1] || locked_axis[2]) {
 		for (int i = 0; i < 3; i++) {
-			if (i == axis) {
+			if (locked_axis[i]) {
 				linear_velocity[i] = 0;
 				biased_linear_velocity[i] = 0;
+				new_transform.origin[i] = get_transform().origin[i];
 			} else {
-
 				angular_velocity[i] = 0;
 				biased_angular_velocity[i] = 0;
 			}
 		}
 	}
 
+	if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {
+
+		_set_transform(new_transform, false);
+		_set_inv_transform(new_transform.affine_inverse());
+		if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3())
+			set_active(false); //stopped moving, deactivate
+
+		return;
+	}
+
 	Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity;
 
 	real_t ang_vel = total_angular_velocity.length();
@@ -772,7 +770,6 @@ BodySW::BodySW()
 	continuous_cd = false;
 	can_sleep = false;
 	fi_callback = NULL;
-	axis_lock = PhysicsServer::BODY_AXIS_LOCK_DISABLED;
 }
 
 BodySW::~BodySW() {
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 738d99c764..aab6def1a9 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -53,7 +53,7 @@ class BodySW : public CollisionObjectSW {
 	real_t angular_damp;
 	real_t gravity_scale;
 
-	PhysicsServer::BodyAxisLock axis_lock;
+	bool locked_axis[3] = { false, false, false };
 
 	real_t kinematic_safe_margin;
 	real_t _inv_mass;
@@ -288,8 +288,8 @@ public:
 	_FORCE_INLINE_ Vector3 get_gravity() const { return gravity; }
 	_FORCE_INLINE_ real_t get_bounce() const { return bounce; }
 
-	_FORCE_INLINE_ void set_axis_lock(PhysicsServer::BodyAxisLock p_lock) { axis_lock = p_lock; }
-	_FORCE_INLINE_ PhysicsServer::BodyAxisLock get_axis_lock() const { return axis_lock; }
+	_FORCE_INLINE_ void set_axis_lock(int axis, bool lock) { locked_axis[axis] = lock; }
+	_FORCE_INLINE_ bool get_axis_lock() const { return locked_axis; }
 
 	void integrate_forces(real_t p_step);
 	void integrate_velocities(real_t p_step);
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index ce63d84617..2909308366 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -794,19 +794,19 @@ void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_v
 	body->wakeup();
 };
 
-void PhysicsServerSW::body_set_axis_lock(RID p_body, BodyAxisLock p_lock) {
+void PhysicsServerSW::body_set_axis_lock(RID p_body, int axis, bool lock) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_axis_lock(p_lock);
+	body->set_axis_lock(axis, lock);
 	body->wakeup();
 }
 
-PhysicsServerSW::BodyAxisLock PhysicsServerSW::body_get_axis_lock(RID p_body) const {
+bool PhysicsServerSW::body_get_axis_lock(RID p_body) const {
 
 	const BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body, BODY_AXIS_LOCK_DISABLED);
+	ERR_FAIL_COND_V(!body, 0);
 	return body->get_axis_lock();
 }
 
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index fa754a1c8f..fea6e34ebd 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -203,8 +203,8 @@ public:
 	virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse);
 	virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity);
 
-	virtual void body_set_axis_lock(RID p_body, BodyAxisLock p_lock);
-	virtual BodyAxisLock body_get_axis_lock(RID p_body) const;
+	virtual void body_set_axis_lock(RID p_body, int axis, bool p_lock);
+	virtual bool body_get_axis_lock(RID p_body) const;
 
 	virtual void body_add_collision_exception(RID p_body, RID p_body_b);
 	virtual void body_remove_collision_exception(RID p_body, RID p_body_b);
diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp
index 76fb5bc46b..8365b3adab 100644
--- a/servers/physics_server.cpp
+++ b/servers/physics_server.cpp
@@ -491,7 +491,7 @@ void PhysicsServer::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("body_apply_torque_impulse", "body", "impulse"), &PhysicsServer::body_apply_torque_impulse);
 	ClassDB::bind_method(D_METHOD("body_set_axis_velocity", "body", "axis_velocity"), &PhysicsServer::body_set_axis_velocity);
 
-	ClassDB::bind_method(D_METHOD("body_set_axis_lock", "body", "axis"), &PhysicsServer::body_set_axis_lock);
+	ClassDB::bind_method(D_METHOD("body_set_axis_lock", "body", "axis", "lock"), &PhysicsServer::body_set_axis_lock);
 	ClassDB::bind_method(D_METHOD("body_get_axis_lock", "body"), &PhysicsServer::body_get_axis_lock);
 
 	ClassDB::bind_method(D_METHOD("body_add_collision_exception", "body", "excepted_body"), &PhysicsServer::body_add_collision_exception);
@@ -720,11 +720,6 @@ void PhysicsServer::_bind_methods() {
 	BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP);
 	BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO);
 	BIND_ENUM_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS);
-
-	BIND_ENUM_CONSTANT(BODY_AXIS_LOCK_DISABLED);
-	BIND_ENUM_CONSTANT(BODY_AXIS_LOCK_X);
-	BIND_ENUM_CONSTANT(BODY_AXIS_LOCK_Y);
-	BIND_ENUM_CONSTANT(BODY_AXIS_LOCK_Z);
 }
 
 PhysicsServer::PhysicsServer() {
diff --git a/servers/physics_server.h b/servers/physics_server.h
index 64c67eae2a..7705fe1254 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -437,15 +437,8 @@ public:
 	virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) = 0;
 	virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) = 0;
 
-	enum BodyAxisLock {
-		BODY_AXIS_LOCK_DISABLED,
-		BODY_AXIS_LOCK_X,
-		BODY_AXIS_LOCK_Y,
-		BODY_AXIS_LOCK_Z,
-	};
-
-	virtual void body_set_axis_lock(RID p_body, BodyAxisLock p_lock) = 0;
-	virtual BodyAxisLock body_get_axis_lock(RID p_body) const = 0;
+	virtual void body_set_axis_lock(RID p_body, int axis, bool lock) = 0;
+	virtual bool body_get_axis_lock(RID p_body) const = 0;
 
 	//fix
 	virtual void body_add_collision_exception(RID p_body, RID p_body_b) = 0;
@@ -705,7 +698,6 @@ VARIANT_ENUM_CAST(PhysicsServer::AreaSpaceOverrideMode);
 VARIANT_ENUM_CAST(PhysicsServer::BodyMode);
 VARIANT_ENUM_CAST(PhysicsServer::BodyParameter);
 VARIANT_ENUM_CAST(PhysicsServer::BodyState);
-VARIANT_ENUM_CAST(PhysicsServer::BodyAxisLock);
 VARIANT_ENUM_CAST(PhysicsServer::PinJointParam);
 VARIANT_ENUM_CAST(PhysicsServer::JointType);
 VARIANT_ENUM_CAST(PhysicsServer::HingeJointParam);