Support for kinematic_motion in StaticBody

Does the same thing as simulate motion from RigidBody in Kinematic mode,
and CharacterBody (previously KinematicBody).

Added support for constant linear/angular velocity with kinematic_motion
in StaticBody, which moves the body in physics.

Updated documentation for StaticBody and CharacterBody to describe their
functionalities more accurately.

4 files changed, 182 insertions, 12 deletions

diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index ed2378c2c3..75bcf2eef2 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -146,12 +146,22 @@ void PhysicsBody2D::remove_collision_exception_with(Node *p_node) {
 
 void StaticBody2D::set_constant_linear_velocity(const Vector2 &p_vel) {
 	constant_linear_velocity = p_vel;
-	PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY, constant_linear_velocity);
+
+	if (kinematic_motion) {
+		_update_kinematic_motion();
+	} else {
+		PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY, constant_linear_velocity);
+	}
 }
 
 void StaticBody2D::set_constant_angular_velocity(real_t p_vel) {
 	constant_angular_velocity = p_vel;
-	PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY, constant_angular_velocity);
+
+	if (kinematic_motion) {
+		_update_kinematic_motion();
+	} else {
+		PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY, constant_angular_velocity);
+	}
 }
 
 Vector2 StaticBody2D::get_constant_linear_velocity() const {
@@ -181,27 +191,74 @@ Ref<PhysicsMaterial> StaticBody2D::get_physics_material_override() const {
 	return physics_material_override;
 }
 
+void StaticBody2D::set_kinematic_motion_enabled(bool p_enabled) {
+	if (p_enabled == kinematic_motion) {
+		return;
+	}
+
+	kinematic_motion = p_enabled;
+
+	if (kinematic_motion) {
+		PhysicsServer2D::get_singleton()->body_set_mode(get_rid(), PhysicsServer2D::BODY_MODE_KINEMATIC);
+	} else {
+		PhysicsServer2D::get_singleton()->body_set_mode(get_rid(), PhysicsServer2D::BODY_MODE_STATIC);
+	}
+
+	_update_kinematic_motion();
+}
+
+bool StaticBody2D::is_kinematic_motion_enabled() const {
+	return kinematic_motion;
+}
+
+void StaticBody2D::_notification(int p_what) {
+	if (p_what == NOTIFICATION_INTERNAL_PHYSICS_PROCESS) {
+#ifdef TOOLS_ENABLED
+		if (Engine::get_singleton()->is_editor_hint()) {
+			return;
+		}
+#endif
+
+		ERR_FAIL_COND(!kinematic_motion);
+
+		real_t delta_time = get_physics_process_delta_time();
+
+		Transform2D new_transform = get_global_transform();
+
+		new_transform.translate(constant_linear_velocity * delta_time);
+		new_transform.set_rotation(new_transform.get_rotation() + constant_angular_velocity * delta_time);
+
+		PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_TRANSFORM, new_transform);
+
+		// Propagate transform change to node.
+		set_block_transform_notify(true);
+		set_global_transform(new_transform);
+		set_block_transform_notify(false);
+	}
+}
+
 void StaticBody2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_constant_linear_velocity", "vel"), &StaticBody2D::set_constant_linear_velocity);
 	ClassDB::bind_method(D_METHOD("set_constant_angular_velocity", "vel"), &StaticBody2D::set_constant_angular_velocity);
 	ClassDB::bind_method(D_METHOD("get_constant_linear_velocity"), &StaticBody2D::get_constant_linear_velocity);
 	ClassDB::bind_method(D_METHOD("get_constant_angular_velocity"), &StaticBody2D::get_constant_angular_velocity);
 
+	ClassDB::bind_method(D_METHOD("set_kinematic_motion_enabled", "enabled"), &StaticBody2D::set_kinematic_motion_enabled);
+	ClassDB::bind_method(D_METHOD("is_kinematic_motion_enabled"), &StaticBody2D::is_kinematic_motion_enabled);
+
 	ClassDB::bind_method(D_METHOD("set_physics_material_override", "physics_material_override"), &StaticBody2D::set_physics_material_override);
 	ClassDB::bind_method(D_METHOD("get_physics_material_override"), &StaticBody2D::get_physics_material_override);
 
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "physics_material_override", PROPERTY_HINT_RESOURCE_TYPE, "PhysicsMaterial"), "set_physics_material_override", "get_physics_material_override");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "constant_linear_velocity"), "set_constant_linear_velocity", "get_constant_linear_velocity");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "constant_angular_velocity"), "set_constant_angular_velocity", "get_constant_angular_velocity");
-	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "physics_material_override", PROPERTY_HINT_RESOURCE_TYPE, "PhysicsMaterial"), "set_physics_material_override", "get_physics_material_override");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "kinematic_motion"), "set_kinematic_motion_enabled", "is_kinematic_motion_enabled");
 }
 
 StaticBody2D::StaticBody2D() :
 		PhysicsBody2D(PhysicsServer2D::BODY_MODE_STATIC) {
 }
 
