1 files changed, 92 insertions, 88 deletions

diff --git a/scene/3d/skeleton_ik_3d.cpp b/scene/3d/skeleton_ik_3d.cpp
index 7366290ed3..466f67afb8 100644
--- a/scene/3d/skeleton_ik_3d.cpp
+++ b/scene/3d/skeleton_ik_3d.cpp
@@ -5,8 +5,8 @@
 /*                           GODOT ENGINE                                */
 /*                      https://godotengine.org                          */
 /*************************************************************************/
-/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md).   */
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md).   */
 /*                                                                       */
 /* Permission is hereby granted, free of charge, to any person obtaining */
 /* a copy of this software and associated documentation files (the       */
@@ -55,7 +55,6 @@ FabrikInverseKinematic::ChainItem *FabrikInverseKinematic::ChainItem::add_child(
 
 /// Build a chain that starts from the root to tip
 bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain) {
-
 	ERR_FAIL_COND_V(-1 == p_task->root_bone, false);
 
 	Chain &chain(p_task->chain);
@@ -64,7 +63,6 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain
 	chain.chain_root.bone = p_task->root_bone;
 	chain.chain_root.initial_transform = p_task->skeleton->get_bone_global_pose(chain.chain_root.bone);
 	chain.chain_root.current_pos = chain.chain_root.initial_transform.origin;
-	chain.chain_root.pb = p_task->skeleton->get_physical_bone(chain.chain_root.bone);
 	chain.middle_chain_item = nullptr;
 
 	// Holds all IDs that are composing a single chain in reverse order
@@ -75,7 +73,6 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain
 	chain_ids.resize(p_task->skeleton->get_bone_count());
 
 	for (int x = p_task->end_effectors.size() - 1; 0 <= x; --x) {
-
 		const EndEffector *ee(&p_task->end_effectors[x]);
 		ERR_FAIL_COND_V(p_task->root_bone >= ee->tip_bone, false);
 		ERR_FAIL_INDEX_V(ee->tip_bone, p_task->skeleton->get_bone_count(), false);
@@ -84,25 +81,20 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain
 		// Picks all IDs that composing a single chain in reverse order (except the root)
 		BoneId chain_sub_tip(ee->tip_bone);
 		while (chain_sub_tip > p_task->root_bone) {
-
 			chain_ids.write[sub_chain_size++] = chain_sub_tip;
 			chain_sub_tip = p_task->skeleton->get_bone_parent(chain_sub_tip);
 		}
 
-		BoneId middle_chain_item_id = (((float)sub_chain_size) * 0.5);
+		BoneId middle_chain_item_id = (BoneId)(sub_chain_size * 0.5);
 
 		// Build chain by reading chain ids in reverse order
 		// For each chain item id will be created a ChainItem if doesn't exists
 		ChainItem *sub_chain(&chain.chain_root);
 		for (int i = sub_chain_size - 1; 0 <= i; --i) {
-
 			ChainItem *child_ci(sub_chain->find_child(chain_ids[i]));
 			if (!child_ci) {
-
 				child_ci = sub_chain->add_child(chain_ids[i]);
 
-				child_ci->pb = p_task->skeleton->get_physical_bone(child_ci->bone);
-
 				child_ci->initial_transform = p_task->skeleton->get_bone_global_pose(child_ci->bone);
 				child_ci->current_pos = child_ci->initial_transform.origin;
 
@@ -118,8 +110,9 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain
 			}
 		}
 
-		if (!middle_chain_item_id)
+		if (!middle_chain_item_id) {
 			chain.middle_chain_item = nullptr;
+		}
 
 		// Initialize current tip
 		chain.tips.write[x].chain_item = sub_chain;
@@ -127,7 +120,7 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain
 
 		if (p_force_simple_chain) {
 			// NOTE:
-			//	This is an "hack" that force to create only one tip per chain since the solver of multi tip (end effector)
+			//	This is a "hack" that force to create only one tip per chain since the solver of multi tip (end effector)
 			//	is not yet created.
 			//	Remove this code when this is done
 			break;
@@ -136,22 +129,7 @@ bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain
 	return true;
 }
 
-void FabrikInverseKinematic::update_chain(const Skeleton3D *p_sk, ChainItem *p_chain_item) {
-
-	if (!p_chain_item)
-		return;
-
-	p_chain_item->initial_transform = p_sk->get_bone_global_pose(p_chain_item->bone);
-	p_chain_item->current_pos = p_chain_item->initial_transform.origin;
-
-	ChainItem *items = p_chain_item->children.ptrw();
-	for (int i = 0; i < p_chain_item->children.size(); i += 1) {
-		update_chain(p_sk, items + i);
-	}
-}
-
-void FabrikInverseKinematic::solve_simple(Task *p_task, bool p_solve_magnet) {
-
+void FabrikInverseKinematic::solve_simple(Task *p_task, bool p_solve_magnet, Vector3 p_origin_pos) {
 	real_t distance_to_goal(1e4);
 	real_t previous_distance_to_goal(0);
 	int can_solve(p_task->max_iterations);
@@ -160,14 +138,13 @@ void FabrikInverseKinematic::solve_simple(Task *p_task, bool p_solve_magnet) {
 		--can_solve;
 
 		solve_simple_backwards(p_task->chain, p_solve_magnet);
-		solve_simple_forwards(p_task->chain, p_solve_magnet);
+		solve_simple_forwards(p_task->chain, p_solve_magnet, p_origin_pos);
 
 		distance_to_goal = (p_task->chain.tips[0].chain_item->current_pos - p_task->chain.tips[0].end_effector->goal_transform.origin).length();
 	}
 }
 
 void FabrikInverseKinematic::solve_simple_backwards(Chain &r_chain, bool p_solve_magnet) {
-
 	if (p_solve_magnet && !r_chain.middle_chain_item) {
 		return;
 	}
@@ -199,20 +176,18 @@ void FabrikInverseKinematic::solve_simple_backwards(Chain &r_chain, bool p_solve
 	}
 }
 
-void FabrikInverseKinematic::solve_simple_forwards(Chain &r_chain, bool p_solve_magnet) {
-
+void FabrikInverseKinematic::solve_simple_forwards(Chain &r_chain, bool p_solve_magnet, Vector3 p_origin_pos) {
 	if (p_solve_magnet && !r_chain.middle_chain_item) {
 		return;
 	}
 
 	ChainItem *sub_chain_root(&r_chain.chain_root);
-	Vector3 origin(r_chain.chain_root.initial_transform.origin);
+	Vector3 origin = p_origin_pos;
 
 	while (sub_chain_root) { // Reach the tip
 		sub_chain_root->current_pos = origin;
 
-		if (!sub_chain_root->children.empty()) {
-
+		if (!sub_chain_root->children.is_empty()) {
 			ChainItem &child(sub_chain_root->children.write[0]);
 
 			// Is not tip
@@ -231,15 +206,13 @@ void FabrikInverseKinematic::solve_simple_forwards(Chain &r_chain, bool p_solve_
 				sub_chain_root = &child;
 			}
 		} else {
-
 			// Is tip
 			sub_chain_root = nullptr;
 		}
 	}
 }
 
-FabrikInverseKinematic::Task *FabrikInverseKinematic::create_simple_task(Skeleton3D *p_sk, BoneId root_bone, BoneId tip_bone, const Transform &goal_transform) {
-
+FabrikInverseKinematic::Task *FabrikInverseKinematic::create_simple_task(Skeleton3D *p_sk, BoneId root_bone, BoneId tip_bone, const Transform3D &goal_transform) {
 	FabrikInverseKinematic::EndEffector ee;
 	ee.tip_bone = tip_bone;
 
@@ -258,23 +231,22 @@ FabrikInverseKinematic::Task *FabrikInverseKinematic::create_simple_task(Skeleto
 }
 
 void FabrikInverseKinematic::free_task(Task *p_task) {
-	if (p_task)
+	if (p_task) {
 		memdelete(p_task);
+	}
 }
 
-void FabrikInverseKinematic::set_goal(Task *p_task, const Transform &p_goal) {
+void FabrikInverseKinematic::set_goal(Task *p_task, const Transform3D &p_goal) {
 	p_task->goal_global_transform = p_goal;
 }
 
-void FabrikInverseKinematic::make_goal(Task *p_task, const Transform &p_inverse_transf, real_t blending_delta) {
-
+void FabrikInverseKinematic::make_goal(Task *p_task, const Transform3D &p_inverse_transf, real_t blending_delta) {
 	if (blending_delta >= 0.99f) {
 		// Update the end_effector (local transform) without blending
 		p_task->end_effectors.write[0].goal_transform = p_inverse_transf * p_task->goal_global_transform;
 	} else {
-
 		// End effector in local transform
-		const Transform end_effector_pose(p_task->skeleton->get_bone_global_pose(p_task->end_effectors.write[0].tip_bone));
+		const Transform3D end_effector_pose(p_task->skeleton->get_bone_global_pose_no_override(p_task->end_effectors[0].tip_bone));
 
 		// Update the end_effector (local transform) by blending with current pose
 		p_task->end_effectors.write[0].goal_transform = end_effector_pose.interpolate_with(p_inverse_transf * p_task->goal_global_transform, blending_delta);
@@ -282,31 +254,43 @@ void FabrikInverseKinematic::make_goal(Task *p_task, const Transform &p_inverse_
 }
 
 void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool override_tip_basis, bool p_use_magnet, const Vector3 &p_magnet_position) {
-
 	if (blending_delta <= 0.01f) {
+		// Before skipping, make sure we undo the global pose overrides
+		ChainItem *ci(&p_task->chain.chain_root);
+		while (ci) {
+			p_task->skeleton->set_bone_global_pose_override(ci->bone, ci->initial_transform, 0.0, false);
+
+			if (!ci->children.is_empty()) {
+				ci = &ci->children.write[0];
+			} else {
+				ci = nullptr;
+			}
+		}
+
 		return; // Skip solving
 	}
 
-	p_task->skeleton->clear_bones_global_pose_override();
+	// Update the initial root transform so its synced with any animation changes
+	_update_chain(p_task->skeleton, &p_task->chain.chain_root);
 
-	make_goal(p_task, p_task->skeleton->get_global_transform().affine_inverse().scaled(p_task->skeleton->get_global_transform().get_basis().get_scale()), blending_delta);
+	p_task->skeleton->set_bone_global_pose_override(p_task->chain.chain_root.bone, Transform3D(), 0.0, false);
+	Vector3 origin_pos = p_task->skeleton->get_bone_global_pose(p_task->chain.chain_root.bone).origin;
 
-	update_chain(p_task->skeleton, &p_task->chain.chain_root);
+	make_goal(p_task, p_task->skeleton->get_global_transform().affine_inverse(), blending_delta);
 
 	if (p_use_magnet && p_task->chain.middle_chain_item) {
-		p_task->chain.magnet_position = p_task->chain.middle_chain_item->initial_transform.origin.linear_interpolate(p_magnet_position, blending_delta);
-		solve_simple(p_task, true);
+		p_task->chain.magnet_position = p_task->chain.middle_chain_item->initial_transform.origin.lerp(p_magnet_position, blending_delta);
+		solve_simple(p_task, true, origin_pos);
 	}
-	solve_simple(p_task, false);
+	solve_simple(p_task, false, origin_pos);
 
 	// Assign new bone position.
 	ChainItem *ci(&p_task->chain.chain_root);
 	while (ci) {
-		Transform new_bone_pose(ci->initial_transform);
+		Transform3D new_bone_pose(ci->initial_transform);
 		new_bone_pose.origin = ci->current_pos;
 
-		if (!ci->children.empty()) {
-
+		if (!ci->children.is_empty()) {
 			/// Rotate basis
 			const Vector3 initial_ori((ci->children[0].initial_transform.origin - ci->initial_transform.origin).normalized());
 			const Vector3 rot_axis(initial_ori.cross(ci->current_ori).normalized());
@@ -315,40 +299,58 @@ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool ove
 				const real_t rot_angle(Math::acos(CLAMP(initial_ori.dot(ci->current_ori), -1, 1)));
 				new_bone_pose.basis.rotate(rot_axis, rot_angle);
 			}
+
 		} else {
 			// Set target orientation to tip
-			if (override_tip_basis)
+			if (override_tip_basis) {
 				new_bone_pose.basis = p_task->chain.tips[0].end_effector->goal_transform.basis;
-			else
+			} else {
 				new_bone_pose.basis = new_bone_pose.basis * p_task->chain.tips[0].end_effector->goal_transform.basis;
+			}
 		}
 
+		// IK should not affect scale, so undo any scaling
+		new_bone_pose.basis.orthonormalize();
+		new_bone_pose.basis.scale(p_task->skeleton->get_bone_global_pose(ci->bone).basis.get_scale());
+
 		p_task->skeleton->set_bone_global_pose_override(ci->bone, new_bone_pose, 1.0, true);
 
-		if (!ci->children.empty())
+		if (!ci->children.is_empty()) {
 			ci = &ci->children.write[0];
-		else
+		} else {
 			ci = nullptr;
+		}
 	}
 }
 
-void SkeletonIK3D::_validate_property(PropertyInfo &property) const {
+void FabrikInverseKinematic::_update_chain(const Skeleton3D *p_sk, ChainItem *p_chain_item) {
+	if (!p_chain_item) {
+		return;
+	}
 
-	if (property.name == "root_bone" || property.name == "tip_bone") {
+	p_chain_item->initial_transform = p_sk->get_bone_global_pose_no_override(p_chain_item->bone);
+	p_chain_item->current_pos = p_chain_item->initial_transform.origin;
 
-		if (skeleton) {
+	ChainItem *items = p_chain_item->children.ptrw();
+	for (int i = 0; i < p_chain_item->children.size(); i += 1) {
+		_update_chain(p_sk, items + i);
+	}
+}
 
+void SkeletonIK3D::_validate_property(PropertyInfo &property) const {
+	if (property.name == "root_bone" || property.name == "tip_bone") {
+		if (skeleton) {
 			String names("--,");
 			for (int i = 0; i < skeleton->get_bone_count(); i++) {
-				if (i > 0)
+				if (i > 0) {
 					names += ",";
+				}
 				names += skeleton->get_bone_name(i);
 			}
 
 			property.hint = PROPERTY_HINT_ENUM;
 			property.hint_string = names;
 		} else {
-
 			property.hint = PROPERTY_HINT_NONE;
 			property.hint_string = "";
 		}
@@ -356,7 +358,6 @@ void SkeletonIK3D::_validate_property(PropertyInfo &property) const {
 }
 
 void SkeletonIK3D::_bind_methods() {
-
 	ClassDB::bind_method(D_METHOD("set_root_bone", "root_bone"), &SkeletonIK3D::set_root_bone);
 	ClassDB::bind_method(D_METHOD("get_root_bone"), &SkeletonIK3D::get_root_bone);
 
@@ -396,7 +397,7 @@ void SkeletonIK3D::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "root_bone"), "set_root_bone", "get_root_bone");
 	ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "tip_bone"), "set_tip_bone", "get_tip_bone");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "interpolation", PROPERTY_HINT_RANGE, "0,1,0.001"), "set_interpolation", "get_interpolation");
-	ADD_PROPERTY(PropertyInfo(Variant::TRANSFORM, "target"), "set_target_transform", "get_target_transform");
+	ADD_PROPERTY(PropertyInfo(Variant::TRANSFORM3D, "target"), "set_target_transform", "get_target_transform");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "override_tip_basis"), "set_override_tip_basis", "is_override_tip_basis");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_magnet"), "set_use_magnet", "is_using_magnet");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "magnet"), "set_magnet_position", "get_magnet_position");
@@ -413,9 +414,9 @@ void SkeletonIK3D::_notification(int p_what) {
 			reload_chain();
 		} break;
 		case NOTIFICATION_INTERNAL_PROCESS: {
-
-			if (target_node_override)
+			if (target_node_override) {
 				reload_goal();
+			}
 
 			_solve_chain();
 
@@ -426,15 +427,7 @@ void SkeletonIK3D::_notification(int p_what) {
 	}
 }
 
-SkeletonIK3D::SkeletonIK3D() :
-		interpolation(1),
-		override_tip_basis(true),
-		use_magnet(false),
-		min_distance(0.01),
-		max_iterations(10),
-		skeleton(nullptr),
-		target_node_override(nullptr),
-		task(nullptr) {
+SkeletonIK3D::SkeletonIK3D() {
 }
 
 SkeletonIK3D::~SkeletonIK3D() {
@@ -468,12 +461,12 @@ real_t SkeletonIK3D::get_interpolation() const {
 	return interpolation;
 }
 
-void SkeletonIK3D::set_target_transform(const Transform &p_target) {
+void SkeletonIK3D::set_target_transform(const Transform3D &p_target) {
 	target = p_target;
 	reload_goal();
 }
 
-const Transform &SkeletonIK3D::get_target_transform() const {
+const Transform3D &SkeletonIK3D::get_target_transform() const {
 	return target;
 }
 
@@ -526,6 +519,11 @@ bool SkeletonIK3D::is_running() {
 void SkeletonIK3D::start(bool p_one_time) {
 	if (p_one_time) {
 		set_process_internal(false);
+
+		if (target_node_override) {
+			reload_goal();
+		}
+
 		_solve_chain();
 	} else {
 		set_process_internal(true);
@@ -534,26 +532,30 @@ void SkeletonIK3D::start(bool p_one_time) {
 
 void SkeletonIK3D::stop() {
 	set_process_internal(false);
+	if (skeleton) {
+		skeleton->clear_bones_global_pose_override();
+	}
 }
 
-Transform SkeletonIK3D::_get_target_transform() {
-
-	if (!target_node_override && !target_node_path_override.is_empty())
+Transform3D SkeletonIK3D::_get_target_transform() {
+	if (!target_node_override && !target_node_path_override.is_empty()) {
 		target_node_override = Object::cast_to<Node3D>(get_node(target_node_path_override));
+	}
 
-	if (target_node_override)
+	if (target_node_override && target_node_override->is_inside_tree()) {
 		return target_node_override->get_global_transform();
-	else
+	} else {
 		return target;
+	}
 }
 
 void SkeletonIK3D::reload_chain() {
-
 	FabrikInverseKinematic::free_task(task);
 	task = nullptr;
 
-	if (!skeleton)
+	if (!skeleton) {
 		return;
+	}
 
 	task = FabrikInverseKinematic::create_simple_task(skeleton, skeleton->find_bone(root_bone), skeleton->find_bone(tip_bone), _get_target_transform());
 	if (task) {
@@ -563,15 +565,17 @@ void SkeletonIK3D::reload_chain() {
 }
 
 void SkeletonIK3D::reload_goal() {
-	if (!task)
+	if (!task) {
 		return;
+	}
 
 	FabrikInverseKinematic::set_goal(task, _get_target_transform());
 }
 
 void SkeletonIK3D::_solve_chain() {
-	if (!task)
+	if (!task) {
 		return;
+	}
 	FabrikInverseKinematic::solve(task, interpolation, override_tip_basis, use_magnet, magnet_position);
 }