diff --git a/core/math/qcp.cpp b/core/math/qcp.cpp new file mode 100644 index 000000000000..c92c6463c21a --- /dev/null +++ b/core/math/qcp.cpp @@ -0,0 +1,261 @@ +/**************************************************************************/ +/* qcp.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 "qcp.h" + +QuaternionCharacteristicPolynomial::QuaternionCharacteristicPolynomial(double p_evec_prec) { + eigenvector_precision = p_evec_prec; +} + +void QuaternionCharacteristicPolynomial::set(PackedVector3Array &r_target, PackedVector3Array &r_moved) { + target = r_target; + moved = r_moved; + transformation_calculated = false; + inner_product_calculated = false; +} + +Quaternion QuaternionCharacteristicPolynomial::_get_rotation() { + Quaternion result; + if (!transformation_calculated) { + if (!inner_product_calculated) { + inner_product(target, moved); + } + result = calculate_rotation(); + transformation_calculated = true; + } + return result; +} + +Quaternion QuaternionCharacteristicPolynomial::calculate_rotation() { + Quaternion result; + + if (moved.size() == 1) { + Vector3 u = moved[0]; + Vector3 v = target[0]; + double norm_product = u.length() * v.length(); + + if (norm_product == 0.0) { + return Quaternion(); + } + + double dot = u.dot(v); + + if (dot < ((2.0e-15 - 1.0) * norm_product)) { + Vector3 w = u.normalized(); + result = Quaternion(w.x, w.y, w.z, 0.0f).normalized(); + } else { + double q0 = Math::sqrt(0.5 * (1.0 + dot / norm_product)); + double coeff = 1.0 / (2.0 * q0 * norm_product); + Vector3 q = v.cross(u).normalized(); + result = Quaternion(coeff * q.x, coeff * q.y, coeff * q.z, q0).normalized(); + } + } else { + double a13 = -sum_xz_minus_zx; + double a14 = sum_xy_minus_yx; + double a21 = sum_yz_minus_zy; + double a22 = sum_xx_minus_yy - sum_zz - max_eigenvalue; + double a23 = sum_xy_plus_yx; + double a24 = sum_xz_plus_zx; + double a31 = a13; + double a32 = a23; + double a33 = sum_yy - sum_xx - sum_zz - max_eigenvalue; + double a34 = sum_yz_plus_zy; + double a41 = a14; + double a42 = a24; + double a43 = a34; + double a44 = sum_zz - sum_xx_plus_yy - max_eigenvalue; + + double a3344_4334 = a33 * a44 - a43 * a34; + double a3244_4234 = a32 * a44 - a42 * a34; + double a3243_4233 = a32 * a43 - a42 * a33; + double a3143_4133 = a31 * a43 - a41 * a33; + double a3144_4134 = a31 * a44 - a41 * a34; + double a3142_4132 = a31 * a42 - a41 * a32; + + double quaternion_w = a22 * a3344_4334 - a23 * a3244_4234 + a24 * a3243_4233; + double quaternion_x = -a21 * a3344_4334 + a23 * a3144_4134 - a24 * a3143_4133; + double quaternion_y = a21 * a3244_4234 - a22 * a3144_4134 + a24 * a3142_4132; + double quaternion_z = -a21 * a3243_4233 + a22 * a3143_4133 - a23 * a3142_4132; + double qsqr = quaternion_w * quaternion_w + quaternion_x * quaternion_x + quaternion_y * quaternion_y + quaternion_z * quaternion_z; + + if (qsqr < eigenvector_precision) { + result = Quaternion(); + } else { + quaternion_x *= -1; + quaternion_y *= -1; + quaternion_z *= -1; + double min = quaternion_w; + min = quaternion_x < min ? quaternion_x : min; + min = quaternion_y < min ? quaternion_y : min; + min = quaternion_z < min ? quaternion_z : min; + quaternion_w /= min; + quaternion_x /= min; + quaternion_y /= min; + quaternion_z /= min; + result = Quaternion(quaternion_x, quaternion_y, quaternion_z, quaternion_w).normalized(); + } + } + + return result; +} + +void QuaternionCharacteristicPolynomial::translate(Vector3 r_translate, PackedVector3Array &r_x) { + for (Vector3 &p : r_x) { + p += r_translate; + } +} + +Vector3 QuaternionCharacteristicPolynomial::_get_translation() { + return target_center - moved_center; +} + +Vector3 QuaternionCharacteristicPolynomial::move_to_weighted_center(PackedVector3Array &r_to_center, Vector &r_weight) { + Vector3 center; + double total_weight = 0; + bool weight_is_empty = r_weight.is_empty(); + int size = r_to_center.size(); + + for (int i = 0; i < size; i++) { + if (!weight_is_empty) { + total_weight += r_weight[i]; + center += r_to_center[i] * r_weight[i]; + } else { + center += r_to_center[i]; + total_weight++; + } + } + + if (total_weight > 0) { + center /= total_weight; + } + + return center; +} + +void QuaternionCharacteristicPolynomial::inner_product(PackedVector3Array &r_coords1, PackedVector3Array &r_coords2) { + Vector3 weighted_coord1, weighted_coord2; + double sum_of_squares1 = 0, sum_of_squares2 = 0; + + sum_xx = 0; + sum_xy = 0; + sum_xz = 0; + sum_yx = 0; + sum_yy = 0; + sum_yz = 0; + sum_zx = 0; + sum_zy = 0; + sum_zz = 0; + + bool weight_is_empty = weight.is_empty(); + int size = r_coords1.size(); + + for (int i = 0; i < size; i++) { + if (!weight_is_empty) { + weighted_coord1 = weight[i] * r_coords1[i]; + sum_of_squares1 += weighted_coord1.dot(r_coords1[i]); + } else { + weighted_coord1 = r_coords1[i]; + sum_of_squares1 += weighted_coord1.dot(weighted_coord1); + } + + weighted_coord2 = r_coords2[i]; + + sum_of_squares2 += weight_is_empty ? weighted_coord2.dot(weighted_coord2) : (weight[i] * weighted_coord2.dot(weighted_coord2)); + + sum_xx += (weighted_coord1.x * weighted_coord2.x); + sum_xy += (weighted_coord1.x * weighted_coord2.y); + sum_xz += (weighted_coord1.x * weighted_coord2.z); + + sum_yx += (weighted_coord1.y * weighted_coord2.x); + sum_yy += (weighted_coord1.y * weighted_coord2.y); + sum_yz += (weighted_coord1.y * weighted_coord2.z); + + sum_zx += (weighted_coord1.z * weighted_coord2.x); + sum_zy += (weighted_coord1.z * weighted_coord2.y); + sum_zz += (weighted_coord1.z * weighted_coord2.z); + } + + double initial_eigenvalue = (sum_of_squares1 + sum_of_squares2) * 0.5; + + sum_xz_plus_zx = sum_xz + sum_zx; + sum_yz_plus_zy = sum_yz + sum_zy; + sum_xy_plus_yx = sum_xy + sum_yx; + sum_yz_minus_zy = sum_yz - sum_zy; + sum_xz_minus_zx = sum_xz - sum_zx; + sum_xy_minus_yx = sum_xy - sum_yx; + sum_xx_plus_yy = sum_xx + sum_yy; + sum_xx_minus_yy = sum_xx - sum_yy; + max_eigenvalue = initial_eigenvalue; + + inner_product_calculated = true; +} + +Quaternion QuaternionCharacteristicPolynomial::_weighted_superpose(const PackedVector3Array &p_moved, const PackedVector3Array &p_target, const Vector &p_weight, bool p_translate) { + set(p_moved, p_target, p_weight, p_translate); + return _get_rotation(); +} + +void QuaternionCharacteristicPolynomial::set(const PackedVector3Array &p_moved, const PackedVector3Array &p_target, const Vector &p_weight, bool p_translate) { + transformation_calculated = false; + inner_product_calculated = false; + + moved = p_moved; + target = p_target; + weight = p_weight; + + if (p_translate) { + moved_center = move_to_weighted_center(moved, weight); + w_sum = 0; // set wsum to 0 so we don't double up. + target_center = move_to_weighted_center(target, weight); + translate(moved_center * -1, moved); + translate(target_center * -1, target); + } else { + if (!p_weight.is_empty()) { + for (int i = 0; i < p_weight.size(); i++) { + w_sum += p_weight[i]; + } + } else { + w_sum = p_moved.size(); + } + } +} + +void QuaternionCharacteristicPolynomial::weighted_superpose( + const PackedVector3Array p_moved, + const PackedVector3Array p_target, + const Vector p_weight, + bool p_translate, + Quaternion &r_rotation, + Vector3 &r_translation, + double p_precision) { + QuaternionCharacteristicPolynomial qcp(p_precision); + r_rotation = qcp._weighted_superpose(p_moved, p_target, p_weight, p_translate); + r_translation = qcp._get_translation(); +} diff --git a/core/math/qcp.h b/core/math/qcp.h new file mode 100644 index 000000000000..b47db441a106 --- /dev/null +++ b/core/math/qcp.h @@ -0,0 +1,111 @@ +/**************************************************************************/ +/* qcp.h */ +/**************************************************************************/ +/* 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. */ +/**************************************************************************/ + +#ifndef QCP_H +#define QCP_H + +#include "core/math/vector3.h" +#include "core/object/class_db.h" +#include "core/object/object.h" +#include "core/variant/variant.h" + +/** + * SPDX short identifier: BSD-2-Clause + * + * Implementation of the Quaternion-Based Characteristic Polynomial algorithm + * for RMSD and Superposition calculations. + * + * Usage: + * 1. Create a QCP object with two Vector3 arrays of equal length as input. + * The input coordinates are not changed. + * 2. Optionally, provide weighting factors [0 - 1] for each point. + * 3. For maximum efficiency, create a QCP object once and reuse it. + * + * A. Calculate rmsd only: double rmsd = qcp.getRmsd(); + * B. Calculate a 4x4 transformation (Quaternion and translation) matrix: Matrix4f trans = qcp.getTransformationMatrix(); + * C. Get transformed points (y superposed onto the reference x): Vector3[] ySuperposed = qcp.getTransformedCoordinates(); + * + * Citations: + * - Liu P, Agrafiotis DK, & Theobald DL (2011) Reply to comment on: "Fast determination of the optimal Quaternionation matrix for macromolecular superpositions." Journal of Computational Chemistry 32(1):185-186. [http://dx.doi.org/10.1002/jcc.21606] + * - Liu P, Agrafiotis DK, & Theobald DL (2010) "Fast determination of the optimal Quaternionation matrix for macromolecular superpositions." Journal of Computational Chemistry 31(7):1561-1563. [http://dx.doi.org/10.1002/jcc.21439] + * - Douglas L Theobald (2005) "Rapid calculation of RMSDs using a quaternion-based characteristic polynomial." Acta Crystallogr A 61(4):478-480. [http://dx.doi.org/10.1107/S0108767305015266] + * + * This is an adaptation of the original C code QCPQuaternion 1.4 (2012, October 10) to C++. + * The original C source code is available from http://theobald.brandeis.edu/qcp/ and was developed by: + * - Douglas L. Theobald, Department of Biochemistry, Brandeis University + * - Pu Liu, Johnson & Johnson Pharmaceutical Research and Development, L.L.C. + * + * @author Douglas L. Theobald (original C code) + * @author Pu Liu (original C code) + * @author Peter Rose (adapted to Java) + * @author Aleix Lafita (adapted to Java) + * @author Eron Gjoni (adapted to EWB IK) + * @author K. S. Ernest (iFire) Lee (adapted to ManyBoneIK) + */ + +class QuaternionCharacteristicPolynomial : Object { + GDCLASS(QuaternionCharacteristicPolynomial, Object); + double eigenvector_precision = 1E-10; + + PackedVector3Array target, moved; + Vector weight; + double w_sum = 0; + + Vector3 target_center, moved_center; + + double sum_xy = 0, sum_xz = 0, sum_yx = 0, sum_yz = 0, sum_zx = 0, sum_zy = 0; + double sum_xx_plus_yy = 0, sum_zz = 0, max_eigenvalue = 0, sum_yz_minus_zy = 0, sum_xz_minus_zx = 0, sum_xy_minus_yx = 0; + double sum_xx_minus_yy = 0, sum_xy_plus_yx = 0, sum_xz_plus_zx = 0; + double sum_yy = 0, sum_xx = 0, sum_yz_plus_zy = 0; + bool transformation_calculated = false, inner_product_calculated = false; + + void inner_product(PackedVector3Array &r_coords1, PackedVector3Array &r_coords2); + void set(PackedVector3Array &r_target, PackedVector3Array &r_moved); + Quaternion calculate_rotation(); + void set(const PackedVector3Array &p_moved, const PackedVector3Array &p_target, const Vector &p_weight, bool p_translate); + static void translate(Vector3 r_translate, PackedVector3Array &r_x); + Vector3 move_to_weighted_center(PackedVector3Array &r_to_center, Vector &r_weight); + Quaternion _weighted_superpose(const PackedVector3Array &p_moved, const PackedVector3Array &p_target, const Vector &p_weight, bool p_translate); + Quaternion _get_rotation(); + Vector3 _get_translation(); + QuaternionCharacteristicPolynomial(double p_evec_prec); + +public: + static void weighted_superpose( + const PackedVector3Array p_moved, + const PackedVector3Array p_target, + const Vector p_weight, + bool p_translate, + Quaternion &r_rotation, + Vector3 &r_translation, + double p_precision = 1E-10); +}; + +#endif // QCP_H diff --git a/scene/3d/ccd_ik_3d.cpp b/scene/3d/ccd_ik_3d.cpp index 999c8959067f..6acf8fc81cb9 100644 --- a/scene/3d/ccd_ik_3d.cpp +++ b/scene/3d/ccd_ik_3d.cpp @@ -30,78 +30,132 @@ #include "ccd_ik_3d.h" +#include "core/math/qcp.h" + +// The basic idea is to create a bunch of ordered sets of bones. +// +// TODO: Solve one effector to one Godot Engine Node3D solve. +// +// Building each set by starting at an effector and traversing all the way down to the skeleton root. (Keep track of each effector you started from) +// +// The effector is the end of the chain, and the root is the beginning of the chain. A root is also the bone rootmost to the `Vector bones_to_process = skeleton->get_parentless_bones()`; +// +// Then you take the intersections of all of the bonesets, giving you the sets of bonesets which solve for the same effectors. +// This allows you to initialize moved/target arrays once for each boneset, since by definition all of the bones inside of them are solving for the same effectors. +// +// Finally, determine the default traversal order for the bonesets, then flatten those into a bone traversal list. +// +// Assume the existence and usage of the disjoint-set data structure. Do not need to implement it here. +// +// In computer science, a disjoint-set data structure, also called a union–find data structure or merge–find set, is a data structure that stores a collection of disjoint (non-overlapping) sets. + void CCDIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, Vector &p_joints, Vector &p_chain, const Transform3D &p_space, const Vector3 &p_destination, const Vector3 &p_target_vector, int p_max_iterations, real_t p_min_distance) { - double min_distance_sq = p_min_distance * p_min_distance; - real_t distance_to_target_sq = INFINITY; + ERR_FAIL_NULL(p_skeleton); + ERR_FAIL_INDEX(p_joints.size() - 1, p_joints.size()); + ERR_FAIL_INDEX(0, p_joints.size()); + ERR_FAIL_COND_MSG(!is_rootmost_bone(p_skeleton, p_joints[0]->bone, p_joints[p_joints.size() - 1]->bone), "The first bone must be the rootmost bone."); int iteration_count = 0; + Vector solver_info_data; + solver_info_data.resize(p_joints.size()); + for (int joints_i = 0; joints_i < p_joints.size(); ++joints_i) { + if (p_joints[joints_i] == nullptr) { + continue; + } + ManyBoneIK3DSolverInfo *solver_info = p_joints[joints_i]->solver_info; + if (!solver_info) { + continue; + } + SolverInfoData ¤t_solver_data = solver_info_data.write[joints_i]; + current_solver_data.target_transform.origin = p_chain[joints_i] + p_destination; + current_solver_data.moved_transform.origin = p_chain[joints_i]; + } - // Quaternion destination_rotation = p_space.basis.get_rotation_quaternion() * p_skeleton->get_bone_global_pose(p_joints[p_joints.size() - 1]->bone).basis.get_rotation_quaternion(); - - while (distance_to_target_sq > min_distance_sq && iteration_count < p_max_iterations) { + while (iteration_count < p_max_iterations) { iteration_count++; - - // Backwards. - int end = -1; - for (int i = p_joints.size() - 1; i >= 0; i--) { - ManyBoneIK3DSolverInfo *solver_info = p_joints[i]->solver_info; - if (!solver_info) { - continue; - } - const int HEAD = i; - if (end == -1) { - end = i + 1; - } - Vector3 to_vec = p_destination - p_chain[HEAD]; - Vector3 head_to_end = p_chain[end] - p_chain[HEAD]; - Quaternion to_rot = Quaternion(head_to_end.normalized(), to_vec.normalized()); - for (int j = p_chain.size() - 1; j > i; j--) { - Vector3 head_to_any_joint = p_chain[j] - p_chain[HEAD]; - p_chain.write[j] = p_chain[HEAD] + to_rot.xform(head_to_any_joint); - - // For constraint. - /* - Vector3 current_head_to_tail = p_skeleton->get_bone_global_pose(p_joints[i]->bone).basis.get_rotation_quaternion().xform_inv(to_vec); - Quaternion rotation_result = solver_info->current_rot = Quaternion(-solver_info->forward_vector, current_head_to_tail); - Transform3D rest = p_skeleton->get_bone_global_pose(p_joints[i]->bone) * Basis(p_joints[i]->constraint_rotation_offset); - Quaternion rest_rotation = rest.basis.get_rotation_quaternion(); - bool rotation_modified = false; - TwistSwing ts = decompose_rotation_to_twist_and_swing(rest_rotation, rotation_result); - if (p_joints[i]->twist_limitation < Math_PI) { - // TODO: coding which limit twist. - // ts.twist = XXXXX; - rotation_modified = true; - } - Ref constraint = p_joints[i]->constraint; - if (constraint.is_valid()) { - ts.swing = constraint->solve(rest_rotation, ts.swing); - rotation_modified = true; - } - if (rotation_modified) { - // TODO: coding which fix any joint by constraintated rotation. - p_chain.write[j] = compose_rotation_from_twist_and_swing(rest_rotation, ts).xform(-solver_info->forward_vector); - } - */ + Vector moveds; + Vector targets; + for (int joints_i = p_joints.size() - 1; joints_i >= 0; --joints_i) { + moveds.resize(joints_i + 1); + targets.resize(joints_i + 1); + for (int inner_joints_i = joints_i; inner_joints_i >= 0; --inner_joints_i) { + SolverInfoData &inner_solver_info_data = solver_info_data.write[inner_joints_i]; + targets.write[inner_joints_i] = inner_solver_info_data.target_transform; + moveds.write[inner_joints_i] = inner_solver_info_data.moved_transform; } + bool translate = joints_i == 0; + Transform3D result = _solve(targets, moveds, translate); + solver_info_data.write[joints_i].moved_transform.origin = result.origin; } - - distance_to_target_sq = p_chain[p_chain.size() - 1].distance_squared_to(p_destination); } - - for (int i = 0; i < p_joints.size(); i++) { - ManyBoneIK3DSolverInfo *solver_info = p_joints[i]->solver_info; - if (!solver_info) { + for (int joints_i = 0; joints_i < p_joints.size(); ++joints_i) { + if (!p_joints[joints_i]) { continue; } - Vector3 current_head_to_tail = (p_chain[i + 1] - p_chain[i]).normalized(); - current_head_to_tail = p_skeleton->get_bone_global_pose(p_joints[i]->bone).basis.get_rotation_quaternion().xform_inv(current_head_to_tail); - if (solver_info->forward_vector.dot(current_head_to_tail) > 1.0f - CMP_EPSILON) { + ManyBoneIK3DSolverInfo *solver_info = p_joints[joints_i]->solver_info; + if (!solver_info) { continue; } - solver_info->current_rot = Quaternion(solver_info->forward_vector, current_head_to_tail); - p_skeleton->set_bone_pose_rotation(p_joints[i]->bone, - get_local_pose_rotation( - p_skeleton, - p_joints[i]->bone, - p_skeleton->get_bone_global_pose(p_joints[i]->bone).basis.get_rotation_quaternion() * solver_info->current_rot)); + p_chain.write[joints_i] = solver_info_data[joints_i].moved_transform.origin; + bool translate = joints_i == 0; + if (translate) { + p_skeleton->set_bone_pose_position(p_joints[joints_i]->bone, solver_info_data[joints_i].moved_transform.origin); + } } } + +Transform3D CCDIK3D::_solve(const Vector &p_targets, const Vector &p_moveds, bool p_translate) { + ERR_FAIL_COND_V(p_targets.size() != p_moveds.size(), Transform3D()); + + Vector moved_positions; + Vector target_positions; + Vector weights; + + moved_positions.resize(p_moveds.size() * 7); + target_positions.resize(p_targets.size() * 7); + weights.resize(p_moveds.size() * 7); + real_t pin_weight = 1.0; + weights.fill(pin_weight); + + for (int i = 0; i < p_targets.size(); ++i) { + moved_positions.write[i * 7] = p_moveds[i].origin; + int rest_index = 1; + for (int axis_i = Vector3::AXIS_X; axis_i <= Vector3::AXIS_Z; ++axis_i) { + Vector3 column = p_moveds[i].basis.get_column(axis_i); + moved_positions.write[i * 7 + rest_index] = column + p_moveds[i].origin; + rest_index++; + moved_positions.write[i * 7 + rest_index] = p_moveds[i].origin - column; + rest_index++; + } + + target_positions.write[i * 7] = p_targets[i].origin; + int current_index = 1; + for (int axis_j = Vector3::AXIS_X; axis_j <= Vector3::AXIS_Z; ++axis_j) { + Vector3 column = p_targets[i].basis.get_column(axis_j); + target_positions.write[i * 7 + current_index] = column + p_targets[i].origin; + current_index++; + target_positions.write[i * 7 + current_index] = p_targets[i].origin - column; + current_index++; + } + } + Quaternion rotation; + Vector3 translation; + QuaternionCharacteristicPolynomial::weighted_superpose( + moved_positions, target_positions, weights, p_translate, rotation, translation); + return Transform3D(Basis(rotation), translation); +} + +bool CCDIK3D::is_rootmost_bone(Skeleton3D *p_skeleton, BoneId p_root_bone, BoneId p_leaf_bone) const { + if (p_root_bone == p_leaf_bone) { + return true; + } + + BoneId current_bone = p_leaf_bone; + while (current_bone != -1) { + if (current_bone == p_root_bone) { + return true; + } + current_bone = p_skeleton->get_bone_parent(current_bone); + } + + return false; +} diff --git a/scene/3d/ccd_ik_3d.h b/scene/3d/ccd_ik_3d.h index 5870d4ef2364..5b34ba66d1bd 100644 --- a/scene/3d/ccd_ik_3d.h +++ b/scene/3d/ccd_ik_3d.h @@ -36,6 +36,16 @@ class CCDIK3D : public ManyBoneIK3D { GDCLASS(CCDIK3D, ManyBoneIK3D); + struct SolverInfoData { + Transform3D target_transform; + Transform3D moved_transform; + }; + + bool is_rootmost_bone(Skeleton3D *p_skeleton, BoneId p_root_bone, BoneId p_leaf_bone) const; + +public: + virtual Transform3D _solve(const Vector &p_targets, const Vector &p_moveds, bool p_translate); + protected: virtual void _process_joints(double p_delta, Skeleton3D *p_skeleton, Vector &p_joints, Vector &p_chain, const Transform3D &p_space, const Vector3 &p_destination, const Vector3 &p_target_vector, int p_max_iterations, real_t p_min_distance) override; }; diff --git a/scene/3d/fabr_ik_3d.cpp b/scene/3d/fabr_ik_3d.cpp index 876fe64dad10..8116a628b304 100644 --- a/scene/3d/fabr_ik_3d.cpp +++ b/scene/3d/fabr_ik_3d.cpp @@ -137,9 +137,9 @@ void FABRIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, Vectorcurrent_rot = Quaternion(solver_info->forward_vector, current_head_to_tail); p_skeleton->set_bone_pose_rotation(p_joints[i]->bone, - get_local_pose_rotation( - p_skeleton, - p_joints[i]->bone, - p_skeleton->get_bone_global_pose(p_joints[i]->bone).basis.get_rotation_quaternion() * solver_info->current_rot)); + get_local_pose_rotation( + p_skeleton, + p_joints[i]->bone, + p_skeleton->get_bone_global_pose(p_joints[i]->bone).basis.get_rotation_quaternion() * solver_info->current_rot)); } } diff --git a/tests/scene/test_ccd_ik_3d.h b/tests/scene/test_ccd_ik_3d.h new file mode 100644 index 000000000000..bdc5040ea085 --- /dev/null +++ b/tests/scene/test_ccd_ik_3d.h @@ -0,0 +1,227 @@ +/**************************************************************************/ +/* test_ccd_ik_3d.h */ +/**************************************************************************/ +/* 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. */ +/**************************************************************************/ + +#ifndef TEST_CCD_IK_3D_H +#define TEST_CCD_IK_3D_H + +#include "scene/3d/ccd_ik_3d.h" + +#include "scene/resources/skeleton_profile.h" + +#include "tests/test_macros.h" + +namespace TestCCDIK3D { + +class TestCCDIK3D : public CCDIK3D { + GDCLASS(TestCCDIK3D, CCDIK3D); + +public: + using CCDIK3D::_process_joints; +}; + +struct JointTailInfo { + Vector3 current_tail; + Vector3 prev_tail; +}; + +TEST_CASE("[CCDIK3D] solve function with identity rotation") { + CCDIK3D *ik = memnew(CCDIK3D); + Vector targets; + targets.push_back(Transform3D()); + Vector moveds; + moveds.push_back(Transform3D()); + Transform3D result_transform = ik->_solve(targets, moveds, false); + INFO("Target Transform:", targets[0]); + INFO("Result Transform:", result_transform); + INFO("Is target approximately equal to result:", targets[0].is_equal_approx(result_transform)); + CHECK(targets[0].is_equal_approx(result_transform)); + CHECK((Transform3D() * moveds[0]).is_equal_approx(targets[0])); + memdelete_notnull(ik); +} + +TEST_CASE("[CCDIK3D] solve function with translation only") { + CCDIK3D *ik = memnew(CCDIK3D); + Vector targets; + Transform3D target_transform; + target_transform.origin = Vector3(10, 20, 30); + targets.push_back(target_transform); + Vector moveds; + moveds.push_back(Transform3D()); + Transform3D result_transform = ik->_solve(targets, moveds, true); + INFO("Target Origin:", targets[0].origin); + INFO("Result Origin:", result_transform.origin); + INFO("Target Basis:", targets[0].basis); + INFO("Result Basis:", result_transform.basis); + INFO("Is origin approximately equal:", targets[0].origin.is_equal_approx(result_transform.origin)); + INFO("Is basis approximately equal:", result_transform.basis.is_equal_approx(target_transform.basis)); + CHECK(targets[0].origin.is_equal_approx(result_transform.origin)); + CHECK(targets[0].basis.is_equal_approx(result_transform.basis)); // Basis should remain unchanged + CHECK((target_transform * moveds[0]).is_equal_approx(targets[0])); + memdelete_notnull(ik); +} + +TEST_CASE("[CCDIK3D] solve function with rotation only") { + CCDIK3D *ik = memnew(CCDIK3D); + Vector targets; + Transform3D target_transform; + target_transform.basis = Basis(Quaternion(0.3, 0.4, 0.5, 0.6).normalized()); + targets.push_back(target_transform); + Vector moveds; + moveds.push_back(Transform3D()); + Transform3D result_transform = ik->_solve(targets, moveds, false); + INFO("Target Basis:", targets[0].basis); + INFO("Result Basis:", result_transform.basis); + INFO("Result Origin:", result_transform.origin); + INFO("Expected Origin:", Vector3()); + INFO("Is basis approximately equal:", result_transform.basis.is_equal_approx(target_transform.basis)); + INFO("Is origin approximately equal to expected:", result_transform.origin.is_equal_approx(Vector3())); + CHECK(result_transform.basis.is_equal_approx(target_transform.basis)); + // Origin should remain the same if p_translate is false + CHECK(result_transform.origin.is_equal_approx(Vector3())); + CHECK((target_transform * moveds[0]).is_equal_approx(targets[0])); + memdelete_notnull(ik); +} + +TEST_CASE("[CCDIK3D] solve function with multiple transforms") { + CCDIK3D *ik = memnew(CCDIK3D); + Vector targets; + + Transform3D target_transform(Quaternion(0.5, 0.6, 0.7, 0.8).normalized(), Vector3()); + CHECK(target_transform.origin.is_zero_approx()); + + Transform3D target_transform_1; + target_transform_1.origin = Vector3(1, 2, 3); + target_transform_1.basis = Basis(Quaternion(0.1, 0.2, 0.3, 0.4).normalized()); + CHECK_FALSE(target_transform_1.origin.is_zero_approx()); + CHECK_FALSE(target_transform_1.basis.is_equal_approx(Basis())); + targets.push_back(target_transform * target_transform_1); + + Transform3D target_transform_2; + target_transform_2.origin = Vector3(4, 5, 6); + target_transform_2.basis = Basis(Quaternion(0.1, 0.2, 0.3, 0.4).normalized()); + CHECK_FALSE(target_transform_2.origin.is_zero_approx()); + CHECK_FALSE(target_transform_2.basis.is_equal_approx(Basis())); + targets.push_back(target_transform * target_transform_2); + + Vector moveds; + moveds.push_back(target_transform_1); + moveds.push_back(target_transform_2); + + Transform3D result_transform = ik->_solve(targets, moveds, false); + INFO("Target 1: ", targets[0]); + INFO("Target 2: ", targets[1]); + INFO("Result Transform: ", result_transform); + INFO("Result 1: ", result_transform * moveds[0]); + INFO("Result 2: ", result_transform * moveds[1]); + CHECK((result_transform * moveds[0]).is_equal_approx(targets[0])); + CHECK((result_transform * moveds[1]).is_equal_approx(targets[1])); + memdelete_notnull(ik); +} + +TEST_CASE("[SceneTree][CCDIK3D] _process_joints with root to hips joints") { + TestCCDIK3D *ik = memnew(TestCCDIK3D); + double delta = 0.016; // Assume 60 FPS. + Skeleton3D *skeleton = memnew(Skeleton3D); + skeleton->add_child(ik); + ik->set_owner(skeleton); + + const String root_bone_name = "Root"; + const String hips_bone_name = "Hips"; + skeleton->add_bone(root_bone_name); + skeleton->add_bone(hips_bone_name); + skeleton->set_bone_parent(skeleton->find_bone(hips_bone_name), skeleton->find_bone(root_bone_name)); + + Vector joints; + CCDIK3D::ManyBoneIK3DJointSetting *root_joint = memnew(CCDIK3D::ManyBoneIK3DJointSetting); + root_joint->bone = skeleton->find_bone(root_bone_name); + root_joint->bone_name = root_bone_name; + root_joint->twist_limitation = Math_PI; + root_joint->constraint = nullptr; + root_joint->constraint_rotation_offset = Quaternion(); + root_joint->solver_info = memnew(CCDIK3D::ManyBoneIK3DSolverInfo); + + JointTailInfo root_tail_info; + root_tail_info.prev_tail = Vector3(0, 0, 0); + root_tail_info.current_tail = Vector3(0, 1, 0); + root_joint->solver_info->forward_vector = Vector3(0, 1, 0); + root_joint->solver_info->current_rot = Quaternion(0, 1, 0, 1).normalized(); + root_joint->solver_info->length = (root_tail_info.current_tail - root_tail_info.prev_tail).length(); + joints.push_back(root_joint); + + CCDIK3D::ManyBoneIK3DJointSetting *hips_joint = memnew(CCDIK3D::ManyBoneIK3DJointSetting); + hips_joint->bone = skeleton->find_bone(hips_bone_name); + hips_joint->bone_name = hips_bone_name; + hips_joint->twist_limitation = Math_PI; + hips_joint->constraint = nullptr; + hips_joint->constraint_rotation_offset = Quaternion(); + hips_joint->solver_info = memnew(CCDIK3D::ManyBoneIK3DSolverInfo); + + JointTailInfo hips_tail_info; + hips_tail_info.prev_tail = root_tail_info.current_tail; + hips_tail_info.current_tail = Vector3(0, 0, 0); + hips_joint->solver_info->forward_vector = Vector3(0, 1, 0); + hips_joint->solver_info->current_rot = Quaternion(0, 0, 1, 1).normalized(); + hips_joint->solver_info->length = (hips_tail_info.current_tail - hips_tail_info.prev_tail).length(); + joints.push_back(hips_joint); + + Vector p_chain; + p_chain.push_back(root_tail_info.current_tail); + p_chain.push_back(hips_tail_info.current_tail); + + Transform3D root_reference_pose; + root_reference_pose.basis = Basis(Quaternion(0, 1, 0, 1).normalized()); + root_reference_pose.origin = Vector3(0, 1, 0); + skeleton->set_bone_rest(root_joint->bone, root_reference_pose); + + Transform3D hips_reference_pose; + hips_reference_pose.basis = Basis(Quaternion(0, 0, 1, 1).normalized()); + skeleton->set_bone_rest(hips_joint->bone, hips_reference_pose); + + skeleton->reset_bone_poses(); + Transform3D space = Transform3D(); + Vector3 target_vector = Vector3(0, 1, 0); + int max_iterations = 10; + real_t min_distance = CMP_EPSILON; + Vector3 destination = Vector3(); + ik->_process_joints(delta, skeleton, joints, p_chain, space, destination, target_vector, max_iterations, min_distance); + + CHECK_EQ(skeleton->get_bone_pose(root_joint->bone).origin, root_reference_pose.origin); + CHECK_EQ(skeleton->get_bone_pose(hips_joint->bone).origin, hips_reference_pose.origin); + + memdelete_notnull(root_joint->solver_info); + memdelete_notnull(root_joint); + memdelete_notnull(hips_joint->solver_info); + memdelete_notnull(hips_joint); + memdelete_notnull(ik); + memdelete_notnull(skeleton); +} +} // namespace TestCCDIK3D + +#endif // TEST_CCD_IK_3D_H diff --git a/tests/test_main.cpp b/tests/test_main.cpp index af334dbbf4a6..d4f583c4711a 100644 --- a/tests/test_main.cpp +++ b/tests/test_main.cpp @@ -171,6 +171,7 @@ #include "tests/scene/test_arraymesh.h" #include "tests/scene/test_camera_3d.h" +#include "tests/scene/test_ccd_ik_3d.h" #include "tests/scene/test_gltf_document.h" #include "tests/scene/test_height_map_shape_3d.h" #include "tests/scene/test_path_3d.h"