godotengine/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-11-21 14:57:09 +00:00
Code Releases Wiki Activity
Files
ada95cb543f21bb9f39d5fc0d1f55bcdcf0480e4
godot/scene/3d/iterate_ik_3d.cpp
Silc Lizard (Tokage) Renew bf22eb25e3 Add IKModifier3D
2025-11-04 02:38:48 +09:00

555 lines
25 KiB
C++
Raw Blame History

/**************************************************************************/
/* iterate_ik_3d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "iterate_ik_3d.h"
bool IterateIK3D::_set(const StringName &p_path, const Variant &p_value) {
String path = p_path;
if (path.begins_with("settings/")) {
int which = path.get_slicec('/', 1).to_int();
String what = path.get_slicec('/', 2);
ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
if (what == "target_node") {
set_target_node(which, p_value);
} else if (what == "joints") {
int idx = path.get_slicec('/', 3).to_int();
String prop = path.get_slicec('/', 4);
if (prop == "rotation_axis") {
set_joint_rotation_axis(which, idx, static_cast<RotationAxis>((int)p_value));
} else if (prop == "rotation_axis_vector") {
set_joint_rotation_axis_vector(which, idx, p_value);
} else if (prop == "limitation") {
String opt = path.get_slicec('/', 5);
if (opt.is_empty()) {
set_joint_limitation(which, idx, p_value);
} else if (opt == "right_axis") {
set_joint_limitation_right_axis(which, idx, p_value);
} else if (opt == "right_axis_vector") {
set_joint_limitation_right_axis_vector(which, idx, p_value);
} else if (opt == "rotation_offset") {
set_joint_limitation_rotation_offset(which, idx, p_value);
} else {
return false;
}
} else {
return false;
}
} else {
return false;
}
}
return true;
}
bool IterateIK3D::_get(const StringName &p_path, Variant &r_ret) const {
String path = p_path;
if (path.begins_with("settings/")) {
int which = path.get_slicec('/', 1).to_int();
String what = path.get_slicec('/', 2);
ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
if (what == "target_node") {
r_ret = get_target_node(which);
} else if (what == "joints") {
int idx = path.get_slicec('/', 3).to_int();
String prop = path.get_slicec('/', 4);
if (prop == "rotation_axis") {
r_ret = (int)get_joint_rotation_axis(which, idx);
} else if (prop == "rotation_axis_vector") {
r_ret = get_joint_rotation_axis_vector(which, idx);
} else if (prop == "limitation") {
String opt = path.get_slicec('/', 5);
if (opt.is_empty()) {
r_ret = get_joint_limitation(which, idx);
} else if (opt == "right_axis") {
r_ret = get_joint_limitation_right_axis(which, idx);
} else if (opt == "right_axis_vector") {
r_ret = get_joint_limitation_right_axis_vector(which, idx);
} else if (opt == "rotation_offset") {
r_ret = get_joint_limitation_rotation_offset(which, idx);
} else {
return false;
}
} else {
return false;
}
} else {
return false;
}
}
return true;
}
void IterateIK3D::_get_property_list(List<PropertyInfo> *p_list) const {
LocalVector<PropertyInfo> props;
for (uint32_t i = 0; i < settings.size(); i++) {
String path = "settings/" + itos(i) + "/";
p_list->push_back(PropertyInfo(Variant::NODE_PATH, path + "target_node"));
for (uint32_t j = 0; j < iterate_settings[i]->joints.size(); j++) {
String joint_path = path + "joints/" + itos(j) + "/";
props.push_back(PropertyInfo(Variant::INT, joint_path + "rotation_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_rotation_axis()));
props.push_back(PropertyInfo(Variant::VECTOR3, joint_path + "rotation_axis_vector"));
props.push_back(PropertyInfo(Variant::OBJECT, joint_path + "limitation", PROPERTY_HINT_RESOURCE_TYPE, "JointLimitation3D"));
props.push_back(PropertyInfo(Variant::INT, joint_path + "limitation/right_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_secondary_direction()));
props.push_back(PropertyInfo(Variant::VECTOR3, joint_path + "limitation/right_axis_vector"));
props.push_back(PropertyInfo(Variant::QUATERNION, joint_path + "limitation/rotation_offset"));
}
}
ChainIK3D::get_property_list(p_list);
for (PropertyInfo &p : props) {
_validate_dynamic_prop(p);
p_list->push_back(p);
}
}
void IterateIK3D::_validate_dynamic_prop(PropertyInfo &p_property) const {
PackedStringArray split = p_property.name.split("/");
if (split.size() > 3 && split[0] == "settings") {
int which = split[1].to_int();
int joint = split[3].to_int();
// Joints option.
if (split[2] == "joints" && split.size() > 4) {
if (split[4] == "rotation_axis_vector" && get_joint_rotation_axis(which, joint) != ROTATION_AXIS_CUSTOM) {
p_property.usage = PROPERTY_USAGE_NONE;
}
if (split[4] == "limitation" && split.size() > 5) {
if (get_joint_limitation(which, joint).is_null()) {
p_property.usage = PROPERTY_USAGE_NONE;
} else if (split[5] == "right_axis_vector" && get_joint_limitation_right_axis(which, joint) != SECONDARY_DIRECTION_CUSTOM) {
p_property.usage = PROPERTY_USAGE_NONE;
}
}
}
}
}
PackedStringArray IterateIK3D::get_configuration_warnings() const {
PackedStringArray warnings = SkeletonModifier3D::get_configuration_warnings();
for (uint32_t i = 0; i < iterate_settings.size(); i++) {
if (iterate_settings[i]->target_node.is_empty()) {
warnings.push_back(RTR("Detecting settings with no target set! IterateIK3D must have a target to work."));
break;
}
}
return warnings;
}
void IterateIK3D::set_max_iterations(int p_max_iterations) {
max_iterations = p_max_iterations;
}
int IterateIK3D::get_max_iterations() const {
return max_iterations;
}
void IterateIK3D::set_min_distance(double p_min_distance) {
min_distance = p_min_distance;
}
double IterateIK3D::get_min_distance() const {
return min_distance;
}
void IterateIK3D::set_angular_delta_limit(double p_angular_delta_limit) {
angular_delta_limit = p_angular_delta_limit;
}
double IterateIK3D::get_angular_delta_limit() const {
return angular_delta_limit;
}
// Setting.
void IterateIK3D::set_target_node(int p_index, const NodePath &p_node_path) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
iterate_settings[p_index]->target_node = p_node_path;
update_configuration_warnings();
}
NodePath IterateIK3D::get_target_node(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), NodePath());
return iterate_settings[p_index]->target_node;
}
// Individual joints.
void IterateIK3D::set_joint_rotation_axis(int p_index, int p_joint, RotationAxis p_axis) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
joint_settings[p_joint]->rotation_axis = p_axis;
Skeleton3D *sk = get_skeleton();
if (sk) {
_validate_axis(sk, p_index, p_joint);
}
notify_property_list_changed();
iterate_settings[p_index]->simulation_dirty = true; // Snapping to planes is needed in the initialization, so need to restructure.
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
SkeletonModifier3D::RotationAxis IterateIK3D::get_joint_rotation_axis(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), ROTATION_AXIS_ALL);
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), ROTATION_AXIS_ALL);
return joint_settings[p_joint]->rotation_axis;
}
void IterateIK3D::set_joint_rotation_axis_vector(int p_index, int p_joint, const Vector3 &p_vector) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
joint_settings[p_joint]->rotation_axis_vector = p_vector;
Skeleton3D *sk = get_skeleton();
if (sk) {
_validate_axis(sk, p_index, p_joint);
}
iterate_settings[p_index]->simulation_dirty = true; // Snapping to planes is needed in the initialization, so need to restructure.
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
Vector3 IterateIK3D::get_joint_rotation_axis_vector(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3());
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Vector3());
return joint_settings[p_joint]->get_rotation_axis_vector();
}
Quaternion IterateIK3D::get_joint_limitation_space(int p_index, int p_joint, const Vector3 &p_forward) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Quaternion());
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Quaternion());
return joint_settings[p_joint]->get_limitation_space(p_forward);
}
void IterateIK3D::set_joint_limitation(int p_index, int p_joint, const Ref<JointLimitation3D> &p_limitation) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
_unbind_joint_limitation(p_index, p_joint);
joint_settings[p_joint]->limitation = p_limitation;
_bind_joint_limitation(p_index, p_joint);
notify_property_list_changed();
_update_joint_limitation(p_index, p_joint);
}
Ref<JointLimitation3D> IterateIK3D::get_joint_limitation(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Ref<JointLimitation3D>());
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Ref<JointLimitation3D>());
return joint_settings[p_joint]->limitation;
}
void IterateIK3D::set_joint_limitation_right_axis(int p_index, int p_joint, SecondaryDirection p_direction) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
joint_settings[p_joint]->limitation_right_axis = p_direction;
notify_property_list_changed();
_update_joint_limitation(p_index, p_joint);
}
IKModifier3D::SecondaryDirection IterateIK3D::get_joint_limitation_right_axis(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), SECONDARY_DIRECTION_NONE);
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), SECONDARY_DIRECTION_NONE);
return joint_settings[p_joint]->limitation_right_axis;
}
void IterateIK3D::set_joint_limitation_right_axis_vector(int p_index, int p_joint, const Vector3 &p_vector) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
joint_settings[p_joint]->limitation_right_axis_vector = p_vector;
_update_joint_limitation(p_index, p_joint);
}
Vector3 IterateIK3D::get_joint_limitation_right_axis_vector(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3());
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Vector3());
return joint_settings[p_joint]->get_limitation_right_axis_vector();
}
void IterateIK3D::set_joint_limitation_rotation_offset(int p_index, int p_joint, const Quaternion &p_offset) {
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
joint_settings[p_joint]->limitation_rotation_offset = p_offset;
_update_joint_limitation(p_index, p_joint);
}
Quaternion IterateIK3D::get_joint_limitation_rotation_offset(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Quaternion());
const LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Quaternion());
return joint_settings[p_joint]->limitation_rotation_offset;
}
void IterateIK3D::_set_joint_count(int p_index, int p_count) {
_unbind_joint_limitations(p_index);
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
int delta = p_count - joint_settings.size();
if (delta < 0) {
for (int i = delta; i < 0; i++) {
memdelete(joint_settings[joint_settings.size() + i]);
joint_settings[joint_settings.size() + i] = nullptr;
}
}
joint_settings.resize(p_count);
delta++;
if (delta > 1) {
for (int i = 1; i < delta; i++) {
joint_settings[p_count - i] = memnew(IterateIK3DJointSetting);
}
}
}
void IterateIK3D::_validate_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const {
RotationAxis axis = iterate_settings[p_index]->joint_settings[p_joint]->rotation_axis;
if (axis == ROTATION_AXIS_ALL) {
return;
}
Vector3 rot = get_joint_rotation_axis_vector(p_index, p_joint).normalized();
Vector3 fwd;
if (p_joint < (int)iterate_settings[p_index]->joints.size() - 1) {
fwd = p_skeleton->get_bone_rest(iterate_settings[p_index]->joints[p_joint + 1].bone).origin;
} else if (iterate_settings[p_index]->extend_end_bone) {
fwd = get_bone_axis(iterate_settings[p_index]->end_bone.bone, iterate_settings[p_index]->end_bone_direction);
if (fwd.is_zero_approx()) {
return;
}
}
fwd.normalize();
if (Math::is_equal_approx(Math::abs(rot.dot(fwd)), 1)) {
WARN_PRINT_ED("Setting: " + itos(p_index) + " Joint: " + itos(p_joint) + ": Rotation axis and forward vector are colinear. This is not advised as it may cause unwanted rotation.");
}
}
void IterateIK3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_max_iterations", "max_iterations"), &IterateIK3D::set_max_iterations);
ClassDB::bind_method(D_METHOD("get_max_iterations"), &IterateIK3D::get_max_iterations);
ClassDB::bind_method(D_METHOD("set_min_distance", "min_distance"), &IterateIK3D::set_min_distance);
ClassDB::bind_method(D_METHOD("get_min_distance"), &IterateIK3D::get_min_distance);
ClassDB::bind_method(D_METHOD("set_angular_delta_limit", "angular_delta_limit"), &IterateIK3D::set_angular_delta_limit);
ClassDB::bind_method(D_METHOD("get_angular_delta_limit"), &IterateIK3D::get_angular_delta_limit);
// Setting.
ClassDB::bind_method(D_METHOD("set_target_node", "index", "target_node"), &IterateIK3D::set_target_node);
ClassDB::bind_method(D_METHOD("get_target_node", "index"), &IterateIK3D::get_target_node);
// Individual joints.
ClassDB::bind_method(D_METHOD("set_joint_rotation_axis", "index", "joint", "axis"), &IterateIK3D::set_joint_rotation_axis);
ClassDB::bind_method(D_METHOD("get_joint_rotation_axis", "index", "joint"), &IterateIK3D::get_joint_rotation_axis);
ClassDB::bind_method(D_METHOD("set_joint_rotation_axis_vector", "index", "joint", "axis_vector"), &IterateIK3D::set_joint_rotation_axis_vector);
ClassDB::bind_method(D_METHOD("get_joint_rotation_axis_vector", "index", "joint"), &IterateIK3D::get_joint_rotation_axis_vector);
ClassDB::bind_method(D_METHOD("set_joint_limitation", "index", "joint", "limitation"), &IterateIK3D::set_joint_limitation);
ClassDB::bind_method(D_METHOD("get_joint_limitation", "index", "joint"), &IterateIK3D::get_joint_limitation);
ClassDB::bind_method(D_METHOD("set_joint_limitation_right_axis", "index", "joint", "direction"), &IterateIK3D::set_joint_limitation_right_axis);
ClassDB::bind_method(D_METHOD("get_joint_limitation_right_axis", "index", "joint"), &IterateIK3D::get_joint_limitation_right_axis);
ClassDB::bind_method(D_METHOD("set_joint_limitation_right_axis_vector", "index", "joint", "vector"), &IterateIK3D::set_joint_limitation_right_axis_vector);
ClassDB::bind_method(D_METHOD("get_joint_limitation_right_axis_vector", "index", "joint"), &IterateIK3D::get_joint_limitation_right_axis_vector);
ClassDB::bind_method(D_METHOD("set_joint_limitation_rotation_offset", "index", "joint", "offset"), &IterateIK3D::set_joint_limitation_rotation_offset);
ClassDB::bind_method(D_METHOD("get_joint_limitation_rotation_offset", "index", "joint"), &IterateIK3D::get_joint_limitation_rotation_offset);
ADD_PROPERTY(PropertyInfo(Variant::INT, "max_iterations", PROPERTY_HINT_RANGE, "0,100,or_greater"), "set_max_iterations", "get_max_iterations");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "min_distance", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater"), "set_min_distance", "get_min_distance");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "angular_delta_limit", PROPERTY_HINT_RANGE, "0,180,0.001,radians_as_degrees"), "set_angular_delta_limit", "get_angular_delta_limit");
ADD_ARRAY_COUNT("Settings", "setting_count", "set_setting_count", "get_setting_count", "settings/");
}
void IterateIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) {
IterateIK3DSetting *setting = iterate_settings[p_index];
cached_space = p_skeleton->get_global_transform_interpolated();
if (!setting->simulation_dirty) {
return;
}
_unbind_joint_limitations(p_index);
for (uint32_t i = 0; i < setting->solver_info_list.size(); i++) {
if (setting->solver_info_list[i]) {
memdelete(setting->solver_info_list[i]);
setting->solver_info_list[i] = nullptr;
}
}
setting->solver_info_list.clear();
setting->solver_info_list.resize_initialized(setting->joints.size());
setting->chain.clear();
bool extend_end_bone = setting->extend_end_bone && setting->end_bone_length > 0;
for (uint32_t i = 0; i < setting->joints.size(); i++) {
setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).origin);
bool last = i == setting->joints.size() - 1;
if (last && extend_end_bone && setting->end_bone_length > 0) {
Vector3 axis = get_bone_axis(setting->end_bone.bone, setting->end_bone_direction);
if (axis.is_zero_approx()) {
continue;
}
setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo);
setting->solver_info_list[i]->forward_vector = snap_vector_to_plane(setting->joint_settings[i]->get_rotation_axis_vector(), axis.normalized());
setting->solver_info_list[i]->length = setting->end_bone_length;
setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).xform(axis * setting->end_bone_length));
} else if (!last) {
Vector3 axis = p_skeleton->get_bone_rest(setting->joints[i + 1].bone).origin;
if (axis.is_zero_approx()) {
continue; // Means always we need to check solver info, but `!solver_info` means that the bone is zero length, so IK should skip it in the all process.
}
setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo);
setting->solver_info_list[i]->forward_vector = snap_vector_to_plane(setting->joint_settings[i]->get_rotation_axis_vector(), axis.normalized());
setting->solver_info_list[i]->length = axis.length();
}
}
_bind_joint_limitations(p_index);
setting->init_current_joint_rotations(p_skeleton);
setting->simulation_dirty = false;
setting->simulated = false;
}
void IterateIK3D::_make_simulation_dirty(int p_index) {
IterateIK3DSetting *setting = iterate_settings[p_index];
if (!setting) {
return;
}
setting->simulation_dirty = true;
}
void IterateIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) {
min_distance_squared = min_distance * min_distance;
for (uint32_t i = 0; i < settings.size(); i++) {
_init_joints(p_skeleton, i);
Node3D *target = Object::cast_to<Node3D>(get_node_or_null(iterate_settings[i]->target_node));
if (!target || iterate_settings[i]->chain.is_empty()) {
continue; // Abort.
}
iterate_settings[i]->cache_current_joint_rotations(p_skeleton); // Iterate over first to detect parent (outside of the chain) bone pose changes.
Vector3 destination = cached_space.affine_inverse().xform(target->get_global_transform_interpolated().origin);
_process_joints(p_delta, p_skeleton, iterate_settings[i], destination);
}
}
void IterateIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
double distance_to_target_sq = INFINITY;
int iteration_count = 0;
if (p_setting->is_penetrated(p_destination)) {
return;
}
// To prevent oscillation, if it has been processed at least once and target was reached, abort iterating.
if (p_setting->simulated) {
distance_to_target_sq = p_setting->chain[p_setting->chain.size() - 1].distance_squared_to(p_destination);
}
while (distance_to_target_sq > min_distance_squared && iteration_count < max_iterations) {
// Solve the IK for this iteration.
_solve_iteration(p_delta, p_skeleton, p_setting, p_destination);
// Update virtual bone rest/poses.
p_setting->cache_current_joint_rotations(p_skeleton, angular_delta_limit);
distance_to_target_sq = p_setting->chain[p_setting->chain.size() - 1].distance_squared_to(p_destination);
iteration_count++;
}
// Apply the virtual bone rest/poses to the actual bones.
for (uint32_t i = 0; i < p_setting->solver_info_list.size(); i++) {
IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
if (!solver_info || Math::is_zero_approx(solver_info->length)) {
continue;
}
p_skeleton->set_bone_pose_rotation(p_setting->joints[i].bone, solver_info->current_lpose);
}
p_setting->simulated = true;
}
void IterateIK3D::_solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
//
}
void IterateIK3D::_update_joint_limitation(int p_index, int p_joint) {
ERR_FAIL_INDEX(p_index, (int)iterate_settings.size());
iterate_settings[p_index]->simulated = false;
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); // p_joint is unused directly, but need to identify bound index.
#ifdef TOOLS_ENABLED
update_gizmos();
#endif // TOOLS_ENABLED
}
void IterateIK3D::_bind_joint_limitation(int p_index, int p_joint) {
ERR_FAIL_INDEX(p_index, (int)iterate_settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
if (joint_settings[p_joint]->limitation.is_valid()) {
joint_settings[p_joint]->limitation->connect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, p_joint));
}
}
void IterateIK3D::_unbind_joint_limitation(int p_index, int p_joint) {
ERR_FAIL_INDEX(p_index, (int)iterate_settings.size());
LocalVector<IterateIK3DJointSetting *> &joint_settings = iterate_settings[p_index]->joint_settings;
ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
if (joint_settings[p_joint]->limitation.is_valid()) {
joint_settings[p_joint]->limitation->disconnect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, p_joint));
}
}
void IterateIK3D::_bind_joint_limitations(int p_index) {
for (uint32_t i = 0; i < iterate_settings[p_index]->joints.size(); i++) {
if (iterate_settings[p_index]->joint_settings[i]->limitation.is_valid()) {
iterate_settings[p_index]->joint_settings[i]->limitation->connect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, i));
}
}
}
void IterateIK3D::_unbind_joint_limitations(int p_index) {
for (uint32_t i = 0; i < iterate_settings[p_index]->joint_settings.size(); i++) {
if (iterate_settings[p_index]->joint_settings[i]->limitation.is_valid()) {
iterate_settings[p_index]->joint_settings[i]->limitation->disconnect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, i));
}
}
}
IterateIK3D::~IterateIK3D() {
for (uint32_t i = 0; i < iterate_settings.size(); i++) {
_unbind_joint_limitations(i);
}
clear_settings();
}
View Git Blame Copy Permalink