double ThreadPiece::twist_angle_error()
{
double angle_err_frames = angle_mismatch(_material_frame, _bishop_frame);
/*
double angle = angle_err_frames - _angle_twist;
while (angle < -M_PI)
angle += 2.0*M_PI;
while (angle > M_PI)
angle -= 2.0*M_PI;
return angle;
*/
return atan2( sin(angle_err_frames - _angle_twist), cos(angle_err_frames - _angle_twist));
}
void GLThread::ApplyUserInput(float move_end[], float tangent_end[], float tangent_rotation_end[], float move_start[], float tangent_start[], float tangent_rotation_start[])
{
GLdouble model_view[16];
glGetDoublev(GL_MODELVIEW_MATRIX, model_view);
GLdouble projection[16];
glGetDoublev(GL_PROJECTION_MATRIX, projection);
GLint viewport[4];
glGetIntegerv(GL_VIEWPORT, viewport);
double winX, winY, winZ;
Two_Motions motion_to_apply;
motion_to_apply._start.set_nomotion();
//change end positions
Vector3d new_end_pos;
gluProject(positions[1](0), positions[1](1), positions[1](2), model_view, projection, viewport, &winX, &winY, &winZ);
winX += move_end[0];
winY += move_end[1];
move_end[0] = 0.0;
move_end[1] = 0.0;
gluUnProject(winX, winY, winZ, model_view, projection, viewport, &new_end_pos(0), &new_end_pos(1), &new_end_pos(2));
// std::cout << "X: " << positions[1](0) << " Y: " << positions[1](1) << " Z: " << positions[1](2) << std::endl;
//change tangents
Vector3d new_end_tan;
gluProject(positions[1](0)+tangents[1](0),positions[1](1)+tangents[1](1), positions[1](2)+tangents[1](2), model_view, projection, viewport, &winX, &winY, &winZ);
winX += tangent_end[0];
winY += tangent_end[1];
tangent_end[0] = 0.0;
tangent_end[1] = 0.0;
gluUnProject(winX, winY, winZ, model_view, projection, viewport, &new_end_tan(0), &new_end_tan(1), &new_end_tan(2));
new_end_tan -= positions[1];
new_end_tan.normalize();
//positions[1] = new_end_pos;
motion_to_apply._end._pos_movement = new_end_pos-positions[1];
Matrix3d rotation_new_tan;
rotate_between_tangents(tangents[1], new_end_tan, rotation_new_tan);
//rotations[1] = rotation_new_tan*rotations[1];
motion_to_apply._end._frame_rotation = rotation_new_tan;
//check rotation around tangent
Vector3d tangent_normal_rotate_around = rotations[1].col(1);
Vector3d new_end_tan_normal;
gluProject(positions[1](0)+tangent_normal_rotate_around(0), positions[1](1)+tangent_normal_rotate_around(1), positions[1](2)+tangent_normal_rotate_around(2), model_view, projection, viewport, &winX, &winY, &winZ);
winX += tangent_rotation_end[0];
winY += tangent_rotation_end[1];
tangent_rotation_end[0] = 0.0;
tangent_rotation_end[1] = 0.0;
gluUnProject(winX, winY, winZ, model_view, projection, viewport, &new_end_tan_normal(0), &new_end_tan_normal(1), &new_end_tan_normal(2));
new_end_tan_normal -= positions[1];
//project this normal onto the plane normal to X (to ensure Y stays normal to X)
new_end_tan_normal -= new_end_tan_normal.dot(rotations[1].col(0))*rotations[1].col(0);
new_end_tan_normal.normalize();
rotations[1].col(1) = new_end_tan_normal;
rotations[1].col(2) = rotations[1].col(0).cross(new_end_tan_normal);
motion_to_apply._end._frame_rotation = Eigen::AngleAxisd(angle_mismatch(rotations[1], _thread->end_rot()), rotations[1].col(0).normalized())*motion_to_apply._end._frame_rotation;
//change start positions
Vector3d new_start_pos;
gluProject(positions[0](0), positions[0](1), positions[0](2), model_view, projection, viewport, &winX, &winY, &winZ);
winX += move_start[0];
winY += move_start[1];
move_start[0] = 0.0;
move_start[1] = 0.0;
gluUnProject(winX, winY, winZ, model_view, projection, viewport, &new_start_pos(0), &new_start_pos(1), &new_start_pos(2));
// std::cout << "X: " << positions[1](0) << " Y: " << positions[1](1) << " Z: " << positions[1](2) << std::endl;
//change start tangents
Vector3d new_start_tan;
gluProject(positions[0](0)+tangents[0](0),positions[0](1)+tangents[0](1), positions[0](2)+tangents[0](2), model_view, projection, viewport, &winX, &winY, &winZ);
winX += tangent_start[0];
winY += tangent_start[1];
tangent_start[0] = 0.0;
tangent_start[1] = 0.0;
gluUnProject(winX, winY, winZ, model_view, projection, viewport, &new_start_tan(0), &new_start_tan(1), &new_start_tan(2));
new_start_tan -= positions[0];
new_start_tan.normalize();
//positions[0] = new_start_pos;
motion_to_apply._start._pos_movement = new_start_pos-positions[0];
Matrix3d rotation_new_start_tan;
rotate_between_tangents(tangents[0], new_start_tan, rotation_new_tan);
//rotations[0] = rotation_new_start_tan*rotations[0];
motion_to_apply._start._frame_rotation = rotation_new_tan;
//check rotation around start tangent
tangent_normal_rotate_around = rotations[0].col(1);
Vector3d new_start_tan_normal;
gluProject(positions[0](0)+tangent_normal_rotate_around(0), positions[0](1)+tangent_normal_rotate_around(1), positions[0](2)+tangent_normal_rotate_around(2), model_view, projection, viewport, &winX, &winY, &winZ);
winX += tangent_rotation_start[0];
winY += tangent_rotation_start[1];
tangent_rotation_start[0] = 0.0;
tangent_rotation_start[1] = 0.0;
gluUnProject(winX, winY, winZ, model_view, projection, viewport, &new_start_tan_normal(0), &new_start_tan_normal(1), &new_start_tan_normal(2));
new_start_tan_normal -= positions[0];
//project this normal onto the plane normal to X (to ensure Y stays normal to X)
new_start_tan_normal -= new_start_tan_normal.dot(rotations[0].col(0))*rotations[0].col(0);
new_start_tan_normal.normalize();
rotations[0].col(1) = new_start_tan_normal;
rotations[0].col(2) = rotations[0].col(0).cross(new_start_tan_normal);
double angle_change_start = angle_mismatch(rotations[0], _thread->start_rot());
motion_to_apply._start._frame_rotation = Eigen::AngleAxisd(angle_change_start, rotations[0].col(0).normalized())*motion_to_apply._start._frame_rotation;
//change thread
//_thread->set_constraints(positions[0], rotations[0], positions[1], rotations[1]);
//thread->set_end_constraint(positions[1], rotations[1]);
_thread->apply_motion_nearEnds(motion_to_apply, false);
// std::cout <<"CONSTRAINT END:\n" << rotations[1] << std::endl;
//_thread->minimize_energy();
updateThreadPoints();
// std::cout <<"minimized END:\n" << rotations[1] << std::endl;
}
