now current_endeffector_matrix has transformation to the tip

This commit is contained in:
hodasemi 2018-06-14 09:28:23 +02:00
parent a792259004
commit 697268cd0b

View file

@ -131,6 +131,9 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
//TODO: check if common ancestor is visited twice
std::shared_ptr<Transform> static_trans = std::shared_ptr<Transform>(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<Transform> inverse_dof = std::shared_ptr<Transform>(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()