Add angle calculation of new pinocchio file

This commit is contained in:
hodasemi 2018-07-12 16:10:15 +02:00
parent cac81a40d9
commit dc3c6c0ce7

View file

@ -544,6 +544,21 @@ Bone *get_bone(int i, std::vector<Bone *> &bone_list)
return bone_list[i];
}
float angle(const Vec3 &v1, const Vec3 &v2)
{
return std::acos(dot(v1, v2) / (length(v1) * length(v2))) * 180.0f / PI;
}
Vec3 project(Vec3 &v, Vec3 &normal)
{
return v - (dot(v, normal) * normal);
}
float calc_angle_on_plane(Vec3 normal, Vec3 &old_direction, Vec3 &new_direction)
{
return angle(project(old_direction, normal), project(new_direction, normal));
}
void Skeleton::read_pinocchio_file(std::string filename)
{
std::ifstream o;
@ -569,23 +584,56 @@ void Skeleton::read_pinocchio_file(std::string filename)
iss >> bone_id >> x >> y >> z >> parent_id;
//std::cout << bone_id << " " << x << " " << y << " " << z << " " << parent_id << std::endl;
Vec3 position(x, y, z);
Bone *current_bone = get_bone(bone_id, bone_list);
if (current_bone == nullptr)
abort();
Vec3 old_direction = current_bone->get_direction_in_world_space();
current_bone->set_length(position.normalize());
current_bone->set_direction_in_world_space(position);
// TODO: add new orientation
Vec3 new_direction = current_bone->get_direction_in_world_space();
// x rotation
float x_angle = calc_angle_on_plane(Vec3(1, 0, 0), old_direction, new_direction);
if (x_angle != 0.0f)
{
AtomicXRotationTransform *x_rotation = new AtomicXRotationTransform;
x_rotation->set_value(x_angle);
current_bone->add_axis_rotation(x_rotation);
}
// y rotation
float y_angle = calc_angle_on_plane(Vec3(0, 1, 0), old_direction, new_direction);
if (y_angle != 0.0f)
{
AtomicYRotationTransform *y_rotation = new AtomicYRotationTransform;
y_rotation->set_value(y_angle);
current_bone->add_axis_rotation(y_rotation);
}
// z rotation
float z_angle = calc_angle_on_plane(Vec3(0, 0, 1), old_direction, new_direction);
if (z_angle != 0.0f)
{
AtomicZRotationTransform *z_rotation = new AtomicZRotationTransform;
z_rotation->set_value(z_angle);
current_bone->add_axis_rotation(z_rotation);
}
current_bone->calculate_matrices();
}
/*Task 4.1: Read Pinocchio file */
reset_bounding_box();
}
o.close();