-StaticBody2D::~StaticBody2D() {
-}
-
 void StaticBody2D::_reload_physics_characteristics() {
 	if (physics_material_override.is_null()) {
 		PhysicsServer2D::get_singleton()->body_set_param(get_rid(), PhysicsServer2D::BODY_PARAM_BOUNCE, 0);
@@ -212,6 +269,23 @@ void StaticBody2D::_reload_physics_characteristics() {
 	}
 }
 
+void StaticBody2D::_update_kinematic_motion() {
+#ifdef TOOLS_ENABLED
+	if (Engine::get_singleton()->is_editor_hint()) {
+		return;
+	}
+#endif
+
+	if (kinematic_motion) {
+		if (!Math::is_zero_approx(constant_angular_velocity) || !constant_linear_velocity.is_equal_approx(Vector2())) {
+			set_physics_process_internal(true);
+			return;
+		}
+	}
+
+	set_physics_process_internal(false);
+}
+
 void RigidBody2D::_body_enter_tree(ObjectID p_id) {
 	Object *obj = ObjectDB::get_instance(p_id);
 	Node *node = Object::cast_to<Node>(obj);
diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h
index dfe3277a50..afe2eef55f 100644
--- a/scene/2d/physics_body_2d.h
+++ b/scene/2d/physics_body_2d.h
@@ -72,7 +72,10 @@ class StaticBody2D : public PhysicsBody2D {
 
 	Ref<PhysicsMaterial> physics_material_override;
 
+	bool kinematic_motion = false;
+
 protected:
+	void _notification(int p_what);
 	static void _bind_methods();
 
 public:
@@ -86,10 +89,14 @@ public:
 	real_t get_constant_angular_velocity() const;
 
 	StaticBody2D();
-	~StaticBody2D();
 
 private:
 	void _reload_physics_characteristics();
+
+	void _update_kinematic_motion();
+
+	void set_kinematic_motion_enabled(bool p_enabled);
+	bool is_kinematic_motion_enabled() const;
 };
 
 class RigidBody2D : public PhysicsBody2D {
diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index 13eeb29d9f..aa000d4d3a 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -206,14 +206,44 @@ Ref<PhysicsMaterial> StaticBody3D::get_physics_material_override() const {
 	return physics_material_override;
 }
 
+void StaticBody3D::set_kinematic_motion_enabled(bool p_enabled) {
+	if (p_enabled == kinematic_motion) {
+		return;
+	}
+
+	kinematic_motion = p_enabled;
+
+	if (kinematic_motion) {
+		PhysicsServer3D::get_singleton()->body_set_mode(get_rid(), PhysicsServer3D::BODY_MODE_KINEMATIC);
+	} else {
+		PhysicsServer3D::get_singleton()->body_set_mode(get_rid(), PhysicsServer3D::BODY_MODE_STATIC);
+	}
+
+	_update_kinematic_motion();
+}
+
+bool StaticBody3D::is_kinematic_motion_enabled() const {
+	return kinematic_motion;
+}
+
 void StaticBody3D::set_constant_linear_velocity(const Vector3 &p_vel) {
 	constant_linear_velocity = p_vel;
-	PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, constant_linear_velocity);
+
+	if (kinematic_motion) {
+		_update_kinematic_motion();
+	} else {
+		PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, constant_linear_velocity);
+	}
 }
 
 void StaticBody3D::set_constant_angular_velocity(const Vector3 &p_vel) {
 	constant_angular_velocity = p_vel;
-	PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY, constant_angular_velocity);
+
+	if (kinematic_motion) {
+		_update_kinematic_motion();
+	} else {
+		PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY, constant_angular_velocity);
+	}
 }
 
 Vector3 StaticBody3D::get_constant_linear_velocity() const {
@@ -224,26 +254,61 @@ Vector3 StaticBody3D::get_constant_angular_velocity() const {
 	return constant_angular_velocity;
 }
 
+void StaticBody3D::_notification(int p_what) {
+	if (p_what == NOTIFICATION_INTERNAL_PHYSICS_PROCESS) {
+#ifdef TOOLS_ENABLED
+		if (Engine::get_singleton()->is_editor_hint()) {
+			return;
+		}
+#endif
+
+		ERR_FAIL_COND(!kinematic_motion);
+
+		real_t delta_time = get_physics_process_delta_time();
+
+		Transform3D new_transform = get_global_transform();
+		new_transform.origin += constant_linear_velocity * delta_time;
+
+		real_t ang_vel = constant_angular_velocity.length();
+		if (!Math::is_zero_approx(ang_vel)) {
+			Vector3 ang_vel_axis = constant_angular_velocity / ang_vel;
+			Basis rot(ang_vel_axis, ang_vel * delta_time);
+			new_transform.basis = rot * new_transform.basis;
+			new_transform.orthonormalize();
+		}
+
+		PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_TRANSFORM, new_transform);
+
+		// Propagate transform change to node.
+		set_ignore_transform_notification(true);
+		set_global_transform(new_transform);
+		set_ignore_transform_notification(false);
+		_on_transform_changed();
+	}
+}
+
 void StaticBody3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_constant_linear_velocity", "vel"), &StaticBody3D::set_constant_linear_velocity);
 	ClassDB::bind_method(D_METHOD("set_constant_angular_velocity", "vel"), &StaticBody3D::set_constant_angular_velocity);
 	ClassDB::bind_method(D_METHOD("get_constant_linear_velocity"), &StaticBody3D::get_constant_linear_velocity);
 	ClassDB::bind_method(D_METHOD("get_constant_angular_velocity"), &StaticBody3D::get_constant_angular_velocity);
 
