From d8d3ebc4b8d144207f9c3b3cc4f37151ec5d7e8b Mon Sep 17 00:00:00 2001 From: hodasemi Date: Sun, 10 Jun 2018 00:17:02 +0200 Subject: [PATCH] Least common ancestor frame --- CGII/src/AtomicTransform.cpp | 14 ++++- CGII/src/IKViewer.cpp | 110 ++++++++++++++++++++++++++++++----- CGII/src/IKViewer.h | 1 + 3 files changed, 111 insertions(+), 14 deletions(-) diff --git a/CGII/src/AtomicTransform.cpp b/CGII/src/AtomicTransform.cpp index 5e54dc8..9cd619b 100644 --- a/CGII/src/AtomicTransform.cpp +++ b/CGII/src/AtomicTransform.cpp @@ -80,6 +80,18 @@ void AtomicRotationTransform::drawActualIndicator(float size) glEnd(); } +template +T clamp(T &v, T &c1, T &c2) +{ + if (v < c1) + return c1; + + if (v > c2) + return c2; + + return v; +} + void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse) { /*Task: Implement parameter optimization*/ @@ -97,7 +109,7 @@ void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec if (inverse) result = -result; - result = std::clamp(result, lower_limit, upper_limit); + result = clamp(result, lower_limit, upper_limit); set_value(result); } diff --git a/CGII/src/IKViewer.cpp b/CGII/src/IKViewer.cpp index 4ee7431..5948a4f 100644 --- a/CGII/src/IKViewer.cpp +++ b/CGII/src/IKViewer.cpp @@ -38,6 +38,19 @@ void IKViewer::base_changed(Bone *b) post_redraw(); } +int IKViewer::find_element(std::vector &v, Bone *element) +{ + for (int i = 0; i < v.size(); i++) + { + if (v[i] == element) + { + return i; + } + } + + return -1; +} + void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector) { std::cout << "calculate_kinematic_chain" << std::endl; @@ -63,38 +76,108 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector) std::cout << std::endl; /*Task 3.1: Calculate kinematic chain*/ + kinematic_chain.clear(); this->base = base; this->endeffector = endeffector; // get list of bones in order of endeffector to base - std::vector bones; + std::vector endeffector_tree; + std::vector base_tree; + + // gather all bones from endeffector to root Bone *current_bone = endeffector; while (1) { - bones.emplace_back(current_bone); + endeffector_tree.emplace_back(current_bone); // break if we reached base bone - if (current_bone == base) + if (current_bone->get_parent() == nullptr) break; // set next bone in hierarchy current_bone = current_bone->get_parent(); } - kinematic_chain.clear(); + // gather all bones from base to root + current_bone = base; - for (Bone *bone : bones) + while (1) { - for (int i = bone->dof_count() - 1; i >= 0; i--) - { - kinematic_chain.emplace_front(bone->get_dof(i)); - } + base_tree.emplace_back(current_bone); - Transform *tmp = new StaticTransform(bone->calculate_transform_prev_to_current_without_dofs()); - std::shared_ptr transform = std::shared_ptr(tmp); - kinematic_chain.emplace_front(transform); + // break if we reached base bone + if (current_bone->get_parent() == nullptr) + break; + + // set next bone in hierarchy + current_bone = current_bone->get_parent(); + } + + // add all transform from endeffector to common ancestor and to reverse base + + // state == 0 means, iterating through endeffector_tree + // state == 1 means, reverse iterating through base_tree + int state = 0; + + int endeffector_index = 0; + int base_index = 0; + + //TODO: check if common ancestor is visited twice + + while (1) + { + if (state == 0) + { + if (endeffector_index >= endeffector_tree.size()) + break; + + Bone *bone = endeffector_tree[endeffector_index]; + + for (int i = bone->dof_count() - 1; i >= 0; i--) + { + kinematic_chain.emplace_front(bone->get_dof(i)); + } + + Transform *tmp = new StaticTransform(bone->calculate_transform_prev_to_current_without_dofs()); + std::shared_ptr transform = std::shared_ptr(tmp); + kinematic_chain.emplace_front(transform); + + int index = find_element(base_tree, bone); + if (index != -1) + { + base_index = index; + state = 1; + continue; + } + + endeffector_index++; + } + else if (state == 1) + { + if (base_index < 0) + break; + + Bone *bone = base_tree[base_index]; + + for (int i = bone->dof_count() - 1; i >= 0; i--) + { + Transform *tmp = new InverseTransform(bone->get_dof(i)); + std::shared_ptr inverse_dof = std::shared_ptr(tmp); + kinematic_chain.emplace_front(inverse_dof); + } + + std::shared_ptr inverse_static = std::shared_ptr(new StaticTransform(inv(bone->calculate_transform_prev_to_current_without_dofs()))); + kinematic_chain.emplace_front(inverse_static); + + base_index--; + } + else + { + // can't ever happen, but when it does, we will know + abort(); + } } current_endeffector_matrix.identity(); @@ -107,6 +190,7 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector) target_position = current_endeffector_matrix * endeffector->get_bone_local_tip_position(); print_vec3(target_position); + std::cout << std::endl; } void IKViewer::optimize() @@ -125,7 +209,7 @@ void IKViewer::optimize() { for (auto it = kinematic_chain.rbegin(); it != kinematic_chain.rend(); i++) { - it->optimize_value(/*local_vector*/, target_position); + //it->optimize_value(/*local_vector*/, target_position); } current_endeffector_matrix.identity(); diff --git a/CGII/src/IKViewer.h b/CGII/src/IKViewer.h index e999df7..1684139 100644 --- a/CGII/src/IKViewer.h +++ b/CGII/src/IKViewer.h @@ -33,6 +33,7 @@ class IKViewer : public node, public drawable, public provider, public event_han void endeffector_changed(Bone *); void base_changed(Bone *); + int find_element(std::vector &v, Bone *element); void calculate_kinematic_chain(Bone *base, Bone *endeffector); bool modifying; //Specifies if the user is currently modifying the IK target position