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

Merge solution of task 3

Browse source
This commit is contained in:
hodasemi 2018-06-23 11:31:31 +02:00
parent d1c4a03f5e
commit 3d00ece41d
5 changed files with 207 additions and 311 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

BIN
04_Skinning.pdf Normal file
View file

Binary file not shown.

78
CGII/src/AtomicTransform.cpp
View file

@ -80,59 +80,41 @@ void AtomicRotationTransform::drawActualIndicator(float size)
glEnd();
}
template <class T>
T clamp(T &v, T &c1, T &c2)
{
if (v < c1)
return c1;
if (v > c2)
return c2;
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*/
//project vectors in the tangent plane of the axis and normalize
Vec3 local_in_tangent_plane = local_vector - cgv::math::dot(local_vector, axis) * axis;
auto length = local_in_tangent_plane.length();
if (length < 0.01)
return; //the local vector is mostly parallel to the axis; there is not much we can do
local_in_tangent_plane.normalize();
// 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)
result = -result;
result = clamp(result, lower_limit, upper_limit);
set_value(result);
optimal_value *= -1;
if (optimal_value < lower_limit || optimal_value > upper_limit)
{
//check which limit is closer
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)

331
CGII/src/IKViewer.cpp
View file

@ -12,11 +12,6 @@
#include <cgv/math/inv.h>
#include <cgv/math/mat.h>
Vec3 into_vec3(const Vec4 &v)
{
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)
{
@ -38,173 +33,99 @@ void IKViewer::base_changed(Bone *b)
post_redraw();
}
int IKViewer::find_element(std::vector<Bone *> &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)
{
/*Task 3.1: Calculate kinematic chain*/
kinematic_chain.clear();
this->base = base;
this->endeffector = endeffector;
if (!base)
if (!base || !endeffector)
return;
// gather all bones from base to root
std::vector<Bone *> base_tree;
Bone *current_bone = base;
while (1)
Bone *b;
Mat4 transform_root_to_base;
transform_root_to_base.identity();
std::unordered_set<Bone*> base_path_to_root;
if (base)
{
base_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();
}
// calculate base transformation
std::list<std::shared_ptr<Transform>> base_chain;
for (Bone *bone : base_tree)
//traverse from base to root
b = base;
while (b)
{
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();
}
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);
current_base_matrix = data->get_skeleton()->get_origin() * transform_root_to_base;
}
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)
if (!base || !endeffector)
return;
// 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);
Mat4 t;
current_endeffector_matrix.identity();
kinematic_vector.clear();
for (auto &transform : kinematic_chain)
kinematic_chain.clear();
common_ancestor = nullptr;
//traverse from endeffector to lowest common ancestor
b = endeffector;
while (b)
{
kinematic_vector.emplace_back(transform);
current_endeffector_matrix = current_endeffector_matrix * transform->calculate_matrix();
if (base_path_to_root.find(b) != base_path_to_root.end())
{
common_ancestor = b;
break;
}
target_position = current_base_matrix * current_endeffector_matrix * Vec4(0.0f, 0.0f, 0.0f, 1.0f);
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
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()
@ -215,54 +136,49 @@ 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() });
/*Task 3.3: Implement CCD */
for (int i = 0; i < max_iterations; i++)
{
int kc_size = kinematic_vector.size();
//split the current matrix in:
// before_dof -> dof -> after_dof
// reverse iterate through kinematic chain
for (int j = kc_size - 1; j >= 0; j--)
for (unsigned int iteration = 0; iteration < max_iterations; ++iteration)
{
Mat4 before_dof = current_base_matrix;
for (int k = 0; k < j; k++)
{
before_dof = before_dof * kinematic_vector[k]->calculate_matrix();
}
Mat4 after_dof;
after_dof.identity();
int after_dof_index = j + 1;
for (int k = j + 1; k < kc_size; k++)
Mat4 before_dof = current_endeffector_matrix;
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)
{
after_dof = after_dof * kinematic_vector[k]->calculate_matrix();
auto t = *it;
before_dof = before_dof * inv(t->calculate_matrix());
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));
after_dof = t->calculate_matrix() * after_dof;
}
// 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.identity();
for (auto &transform : kinematic_chain)
{
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))
current_endeffector_matrix = before_dof * after_dof;
float error = Vec3(current_endeffector_matrix(0, 3) - target_base_local.x(),
current_endeffector_matrix(1, 3) - target_base_local.y(),
current_endeffector_matrix(2, 3) - target_base_local.z()).length();
if (error <= distance_threshold)
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
data->dof_changed_by_ik = false;
@ -308,17 +224,15 @@ void IKViewer::set_target_position_2d(int x, int y)
post_redraw();
}
bool IKViewer::handle(event& e)
{
if (e.get_kind() == EID_MOUSE)
{
if (e.get_kind() == EID_MOUSE) {
cgv::gui::mouse_event me = (cgv::gui::mouse_event&) e;
switch (me.get_action())
{
switch (me.get_action()) {
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;
set_target_position_2d(me.get_x(), me.get_y());
return true;
@ -334,8 +248,7 @@ bool IKViewer::handle(event &e)
return true;
}
break;
default:
break;
default: break;
}
}
return false;

7
CGII/src/IKViewer.h
View file

@ -33,7 +33,6 @@ class IKViewer : public node, public drawable, public provider, public event_han
void endeffector_changed(Bone*);
void base_changed(Bone*);
int find_element(std::vector<Bone *> &v, Bone *element);
void calculate_kinematic_chain(Bone* base, Bone* endeffector);
bool modifying; //Specifies if the user is currently modifying the IK target position
@ -42,13 +41,12 @@ class IKViewer : public node, public drawable, public provider, public event_han
Mat4 current_endeffector_matrix; //transform from base to endeffector
Mat4 current_base_matrix; //transform from global origin to base
Bone *base;
Bone *endeffector;
Bone* common_ancestor; //lowest common ancestor of base and endeffector
Mat4 current_ancestor_matrix; //transform from root to common_ancestor
unsigned int max_iterations;
std::list<std::shared_ptr<Transform>> kinematic_chain;
std::vector<std::shared_ptr<Transform>> kinematic_vector;
void optimize();
@ -64,3 +62,4 @@ class IKViewer : public node, public drawable, public provider, public event_han
// Draw the scene
void draw(context& c);
};

2
CGII/src/main.cpp
View file

@ -20,6 +20,8 @@ struct Initializer
Initializer()
{
data = new DataStore();
register_object(base_ptr(new SkeletonViewer(data)), "");