Uni/CGII
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

Start CCD

Browse source
This commit is contained in:
hodasemi 2018-06-18 08:38:09 +02:00
parent f6ebfa09ae
commit 4905a5ff8a
1 changed files with 30 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

35
CGII/src/IKViewer.cpp
View file

@ -209,15 +209,40 @@ void IKViewer::optimize()
auto skeleton_size = (data->get_skeleton()->getMax() - data->get_skeleton()->getMin());
float distance_threshold = 0.0001f * std::max({skeleton_size.x(), skeleton_size.y(), skeleton_size.z()});
//split the current matrix in:
// before_dof -> dof -> after_dof
/*Task 3.3: Implement CCD */
for (int i = 0; i < max_iterations; i++)
{
for (auto it = kinematic_chain.rbegin(); it != kinematic_chain.rend(); it++)
int kc_size = kinematic_chain.size();
// reverse iterate through kinematic chain
for (int j = kc_size - 1; j >= 0; j--)
{
//it->optimize_value(/*local_vector*/, target_position);
auto before_dof = current_base_matrix;
for (int k = 0; k < j; k++)
{
before_dof = before_dof * kinematic_chain[k];
}
auto after_dof;
after_dof.identity();
int after_dof_index = j + 1;
for (int k = j + 1; k < kc_size; k++)
{
after_dof = after_dof * kinematic_chain[k];
}
// now we got 3 matrices
// (1) before dof: base_matrix * kinematic_chain[0...j-1]
// (2) dof: kinematic_chain[j]
// (3) after_dof: kinematic_chain[j+1...kc_size-1]
auto v_local = after_dof * Vec4(0.0f, 0.0f, 0.0f, 1.0f);
auto v_target = inv(before_dof) * target_position;
it->optimize_value(v_local, v_target);
}
current_endeffector_matrix.identity();