Merge solution of task 3
This commit is contained in:
parent
d1c4a03f5e
commit
3d00ece41d
5 changed files with 207 additions and 311 deletions
BIN
04_Skinning.pdf
Normal file
BIN
04_Skinning.pdf
Normal file
Binary file not shown.
|
@ -1,5 +1,5 @@
|
||||||
// This source code is property of the Computer Graphics and Visualization
|
// This source code is property of the Computer Graphics and Visualization
|
||||||
// chair of the TU Dresden. Do not distribute!
|
// chair of the TU Dresden. Do not distribute!
|
||||||
// Copyright (C) CGV TU Dresden - All Rights Reserved
|
// Copyright (C) CGV TU Dresden - All Rights Reserved
|
||||||
//
|
//
|
||||||
#include "AtomicTransform.h"
|
#include "AtomicTransform.h"
|
||||||
|
@ -16,14 +16,14 @@ void AtomicTransform::set_limits(double lower, double upper)
|
||||||
this->value = 0;
|
this->value = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void AtomicTransform::set_value(const double &value, void *)
|
void AtomicTransform::set_value(const double& value, void*)
|
||||||
{
|
{
|
||||||
this->value = value;
|
this->value = value;
|
||||||
changed_signal(value);
|
changed_signal(value);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::string AtomicTransform::get_name() const { return title; }
|
std::string AtomicTransform::get_name() const { return title; }
|
||||||
const double AtomicTransform::get_value(void *) const { return value; }
|
const double AtomicTransform::get_value(void*) const { return value; }
|
||||||
const double AtomicTransform::get_lower_limit() const { return lower_limit; }
|
const double AtomicTransform::get_lower_limit() const { return lower_limit; }
|
||||||
const double AtomicTransform::get_upper_limit() const { return upper_limit; }
|
const double AtomicTransform::get_upper_limit() const { return upper_limit; }
|
||||||
|
|
||||||
|
@ -37,7 +37,7 @@ AtomicRotationTransform::AtomicRotationTransform(Vec3 axis)
|
||||||
|
|
||||||
Mat4 AtomicRotationTransform::calculate_matrix()
|
Mat4 AtomicRotationTransform::calculate_matrix()
|
||||||
{
|
{
|
||||||
return rotate(axis, (float)value);
|
return rotate(axis, (float)value);
|
||||||
}
|
}
|
||||||
|
|
||||||
void AtomicRotationTransform::drawIndicator(float size)
|
void AtomicRotationTransform::drawIndicator(float size)
|
||||||
|
@ -80,76 +80,58 @@ void AtomicRotationTransform::drawActualIndicator(float size)
|
||||||
glEnd();
|
glEnd();
|
||||||
}
|
}
|
||||||
|
|
||||||
template <class T>
|
void AtomicRotationTransform::optimize_value(const Vec3& local_vector, const Vec3& target, bool inverse)
|
||||||
T clamp(T &v, T &c1, T &c2)
|
|
||||||
{
|
{
|
||||||
if (v < c1)
|
//project vectors in the tangent plane of the axis and normalize
|
||||||
return c1;
|
Vec3 local_in_tangent_plane = local_vector - cgv::math::dot(local_vector, axis) * axis;
|
||||||
|
auto length = local_in_tangent_plane.length();
|
||||||
if (v > c2)
|
if (length < 0.01)
|
||||||
return c2;
|
return; //the local vector is mostly parallel to the axis; there is not much we can do
|
||||||
|
local_in_tangent_plane.normalize();
|
||||||
return v;
|
|
||||||
}
|
|
||||||
|
|
||||||
float angle(const Vec3 &v1, const Vec3 &v2)
|
|
||||||
{
|
|
||||||
return std::acos(dot(v1, v2) / (length(v1) * length(v2))) * 180.0f / PI;
|
|
||||||
}
|
|
||||||
|
|
||||||
float optimize_angle(const Vec3 &v1, const Vec3 &v2)
|
|
||||||
{
|
|
||||||
Vec3 first_cross = cross(v1, v2);
|
|
||||||
Vec3 second_cross = cross(v1, first_cross);
|
|
||||||
|
|
||||||
float plane_angle = angle(second_cross, v2);
|
|
||||||
|
|
||||||
if (plane_angle >= 90.0f)
|
|
||||||
{
|
|
||||||
return -angle(v1, v2);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
return angle(v1, v2);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse)
|
|
||||||
{
|
|
||||||
/*Task: Implement parameter optimization*/
|
|
||||||
|
|
||||||
// optimize this that: target = this->calculate_matrix() * local_vector;
|
|
||||||
|
|
||||||
double result = 0.0;
|
|
||||||
|
|
||||||
Vec3 lp = local_vector - (dot(local_vector, axis) * axis);
|
|
||||||
Vec3 tp = target - (dot(target, axis) * axis);
|
|
||||||
|
|
||||||
result = (double)angle(lp, tp);
|
|
||||||
|
|
||||||
|
Vec3 target_in_tangent_plane = target - cgv::math::dot(target, axis) * axis;
|
||||||
|
length = target_in_tangent_plane.length();
|
||||||
|
if (length < 0.01)
|
||||||
|
return; //the target vector is mostly parallel to the axis; there is not much we can do
|
||||||
|
target_in_tangent_plane.normalize();
|
||||||
|
|
||||||
|
double optimal_value = atan2(
|
||||||
|
cgv::math::dot(axis, cgv::math::cross(local_in_tangent_plane, target_in_tangent_plane)),
|
||||||
|
cgv::math::dot(local_in_tangent_plane, target_in_tangent_plane)) * 180.0f / 3.1415926f;
|
||||||
if (inverse)
|
if (inverse)
|
||||||
result = -result;
|
optimal_value *= -1;
|
||||||
|
if (optimal_value < lower_limit || optimal_value > upper_limit)
|
||||||
result = clamp(result, lower_limit, upper_limit);
|
{
|
||||||
|
//check which limit is closer
|
||||||
set_value(result);
|
while (abs(upper_limit - optimal_value) > 180.0f)
|
||||||
|
optimal_value += 360.0f * (optimal_value < upper_limit ? 1.0f : -1.0f);
|
||||||
|
double distance_to_upper = abs(upper_limit - optimal_value);
|
||||||
|
while (abs(lower_limit - optimal_value) > 180.0f)
|
||||||
|
optimal_value += 360.0f * (optimal_value < lower_limit ? 1.0f : -1.0f);
|
||||||
|
double distance_to_lower = abs(lower_limit - optimal_value);
|
||||||
|
if (distance_to_lower < distance_to_upper)
|
||||||
|
optimal_value = lower_limit;
|
||||||
|
else
|
||||||
|
optimal_value = upper_limit;
|
||||||
|
}
|
||||||
|
set_value(optimal_value);
|
||||||
}
|
}
|
||||||
|
|
||||||
AtomicTranslationTransform::AtomicTranslationTransform(int dim)
|
AtomicTranslationTransform::AtomicTranslationTransform(int dim)
|
||||||
: dim(dim)
|
: dim(dim)
|
||||||
{
|
{
|
||||||
lower_limit = -10;
|
lower_limit = -10;
|
||||||
upper_limit = 10;
|
upper_limit = 10;
|
||||||
}
|
}
|
||||||
|
|
||||||
Mat4 AtomicTranslationTransform::calculate_matrix()
|
Mat4 AtomicTranslationTransform::calculate_matrix()
|
||||||
{
|
{
|
||||||
Mat4 result;
|
Mat4 result;
|
||||||
result.identity();
|
result.identity();
|
||||||
result(dim, 3) = (float)value;
|
result(dim, 3) = (float)value;
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
void AtomicTranslationTransform::optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse)
|
void AtomicTranslationTransform::optimize_value(const Vec3& local_vector, const Vec3& target, bool inverse)
|
||||||
{
|
{
|
||||||
}
|
}
|
|
@ -1,5 +1,5 @@
|
||||||
// This source code is property of the Computer Graphics and Visualization
|
// This source code is property of the Computer Graphics and Visualization
|
||||||
// chair of the TU Dresden. Do not distribute!
|
// chair of the TU Dresden. Do not distribute!
|
||||||
// Copyright (C) CGV TU Dresden - All Rights Reserved
|
// Copyright (C) CGV TU Dresden - All Rights Reserved
|
||||||
//
|
//
|
||||||
#include "IKViewer.h"
|
#include "IKViewer.h"
|
||||||
|
@ -12,199 +12,120 @@
|
||||||
#include <cgv/math/inv.h>
|
#include <cgv/math/inv.h>
|
||||||
#include <cgv/math/mat.h>
|
#include <cgv/math/mat.h>
|
||||||
|
|
||||||
Vec3 into_vec3(const Vec4 &v)
|
IKViewer::IKViewer(DataStore* data)
|
||||||
{
|
|
||||||
return Vec3(v.x(), v.y(), v.z());
|
|
||||||
}
|
|
||||||
|
|
||||||
IKViewer::IKViewer(DataStore *data)
|
|
||||||
: node("IK Viewer"), data(data), modifying(false), target_position(0, 0, 0, 1), max_iterations(20)
|
: node("IK Viewer"), data(data), modifying(false), target_position(0, 0, 0, 1), max_iterations(20)
|
||||||
{
|
{
|
||||||
connect(data->endeffector_changed, this, &IKViewer::endeffector_changed);
|
connect(data->endeffector_changed, this, &IKViewer::endeffector_changed);
|
||||||
connect(data->base_changed, this, &IKViewer::base_changed);
|
connect(data->base_changed, this, &IKViewer::base_changed);
|
||||||
}
|
}
|
||||||
|
|
||||||
void IKViewer::endeffector_changed(Bone *b)
|
void IKViewer::endeffector_changed(Bone* b)
|
||||||
{
|
{
|
||||||
calculate_kinematic_chain(data->get_base(), data->get_endeffector());
|
calculate_kinematic_chain(data->get_base(), data->get_endeffector());
|
||||||
|
|
||||||
post_redraw();
|
post_redraw();
|
||||||
}
|
}
|
||||||
|
|
||||||
void IKViewer::base_changed(Bone *b)
|
void IKViewer::base_changed(Bone* b)
|
||||||
{
|
{
|
||||||
calculate_kinematic_chain(data->get_base(), data->get_endeffector());
|
calculate_kinematic_chain(data->get_base(), data->get_endeffector());
|
||||||
|
|
||||||
post_redraw();
|
post_redraw();
|
||||||
}
|
}
|
||||||
|
|
||||||
int IKViewer::find_element(std::vector<Bone *> &v, Bone *element)
|
void IKViewer::calculate_kinematic_chain(Bone* base, Bone* endeffector)
|
||||||
{
|
{
|
||||||
for (int i = 0; i < v.size(); i++)
|
if (!base || !endeffector)
|
||||||
{
|
|
||||||
if (v[i] == element)
|
|
||||||
{
|
|
||||||
return i;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
|
|
||||||
{
|
|
||||||
/*Task 3.1: Calculate kinematic chain*/
|
|
||||||
kinematic_chain.clear();
|
|
||||||
|
|
||||||
this->base = base;
|
|
||||||
this->endeffector = endeffector;
|
|
||||||
|
|
||||||
if (!base)
|
|
||||||
return;
|
return;
|
||||||
|
Bone *b;
|
||||||
// gather all bones from base to root
|
Mat4 transform_root_to_base;
|
||||||
std::vector<Bone *> base_tree;
|
transform_root_to_base.identity();
|
||||||
|
std::unordered_set<Bone*> base_path_to_root;
|
||||||
Bone *current_bone = base;
|
if (base)
|
||||||
|
|
||||||
while (1)
|
|
||||||
{
|
{
|
||||||
base_tree.emplace_back(current_bone);
|
//traverse from base to root
|
||||||
|
b = base;
|
||||||
// break if we reached base bone
|
while (b)
|
||||||
if (current_bone->get_parent() == nullptr)
|
|
||||||
break;
|
|
||||||
|
|
||||||
// set next bone in hierarchy
|
|
||||||
current_bone = current_bone->get_parent();
|
|
||||||
}
|
|
||||||
|
|
||||||
// calculate base transformation
|
|
||||||
std::list<std::shared_ptr<Transform>> base_chain;
|
|
||||||
|
|
||||||
for (Bone *bone : base_tree)
|
|
||||||
{
|
|
||||||
for (int i = bone->dof_count() - 1; i >= 0; i--)
|
|
||||||
{
|
{
|
||||||
base_chain.emplace_front(bone->get_dof(i));
|
transform_root_to_base = b->get_translation_transform_current_joint_to_next() * transform_root_to_base;
|
||||||
|
for (int i = b->dof_count() - 1; i >= 0; --i)
|
||||||
|
transform_root_to_base = b->get_dof(i)->calculate_matrix() * transform_root_to_base;
|
||||||
|
transform_root_to_base = b->get_orientation_transform_prev_joint_to_current() * transform_root_to_base;
|
||||||
|
base_path_to_root.insert(b);
|
||||||
|
b = b->get_parent();
|
||||||
}
|
}
|
||||||
|
current_base_matrix = data->get_skeleton()->get_origin() * transform_root_to_base;
|
||||||
Transform *tmp = new StaticTransform(bone->calculate_transform_prev_to_current_without_dofs());
|
|
||||||
std::shared_ptr<Transform> transform = std::shared_ptr<Transform>(tmp);
|
|
||||||
base_chain.emplace_front(transform);
|
|
||||||
}
|
}
|
||||||
|
if (!base || !endeffector)
|
||||||
current_base_matrix.identity();
|
|
||||||
|
|
||||||
for (auto &transform : base_chain)
|
|
||||||
{
|
|
||||||
current_base_matrix = current_base_matrix * transform->calculate_matrix();
|
|
||||||
}
|
|
||||||
|
|
||||||
current_base_matrix = current_base_matrix * base->get_translation_transform_current_joint_to_next();
|
|
||||||
|
|
||||||
if (!endeffector)
|
|
||||||
return;
|
return;
|
||||||
|
Mat4 t;
|
||||||
// get list of bones in order of endeffector to base
|
|
||||||
std::vector<Bone *> endeffector_tree;
|
|
||||||
|
|
||||||
// gather all bones from endeffector to root
|
|
||||||
current_bone = endeffector;
|
|
||||||
|
|
||||||
while (1)
|
|
||||||
{
|
|
||||||
endeffector_tree.emplace_back(current_bone);
|
|
||||||
|
|
||||||
// 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
|
|
||||||
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
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> transform = std::shared_ptr<Transform>(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];
|
|
||||||
|
|
||||||
std::shared_ptr<Transform> inverse_static = std::shared_ptr<Transform>(new StaticTransform(inv(bone->calculate_transform_prev_to_current_without_dofs())));
|
|
||||||
kinematic_chain.emplace_front(inverse_static);
|
|
||||||
|
|
||||||
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);
|
|
||||||
kinematic_chain.emplace_front(inverse_dof);
|
|
||||||
}
|
|
||||||
|
|
||||||
base_index--;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
// can never happen, but when it does, we will know
|
|
||||||
abort();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
std::shared_ptr<Transform> inverse_static = std::shared_ptr<Transform>(new StaticTransform(inv(base->get_translation_transform_current_joint_to_next())));
|
|
||||||
kinematic_chain.emplace_front(inverse_static);
|
|
||||||
|
|
||||||
current_endeffector_matrix.identity();
|
current_endeffector_matrix.identity();
|
||||||
kinematic_vector.clear();
|
kinematic_chain.clear();
|
||||||
|
common_ancestor = nullptr;
|
||||||
for (auto &transform : kinematic_chain)
|
//traverse from endeffector to lowest common ancestor
|
||||||
|
b = endeffector;
|
||||||
|
while (b)
|
||||||
{
|
{
|
||||||
kinematic_vector.emplace_back(transform);
|
if (base_path_to_root.find(b) != base_path_to_root.end())
|
||||||
current_endeffector_matrix = current_endeffector_matrix * transform->calculate_matrix();
|
{
|
||||||
|
common_ancestor = b;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
t = b->get_translation_transform_current_joint_to_next();
|
||||||
|
kinematic_chain.push_front(std::make_shared<StaticTransform>(t));
|
||||||
|
current_endeffector_matrix = t * current_endeffector_matrix;
|
||||||
|
for (int current_dof_id = b->dof_count() - 1; current_dof_id >= 0; --current_dof_id)
|
||||||
|
{
|
||||||
|
auto dof = b->get_dof(current_dof_id);
|
||||||
|
t = dof->calculate_matrix();
|
||||||
|
kinematic_chain.push_front(dof);
|
||||||
|
current_endeffector_matrix = t * current_endeffector_matrix;
|
||||||
|
}
|
||||||
|
t = b->get_orientation_transform_prev_joint_to_current();
|
||||||
|
kinematic_chain.push_front(std::make_shared<StaticTransform>(t));
|
||||||
|
current_endeffector_matrix = t * current_endeffector_matrix;
|
||||||
|
b = b->get_parent();
|
||||||
}
|
}
|
||||||
|
//traverse from base to lowest common ancestor
|
||||||
target_position = current_base_matrix * current_endeffector_matrix * Vec4(0.0f, 0.0f, 0.0f, 1.0f);
|
std::deque<std::shared_ptr<Transform>> chain_base_to_ancestor;
|
||||||
|
Mat4 matrix_base_to_ancestor; matrix_base_to_ancestor.identity();
|
||||||
|
b = base;
|
||||||
|
while (b)
|
||||||
|
{
|
||||||
|
if (b == common_ancestor)
|
||||||
|
break;
|
||||||
|
t = cgv::math::inv(b->get_translation_transform_current_joint_to_next());
|
||||||
|
chain_base_to_ancestor.push_back(std::make_shared<StaticTransform>(t));
|
||||||
|
matrix_base_to_ancestor = matrix_base_to_ancestor * t;
|
||||||
|
for (int current_dof_id = b->dof_count() - 1; current_dof_id >= 0; --current_dof_id)
|
||||||
|
{
|
||||||
|
auto dof = b->get_dof(current_dof_id);
|
||||||
|
t = cgv::math::inv(dof->calculate_matrix());
|
||||||
|
chain_base_to_ancestor.push_back(std::make_shared<InverseTransform>(dof));
|
||||||
|
matrix_base_to_ancestor = matrix_base_to_ancestor * t;
|
||||||
|
}
|
||||||
|
t = cgv::math::inv(b->get_orientation_transform_prev_joint_to_current());
|
||||||
|
chain_base_to_ancestor.push_back(std::make_shared<StaticTransform>(t));
|
||||||
|
matrix_base_to_ancestor = matrix_base_to_ancestor * t;
|
||||||
|
b = b->get_parent();
|
||||||
|
}
|
||||||
|
current_endeffector_matrix = matrix_base_to_ancestor * current_endeffector_matrix;
|
||||||
|
kinematic_chain.insert(kinematic_chain.begin(), chain_base_to_ancestor.begin(), chain_base_to_ancestor.end());
|
||||||
|
//traverse from ancestor to root
|
||||||
|
current_ancestor_matrix.identity();
|
||||||
|
b = common_ancestor;
|
||||||
|
while (b)
|
||||||
|
{
|
||||||
|
current_ancestor_matrix = b->get_translation_transform_current_joint_to_next() * current_ancestor_matrix;
|
||||||
|
for (int i = b->dof_count() - 1; i >= 0; --i)
|
||||||
|
current_ancestor_matrix = b->get_dof(i)->calculate_matrix() * current_ancestor_matrix;
|
||||||
|
current_ancestor_matrix = b->get_orientation_transform_prev_joint_to_current() * current_ancestor_matrix;
|
||||||
|
b = b->get_parent();
|
||||||
|
}
|
||||||
|
t = data->get_skeleton()->get_origin() * transform_root_to_base * current_endeffector_matrix;
|
||||||
|
target_position.x() = t(0, 3);
|
||||||
|
target_position.y() = t(1, 3);
|
||||||
|
target_position.z() = t(2, 3);
|
||||||
}
|
}
|
||||||
|
|
||||||
void IKViewer::optimize()
|
void IKViewer::optimize()
|
||||||
|
@ -213,56 +134,51 @@ void IKViewer::optimize()
|
||||||
data->dof_changed_by_ik = true;
|
data->dof_changed_by_ik = true;
|
||||||
|
|
||||||
auto skeleton_size = (data->get_skeleton()->getMax() - data->get_skeleton()->getMin());
|
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()});
|
float distance_threshold = 0.0001f * std::max({ skeleton_size.x(), skeleton_size.y(), skeleton_size.z() });
|
||||||
|
|
||||||
/*Task 3.3: Implement CCD */
|
//split the current matrix in:
|
||||||
for (int i = 0; i < max_iterations; i++)
|
// before_dof -> dof -> after_dof
|
||||||
|
|
||||||
|
for (unsigned int iteration = 0; iteration < max_iterations; ++iteration)
|
||||||
{
|
{
|
||||||
int kc_size = kinematic_vector.size();
|
Mat4 after_dof;
|
||||||
|
after_dof.identity();
|
||||||
// reverse iterate through kinematic chain
|
Mat4 before_dof = current_endeffector_matrix;
|
||||||
for (int j = kc_size - 1; j >= 0; j--)
|
Vec4 target_base_local = cgv::math::inv(current_base_matrix) * target_position;
|
||||||
|
//start from the last bone
|
||||||
|
for (auto it = kinematic_chain.rbegin(); it != kinematic_chain.rend(); ++it)
|
||||||
{
|
{
|
||||||
Mat4 before_dof = current_base_matrix;
|
auto t = *it;
|
||||||
|
before_dof = before_dof * inv(t->calculate_matrix());
|
||||||
for (int k = 0; k < j; k++)
|
Vec4 target_dof_local = inv(before_dof) * target_base_local;
|
||||||
{
|
t->optimize_value(Vec3(after_dof(0, 3), after_dof(1, 3), after_dof(2, 3)), Vec3(target_dof_local));
|
||||||
before_dof = before_dof * kinematic_vector[k]->calculate_matrix();
|
after_dof = t->calculate_matrix() * after_dof;
|
||||||
}
|
|
||||||
|
|
||||||
Mat4 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_vector[k]->calculate_matrix();
|
|
||||||
}
|
|
||||||
|
|
||||||
// now we got 3 matrices
|
|
||||||
// (1) before dof: base_matrix * kinematic_vector[0...j-1]
|
|
||||||
// (2) dof: kinematic_vector[j]
|
|
||||||
// (3) after_dof: kinematic_vector[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;
|
|
||||||
|
|
||||||
kinematic_vector[j]->optimize_value(into_vec3(v_local), into_vec3(v_target));
|
|
||||||
}
|
}
|
||||||
|
current_endeffector_matrix = before_dof * after_dof;
|
||||||
current_endeffector_matrix.identity();
|
float error = Vec3(current_endeffector_matrix(0, 3) - target_base_local.x(),
|
||||||
|
current_endeffector_matrix(1, 3) - target_base_local.y(),
|
||||||
for (auto &transform : kinematic_chain)
|
current_endeffector_matrix(2, 3) - target_base_local.z()).length();
|
||||||
{
|
if (error <= distance_threshold)
|
||||||
current_endeffector_matrix = current_endeffector_matrix * transform->calculate_matrix();
|
|
||||||
}
|
|
||||||
|
|
||||||
Vec4 current_endeffector_position = current_endeffector_matrix * endeffector->get_bone_local_tip_position();
|
|
||||||
|
|
||||||
if (distance_threshold >= length(current_endeffector_position - target_position))
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
//update the origin
|
||||||
|
//traverse from base to ancestor
|
||||||
|
Mat4 transform_ancestor_to_base;
|
||||||
|
transform_ancestor_to_base.identity();
|
||||||
|
Bone* b = data->get_base();
|
||||||
|
while (b)
|
||||||
|
{
|
||||||
|
if (b == common_ancestor)
|
||||||
|
break;
|
||||||
|
transform_ancestor_to_base = b->get_translation_transform_current_joint_to_next() * transform_ancestor_to_base;
|
||||||
|
for (int i = b->dof_count() - 1; i >= 0; --i)
|
||||||
|
transform_ancestor_to_base = b->get_dof(i)->calculate_matrix() * transform_ancestor_to_base;
|
||||||
|
transform_ancestor_to_base = b->get_orientation_transform_prev_joint_to_current() * transform_ancestor_to_base;
|
||||||
|
b = b->get_parent();
|
||||||
|
}
|
||||||
|
Mat4 origin_to_base = current_ancestor_matrix * transform_ancestor_to_base;
|
||||||
|
Mat4 global_to_origin = current_base_matrix * cgv::math::inv(origin_to_base);
|
||||||
|
data->get_skeleton()->set_origin(global_to_origin);
|
||||||
|
|
||||||
//used for correct GUI behavior
|
//used for correct GUI behavior
|
||||||
data->dof_changed_by_ik = false;
|
data->dof_changed_by_ik = false;
|
||||||
|
@ -294,7 +210,7 @@ void IKViewer::set_target_position_2d(int x, int y)
|
||||||
int width = ctx->get_width();
|
int width = ctx->get_width();
|
||||||
int height = ctx->get_height();
|
int height = ctx->get_height();
|
||||||
|
|
||||||
float proj_position[4] = {(2.0f * x) / width - 1.0f, (-2.0f * y) / height + 1.0f, z, 1.0f};
|
float proj_position[4] = { (2.0f * x) / width - 1.0f, (-2.0f * y) / height + 1.0f, z, 1.0f };
|
||||||
auto unprojected = inv_mvp * cgv::math::vec<float>(4, proj_position);
|
auto unprojected = inv_mvp * cgv::math::vec<float>(4, proj_position);
|
||||||
unprojected *= 1.0f / unprojected.w();
|
unprojected *= 1.0f / unprojected.w();
|
||||||
|
|
||||||
|
@ -308,21 +224,19 @@ void IKViewer::set_target_position_2d(int x, int y)
|
||||||
post_redraw();
|
post_redraw();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool IKViewer::handle(event &e)
|
|
||||||
{
|
|
||||||
if (e.get_kind() == EID_MOUSE)
|
|
||||||
{
|
|
||||||
cgv::gui::mouse_event me = (cgv::gui::mouse_event &)e;
|
|
||||||
|
|
||||||
switch (me.get_action())
|
bool IKViewer::handle(event& e)
|
||||||
{
|
{
|
||||||
|
if (e.get_kind() == EID_MOUSE) {
|
||||||
|
cgv::gui::mouse_event me = (cgv::gui::mouse_event&) e;
|
||||||
|
|
||||||
|
switch (me.get_action()) {
|
||||||
case MA_PRESS:
|
case MA_PRESS:
|
||||||
if (me.get_button() == MB_LEFT_BUTTON && me.get_modifiers() == EM_CTRL)
|
if (me.get_button() == MB_LEFT_BUTTON && me.get_modifiers() == EM_CTRL) {
|
||||||
{
|
|
||||||
modifying = true;
|
modifying = true;
|
||||||
set_target_position_2d(me.get_x(), me.get_y());
|
set_target_position_2d(me.get_x(), me.get_y());
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case MA_RELEASE:
|
case MA_RELEASE:
|
||||||
modifying = false;
|
modifying = false;
|
||||||
|
@ -333,25 +247,24 @@ bool IKViewer::handle(event &e)
|
||||||
set_target_position_2d(me.get_x(), me.get_y());
|
set_target_position_2d(me.get_x(), me.get_y());
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
default:
|
default: break;
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void IKViewer::stream_help(std::ostream &os)
|
void IKViewer::stream_help(std::ostream& os)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
void IKViewer::draw(cgv::render::context &ctx)
|
void IKViewer::draw(cgv::render::context& ctx)
|
||||||
{
|
{
|
||||||
if (!data->get_skeleton())
|
if (!data->get_skeleton())
|
||||||
return;
|
return;
|
||||||
|
|
||||||
auto skeleton_size = (data->get_skeleton()->getMax() - data->get_skeleton()->getMin());
|
auto skeleton_size = (data->get_skeleton()->getMax() - data->get_skeleton()->getMin());
|
||||||
float scale = 0.2f * std::max({skeleton_size.x(), skeleton_size.y(), skeleton_size.z()});
|
float scale = 0.2f * std::max({ skeleton_size.x(), skeleton_size.y(), skeleton_size.z() });
|
||||||
|
|
||||||
glBegin(GL_LINES);
|
glBegin(GL_LINES);
|
||||||
|
|
||||||
|
|
|
@ -1,5 +1,5 @@
|
||||||
// This source code is property of the Computer Graphics and Visualization
|
// This source code is property of the Computer Graphics and Visualization
|
||||||
// chair of the TU Dresden. Do not distribute!
|
// chair of the TU Dresden. Do not distribute!
|
||||||
// Copyright (C) CGV TU Dresden - All Rights Reserved
|
// Copyright (C) CGV TU Dresden - All Rights Reserved
|
||||||
//
|
//
|
||||||
#pragma once
|
#pragma once
|
||||||
|
@ -25,42 +25,41 @@ using namespace cgv::utils;
|
||||||
|
|
||||||
class IKViewer : public node, public drawable, public provider, public event_handler
|
class IKViewer : public node, public drawable, public provider, public event_handler
|
||||||
{
|
{
|
||||||
private:
|
private:
|
||||||
DataStore *data;
|
DataStore* data;
|
||||||
|
|
||||||
void set_target_position_2d(int x, int y);
|
void set_target_position_2d(int x, int y);
|
||||||
|
|
||||||
void endeffector_changed(Bone *);
|
void endeffector_changed(Bone*);
|
||||||
void base_changed(Bone *);
|
void base_changed(Bone*);
|
||||||
|
|
||||||
int find_element(std::vector<Bone *> &v, Bone *element);
|
void calculate_kinematic_chain(Bone* base, Bone* endeffector);
|
||||||
void calculate_kinematic_chain(Bone *base, Bone *endeffector);
|
|
||||||
|
|
||||||
bool modifying; //Specifies if the user is currently modifying the IK target position
|
bool modifying; //Specifies if the user is currently modifying the IK target position
|
||||||
|
|
||||||
Vec4 target_position;
|
Vec4 target_position;
|
||||||
Mat4 current_endeffector_matrix; //transform from base to endeffector
|
Mat4 current_endeffector_matrix; //transform from base to endeffector
|
||||||
Mat4 current_base_matrix; //transform from global origin to base
|
Mat4 current_base_matrix; //transform from global origin to base
|
||||||
|
|
||||||
Bone *base;
|
Bone* common_ancestor; //lowest common ancestor of base and endeffector
|
||||||
Bone *endeffector;
|
Mat4 current_ancestor_matrix; //transform from root to common_ancestor
|
||||||
|
|
||||||
unsigned int max_iterations;
|
unsigned int max_iterations;
|
||||||
|
|
||||||
std::list<std::shared_ptr<Transform>> kinematic_chain;
|
std::list<std::shared_ptr<Transform>> kinematic_chain;
|
||||||
std::vector<std::shared_ptr<Transform>> kinematic_vector;
|
|
||||||
|
|
||||||
void optimize();
|
void optimize();
|
||||||
|
|
||||||
public:
|
public:
|
||||||
// The constructor of this class
|
// The constructor of this class
|
||||||
IKViewer(DataStore *);
|
IKViewer(DataStore*);
|
||||||
|
|
||||||
bool handle(cgv::gui::event &e);
|
bool handle(cgv::gui::event& e);
|
||||||
void stream_help(std::ostream &os);
|
void stream_help(std::ostream& os);
|
||||||
|
|
||||||
// Create the gui elements
|
// Create the gui elements
|
||||||
void create_gui();
|
void create_gui();
|
||||||
// Draw the scene
|
// Draw the scene
|
||||||
void draw(context &c);
|
void draw(context& c);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -1,5 +1,5 @@
|
||||||
// This source code is property of the Computer Graphics and Visualization
|
// This source code is property of the Computer Graphics and Visualization
|
||||||
// chair of the TU Dresden. Do not distribute!
|
// chair of the TU Dresden. Do not distribute!
|
||||||
// Copyright (C) CGV TU Dresden - All Rights Reserved
|
// Copyright (C) CGV TU Dresden - All Rights Reserved
|
||||||
//
|
//
|
||||||
// The main file of the plugin. It defines a global register variable
|
// The main file of the plugin. It defines a global register variable
|
||||||
|
@ -16,10 +16,12 @@ using namespace cgv::base;
|
||||||
|
|
||||||
struct Initializer
|
struct Initializer
|
||||||
{
|
{
|
||||||
DataStore *data;
|
DataStore* data;
|
||||||
|
|
||||||
Initializer()
|
Initializer()
|
||||||
{
|
{
|
||||||
|
|
||||||
|
|
||||||
data = new DataStore();
|
data = new DataStore();
|
||||||
|
|
||||||
register_object(base_ptr(new SkeletonViewer(data)), "");
|
register_object(base_ptr(new SkeletonViewer(data)), "");
|
||||||
|
|
Loading…
Reference in a new issue