From 697268cd0bbed9056e01834d482cc3761c6b2692 Mon Sep 17 00:00:00 2001 From: hodasemi Date: Thu, 14 Jun 2018 09:28:23 +0200 Subject: [PATCH] now current_endeffector_matrix has transformation to the tip --- CGII/src/IKViewer.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/CGII/src/IKViewer.cpp b/CGII/src/IKViewer.cpp index 79306f2..9f7586e 100644 --- a/CGII/src/IKViewer.cpp +++ b/CGII/src/IKViewer.cpp @@ -131,6 +131,9 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector) //TODO: check if common ancestor is visited twice + std::shared_ptr static_trans = std::shared_ptr(new StaticTransform(endeffector->get_translation_transform_current_joint_to_next())); + kinematic_chain.emplace_front(static_trans); + while (1) { if (state == 0) @@ -140,7 +143,8 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector) Bone *bone = endeffector_tree[endeffector_index]; - for (int i = bone->dof_count() - 1; i >= 0; i--) + //for (int i = bone->dof_count() - 1; i >= 0; i--) + for (int i = 0; i < bone->dof_count(); i++) { kinematic_chain.emplace_front(bone->get_dof(i)); } @@ -166,7 +170,8 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector) Bone *bone = base_tree[base_index]; - for (int i = bone->dof_count() - 1; i >= 0; i--) + //for (int i = bone->dof_count() - 1; i >= 0; i--) + for (int i = 0; i < bone->dof_count(); i++) { Transform *tmp = new InverseTransform(bone->get_dof(i)); std::shared_ptr inverse_dof = std::shared_ptr(tmp); @@ -195,7 +200,7 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector) current_endeffector_matrix = current_endeffector_matrix * transform->calculate_matrix(); } - target_position = current_base_matrix * current_endeffector_matrix * endeffector->get_bone_local_tip_position(); + target_position = current_base_matrix * current_endeffector_matrix * Vec4(0.0f, 0.0f, 0.0f, 1.0f); } void IKViewer::optimize()