bool LegFitter::fix_leg_second_lower_joint(Skeleton::Skel_Leg leg,
const osg::Vec3& goal_pos) {
float best_angle = 0.0;
int steps = 314;
float increment = osg::PI / steps;
osg::Matrix first_bone_old_rot, first_bone_old_trans;
osg::Vec3 dir_vec;
skeleton->get_matrices_for_rotate_keep_end_pos(leg, first_bone_old_rot,
first_bone_old_trans, dir_vec);
osg::Matrix m;
Node* parent = skeleton->get_node(leg - 1);
osg::Vec3 length = parent->get_local_end();
if (parent->parent) {
parent->parent->get_global_matrix(current_frame, m);
} else {
m.makeIdentity();
}
osg::Matrix current_m = first_bone_old_rot * first_bone_old_trans * m;
osg::Vec3 bone_end_pos = length * current_m;
float current_distance = (bone_end_pos - goal_pos).length2();
//Try several rotations to find the best
for (float angle = increment; angle < steps; angle += increment) {
osg::Quat new_rot(angle, dir_vec);
osg::Matrix new_m = first_bone_old_rot * osg::Matrix::rotate(new_rot)
* first_bone_old_trans * m;
//We only care if the bones are closer or not, so we do not
//bother to calculate the actual distance
bone_end_pos = length * new_m;
float new_distance = (bone_end_pos - goal_pos).length2();
if (new_distance < current_distance) {
current_distance = new_distance;
best_angle = angle;
}
}
if (best_angle != 0.0) {
skeleton->rotate_two_bones_keep_end_pos(leg, best_angle);
}
return true;
}
void add_mob(int npc, int x, int y, enum e_dir direc)
{
static GLfloat dir[3] = {0.0, 0.0, 0.0};
static GLfloat vec[3] = {0.0, 0.0, 1.0};
GLfloat pos[3];
t_move *move;
t_rot *rot;
t_anim *anim;
t_square *sq;
t_mob *mob;
set_vecf(pos, (float)x * 2, 0.0, (float)y * 2);
anim = new_anim(rand() % 360, 360, anim_mob);
move = new_move(0, 0, pos, dir);
rot = new_rot(0, vec, 90 * direc, 0);
mob = new_mob(anim, move, rot, npc);
sq = g_env->sq + x + y * g_env->mapw;
g_env->npc[npc].sq = sq;
sq->mobs = new_link(sq->mobs, mob);
}
bool LegFitter::fix_leg_second_lower_joint(Skeleton::Skel_Leg leg,
const std::vector<int>& leg_points_index) {
float max_y = skeleton->get_node(leg)->get_end_bone_global_pos(
current_frame).y();
float half_y = skeleton->get_node(leg)->get_global_pos(current_frame).y();
float min_y =
skeleton->get_node(leg - 1)->get_global_pos(current_frame).y();
max_y = max_y - (max_y - half_y) * 0.5;
min_y = min_y + (half_y - min_y) * 0.5;
std::vector<int> reduced_leg_points_index;
reduce_points_with_height(max_y, min_y, leg_points_index,
reduced_leg_points_index);
float best_angle = 0.0;
int steps = 314;
float increment = osg::PI / steps;
osg::Matrix first_bone_old_rot, first_bone_old_trans;
osg::Vec3 dir_vec;
skeleton->get_matrices_for_rotate_keep_end_pos(leg, first_bone_old_rot,
first_bone_old_trans, dir_vec);
osg::Matrix m;
Node* parent = skeleton->get_node(leg - 1);
osg::Vec3 length = parent->get_local_end();
if (parent->parent) {
parent->parent->get_global_matrix(current_frame, m);
} else {
m.makeIdentity();
}
osg::Matrix current_m = first_bone_old_rot * first_bone_old_trans * m;
osg::Vec3 bone_end_pos = length * current_m;
float current_distance = calculate_sum_distance2_to_cloud(bone_end_pos,
reduced_leg_points_index);
//Try several rotations to find the best
for (float angle = increment; angle < steps; angle += increment) {
osg::Quat new_rot(angle, dir_vec);
osg::Matrix new_m = first_bone_old_rot * osg::Matrix::rotate(new_rot)
* first_bone_old_trans * m;
//We only care if the bones are closer or not, so we do not
//bother to calculate the actual distance
bone_end_pos = length * new_m;
//We only care if the bones are closer or not, so we do not
//bother to calculate the actual distance
float new_distance = calculate_sum_distance2_to_cloud(bone_end_pos,
reduced_leg_points_index);
if (new_distance < current_distance) {
current_distance = new_distance;
best_angle = angle;
}
}
//Since the bones have been rotated current_angle, to put them at
//best angle, they have to be rotated best - current
skeleton->rotate_two_bones_keep_end_pos(leg, best_angle);
return true;
}