+	ClassDB::bind_method(D_METHOD("set_kinematic_motion_enabled", "enabled"), &StaticBody3D::set_kinematic_motion_enabled);
+	ClassDB::bind_method(D_METHOD("is_kinematic_motion_enabled"), &StaticBody3D::is_kinematic_motion_enabled);
+
 	ClassDB::bind_method(D_METHOD("set_physics_material_override", "physics_material_override"), &StaticBody3D::set_physics_material_override);
 	ClassDB::bind_method(D_METHOD("get_physics_material_override"), &StaticBody3D::get_physics_material_override);
 
 	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "physics_material_override", PROPERTY_HINT_RESOURCE_TYPE, "PhysicsMaterial"), "set_physics_material_override", "get_physics_material_override");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "constant_linear_velocity"), "set_constant_linear_velocity", "get_constant_linear_velocity");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "constant_angular_velocity"), "set_constant_angular_velocity", "get_constant_angular_velocity");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "kinematic_motion"), "set_kinematic_motion_enabled", "is_kinematic_motion_enabled");
 }
 
 StaticBody3D::StaticBody3D() :
 		PhysicsBody3D(PhysicsServer3D::BODY_MODE_STATIC) {
 }
 
-StaticBody3D::~StaticBody3D() {}
-
 void StaticBody3D::_reload_physics_characteristics() {
 	if (physics_material_override.is_null()) {
 		PhysicsServer3D::get_singleton()->body_set_param(get_rid(), PhysicsServer3D::BODY_PARAM_BOUNCE, 0);
@@ -254,6 +319,23 @@ void StaticBody3D::_reload_physics_characteristics() {
 	}
 }
 
+void StaticBody3D::_update_kinematic_motion() {
+#ifdef TOOLS_ENABLED
+	if (Engine::get_singleton()->is_editor_hint()) {
+		return;
+	}
+#endif
+
+	if (kinematic_motion) {
+		if (!constant_angular_velocity.is_equal_approx(Vector3()) || !constant_linear_velocity.is_equal_approx(Vector3())) {
+			set_physics_process_internal(true);
+			return;
+		}
+	}
+
+	set_physics_process_internal(false);
+}
+
 void RigidBody3D::_body_enter_tree(ObjectID p_id) {
 	Object *obj = ObjectDB::get_instance(p_id);
 	Node *node = Object::cast_to<Node>(obj);
diff --git a/scene/3d/physics_body_3d.h b/scene/3d/physics_body_3d.h
index a844903067..b513602ba8 100644
--- a/scene/3d/physics_body_3d.h
+++ b/scene/3d/physics_body_3d.h
@@ -82,7 +82,10 @@ class StaticBody3D : public PhysicsBody3D {
 
 	Ref<PhysicsMaterial> physics_material_override;
 
+	bool kinematic_motion = false;
+
 protected:
+	void _notification(int p_what);
 	static void _bind_methods();
 
 public:
@@ -96,10 +99,14 @@ public:
 	Vector3 get_constant_angular_velocity() const;
 
 	StaticBody3D();
-	~StaticBody3D();
 
 private:
 	void _reload_physics_characteristics();
+
+	void _update_kinematic_motion();
+
+	void set_kinematic_motion_enabled(bool p_enabled);
+	bool is_kinematic_motion_enabled() const;
 };
 
 class RigidBody3D : public PhysicsBody3D {