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