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;
}