// update the camera state given a new camera pose void MotionModel::UpdateCameraPose(const cv::Point3d& newPosition, const cv::Vec4d& newOrientation) { // Compute linear velocity cv::Vec3d newLinearVelocity( newPosition - position_ ); // In order to be robust against fast camera movements linear velocity is smoothed over time newLinearVelocity = (newLinearVelocity + linearVelocity_) * 0.5; // compute rotation between q1 and q2: q2 * qInverse(q1); Eigen::Quaterniond newAngularVelocity = cv2eigen( newOrientation ) * orientation_.inverse(); // In order to be robust against fast camera movements angular velocity is smoothed over time newAngularVelocity = newAngularVelocity.slerp(0.5, angularVelocity_); newAngularVelocity.normalize(); // Update the current state variables position_ = newPosition; orientation_ = cv2eigen( newOrientation ); linearVelocity_ = newLinearVelocity; angularVelocity_ = newAngularVelocity; }
void display() { using namespace Eigen; using namespace igl; using namespace std; glClearColor(back[0],back[1],back[2],0); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); if(is_animating) { double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION; if(t > 1) { t = 1; is_animating = false; } const Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized(); s.camera.orbit(q.conjugate()); } glDisable(GL_LIGHTING); lights(); push_scene(); glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LEQUAL); glEnable(GL_NORMALIZE); glEnable(GL_COLOR_MATERIAL); glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE); push_object(); // Draw the model // Set material properties glEnable(GL_COLOR_MATERIAL); const auto draw = []( const MatrixXd & V, const MatrixXi & F, const MatrixXd & N, const MatrixXd & C) { glEnable(GL_COLOR_MATERIAL); glEnable(GL_POLYGON_OFFSET_FILL); // Avoid Stitching! glPolygonOffset(1.0,1); glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); draw_mesh(V,F,N,C); glPolygonMode(GL_FRONT_AND_BACK,GL_LINE); glDisable(GL_COLOR_MATERIAL); const float black[4] = {0,0,0,1}; glColor4fv(black); glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, black); glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, black); glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, black); glLightfv(GL_LIGHT0, GL_AMBIENT, black); glLightfv(GL_LIGHT0, GL_DIFFUSE, black); glLineWidth(1.0); glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); draw_mesh(V,F,N,C); glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); glEnable(GL_COLOR_MATERIAL); }; if(show_A) { draw(V,F,N,C); } if(show_B) { draw(U,G,W,D); } pop_object(); // Draw a nice floor glPushMatrix(); const double floor_offset = -2./bbd*(VU.col(1).maxCoeff()-mid(1)); glTranslated(0,floor_offset,0); const float GREY[4] = {0.5,0.5,0.6,1.0}; const float DARK_GREY[4] = {0.2,0.2,0.3,1.0}; draw_floor(GREY,DARK_GREY); glPopMatrix(); pop_scene(); report_gl_error(); TwDraw(); glutSwapBuffers(); if(is_animating) { glutPostRedisplay(); } }
void display() { using namespace igl; using namespace std; using namespace Eigen; glClearColor(1,1,1,0); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); if(is_animating) { double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION; if(t > 1) { t = 1; is_animating = false; } Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized(); auto & camera = s.camera; switch(center_type) { default: case CENTER_TYPE_ORBIT: camera.orbit(q.conjugate()); break; case CENTER_TYPE_FPS: camera.turn_eye(q.conjugate()); break; } } glEnable(GL_DEPTH_TEST); glEnable(GL_NORMALIZE); lights(); push_scene(); push_object(); // Set material properties glEnable(GL_COLOR_MATERIAL); glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE); if(wireframe_visible) { glPolygonMode(GL_FRONT_AND_BACK,GL_LINE); if(fill_visible) { glColor3f(0,0,0); draw_mesh(V,F,s.N); }else { draw_mesh(V,F,s.N,s.C); } // visualize selected patch glLineWidth(10); glBegin(GL_TRIANGLES); glColor3d(0, 0, 0); // loop over faces for(int i = 0; i<F.rows();i++) { if (CC(i) != cc_selected) continue; // loop over corners of triangle for(int j = 0;j<3;j++) { glVertex3d(V(F(i,j),0),V(F(i,j),1),V(F(i,j),2)); } } glEnd(); glLineWidth(1); glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); glBegin(GL_TRIANGLES); glColor3d(1, 0, 0); // loop over faces for(int i = 0; i<F.rows();i++) { if (CC(i) != cc_selected) continue; // loop over corners of triangle glNormal3d(s.N(i,0),s.N(i,1),s.N(i,2)); for(int j = 0;j<3;j++) { glVertex3d(V(F(i,j),0),V(F(i,j),1),V(F(i,j),2)); } } glEnd(); } glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); if(fill_visible) { glEnable(GL_POLYGON_OFFSET_FILL); // Avoid Stitching! glPolygonOffset(1.0, 0); draw_mesh(V,F,s.N,s.C); } pop_object(); // Draw a nice floor glPushMatrix(); const double floor_offset = -2./bbd*(V.col(1).maxCoeff()-Vmid(1)); glTranslated(0,floor_offset,0); const float GREY[4] = {0.5,0.5,0.6,1.0}; const float DARK_GREY[4] = {0.2,0.2,0.3,1.0}; draw_floor(GREY,DARK_GREY); glPopMatrix(); pop_scene(); report_gl_error(); TwDraw(); glutSwapBuffers(); glutPostRedisplay(); }
void display() { using namespace Eigen; using namespace igl; using namespace std; // Update update_arap(); glClearColor(back[0],back[1],back[2],0); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); if(is_animating) { double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION; if(t > 1) { t = 1; is_animating = false; } const Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized(); s.camera.orbit(q.conjugate()); } glDisable(GL_LIGHTING); lights(); push_scene(); glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LESS); glEnable(GL_NORMALIZE); push_object(); // Draw the model // Set material properties //glDisable(GL_COLOR_MATERIAL); //glMaterialfv(GL_FRONT, GL_AMBIENT, GOLD_AMBIENT); //glMaterialfv(GL_FRONT, GL_DIFFUSE, GOLD_DIFFUSE ); //glMaterialfv(GL_FRONT, GL_SPECULAR, GOLD_SPECULAR); //glMaterialf (GL_FRONT, GL_SHININESS, 128); //glMaterialfv(GL_BACK, GL_AMBIENT, SILVER_AMBIENT); //glMaterialfv(GL_BACK, GL_DIFFUSE, FAST_GREEN_DIFFUSE ); //glMaterialfv(GL_BACK, GL_SPECULAR, SILVER_SPECULAR); //glMaterialf (GL_BACK, GL_SHININESS, 128); glEnable(GL_COLOR_MATERIAL); draw_mesh(U,F,N,C); glDisable(GL_COLOR_MATERIAL); pop_object(); // Draw a nice floor glPushMatrix(); const double floor_offset = -2./bbd*(V.col(1).maxCoeff()-mid(1)); glTranslated(0,floor_offset,0); //const float GREY[4] = {0.5,0.5,0.6,1.0}; //const float DARK_GREY[4] = {0.2,0.2,0.3,1.0}; //draw_floor(GREY,DARK_GREY); draw_floor(); glPopMatrix(); pop_scene(); report_gl_error(); TwDraw(); glutSwapBuffers(); //if(is_animating) //{ glutPostRedisplay(); //} }
void display() { using namespace igl; using namespace std; using namespace Eigen; const float back[4] = {0.75, 0.75, 0.75,0}; glClearColor(back[0],back[1],back[2],0); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); static bool first = true; if(first) { sort(); first = false; } if(is_animating) { double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION; if(t > 1) { t = 1; is_animating = false; } Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized(); camera.orbit(q.conjugate()); } glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LEQUAL); glEnable(GL_NORMALIZE); glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); lights(); push_scene(); // Draw a nice floor glEnable(GL_DEPTH_TEST); glPushMatrix(); const double floor_offset = -2./bbd*(V.col(1).maxCoeff()-Vmid(1)); glTranslated(0,floor_offset,0); const float GREY[4] = {0.5,0.5,0.6,1.0}; const float DARK_GREY[4] = {0.2,0.2,0.3,1.0}; glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); glEnable(GL_CULL_FACE); glCullFace(GL_BACK); draw_floor(GREY,DARK_GREY); glDisable(GL_CULL_FACE); glPopMatrix(); push_object(); const auto & draw_skeleton = []() { switch(skel_style) { default: case SKEL_STYLE_TYPE_3D: { MatrixXf colors = MAYA_VIOLET.transpose().replicate(s.BE.rows(),1); for(int si=0;si<s.sel.size();si++) { for(int b=0;b<s.BE.rows();b++) { if(s.BE(b,0) == s.sel(si) || s.BE(b,1) == s.sel(si)) { colors.row(b) = MAYA_SEA_GREEN; } } } draw_skeleton_3d(s.C,s.BE,MatrixXd(),colors); break; } case SKEL_STYLE_TYPE_VECTOR_GRAPHICS: draw_skeleton_vector_graphics(s.C,s.BE); break; } }; if(!skeleton_on_top) { draw_skeleton(); } // Set material properties glDisable(GL_COLOR_MATERIAL); glMaterialfv(GL_FRONT, GL_AMBIENT, Vector4f(GOLD_AMBIENT[0],GOLD_AMBIENT[1],GOLD_AMBIENT[2],alpha).data()); glMaterialfv(GL_FRONT, GL_DIFFUSE, Vector4f(GOLD_DIFFUSE[0],GOLD_DIFFUSE[1],GOLD_DIFFUSE[2],alpha).data()); glMaterialfv(GL_FRONT, GL_SPECULAR, Vector4f(GOLD_SPECULAR[0],GOLD_SPECULAR[1],GOLD_SPECULAR[2],alpha).data()); glMaterialf (GL_FRONT, GL_SHININESS, 128); glMaterialfv(GL_BACK, GL_AMBIENT, Vector4f(SILVER_AMBIENT[0],SILVER_AMBIENT[1],SILVER_AMBIENT[2],alpha).data()); glMaterialfv(GL_BACK, GL_DIFFUSE, Vector4f(FAST_GREEN_DIFFUSE[0],FAST_GREEN_DIFFUSE[1],FAST_GREEN_DIFFUSE[2],alpha).data()); glMaterialfv(GL_BACK, GL_SPECULAR, Vector4f(SILVER_SPECULAR[0],SILVER_SPECULAR[1],SILVER_SPECULAR[2],alpha).data()); glMaterialf (GL_BACK, GL_SHININESS, 128); if(wireframe) { glPolygonMode(GL_FRONT_AND_BACK,GL_LINE); } glLineWidth(1.0); draw_mesh(V,sorted_F,sorted_N); glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); if(skeleton_on_top) { glDisable(GL_DEPTH_TEST); draw_skeleton(); } pop_object(); pop_scene(); report_gl_error(); TwDraw(); glutSwapBuffers(); if(is_animating) { glutPostRedisplay(); } }
void display() { using namespace igl; using namespace std; using namespace Eigen; const float back[4] = {30.0/255.0,30.0/255.0,50.0/255.0,0}; glClearColor(back[0],back[1],back[2],0); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); if(is_animating) { double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION; if(t > 1) { t = 1; is_animating = false; } Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized(); auto & camera = s.camera; camera.orbit(q.conjugate()); } glEnable(GL_DEPTH_TEST); glEnable(GL_NORMALIZE); lights(); push_scene(); // Draw a nice floor glEnable(GL_DEPTH_TEST); glPushMatrix(); const double floor_offset = -2./bbd*(V.col(1).maxCoeff()-Vmid(1)); glTranslated(0,floor_offset,0); const float GREY[4] = {0.5,0.5,0.6,1.0}; const float DARK_GREY[4] = {0.2,0.2,0.3,1.0}; glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); draw_floor(GREY,DARK_GREY); glPopMatrix(); push_object(); // Set material properties glDisable(GL_COLOR_MATERIAL); glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, SILVER_AMBIENT); glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, SILVER_DIFFUSE ); glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SILVER_SPECULAR); glMaterialf (GL_FRONT_AND_BACK, GL_SHININESS, 128); typedef std::vector< Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > RotationList; RotationList dQ(BE.rows(),Quaterniond::Identity()),vQ; vector<Vector3d> vT; Matrix3d A = Matrix3d::Identity(); for(int e = 0;e<BE.rows();e++) { dQ[e] = AngleAxisd((sin(get_seconds()+e))*0.06*PI,A.col(e%3)); } forward_kinematics(C,BE,P,dQ,vQ,vT); const int dim = C.cols(); MatrixXd T(BE.rows()*(dim+1),dim); for(int e = 0;e<BE.rows();e++) { Affine3d a = Affine3d::Identity(); a.translate(vT[e]); a.rotate(vQ[e]); T.block(e*(dim+1),0,dim+1,dim) = a.matrix().transpose().block(0,0,dim+1,dim); } if(wireframe) { glPolygonMode(GL_FRONT_AND_BACK,GL_LINE); } glLineWidth(1.0); MatrixXd U = M*T; per_face_normals(U,F,N); draw_mesh(U,F,N); glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); if(skeleton_on_top) { glDisable(GL_DEPTH_TEST); } switch(skel_style) { default: case SKEL_STYLE_TYPE_3D: draw_skeleton_3d(C,BE,T,MAYA_VIOLET,bbd*0.5); break; case SKEL_STYLE_TYPE_VECTOR_GRAPHICS: draw_skeleton_vector_graphics(C,BE,T); break; } pop_object(); pop_scene(); report_gl_error(); TwDraw(); glutSwapBuffers(); glutPostRedisplay(); }
void display() { using namespace Eigen; using namespace igl; using namespace std; glClearColor(back[0],back[1],back[2],0); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); if(is_animating) { double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION; if(t > 1) { t = 1; is_animating = false; } const Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized(); s.camera.orbit(q.conjugate()); } glDisable(GL_LIGHTING); lights(); push_scene(); glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LESS); glEnable(GL_NORMALIZE); glEnable(GL_COLOR_MATERIAL); glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE); push_object(); // Draw the model // Set material properties glEnable(GL_COLOR_MATERIAL); glColor3f(1,1,1); glEnable(GL_TEXTURE_2D); glBindTexture(GL_TEXTURE_2D,tex_id); MatrixXd _d; MatrixXi _i; glMatrixMode(GL_TEXTURE); glPushMatrix(); glLoadIdentity(); if(flip_y) { glTranslated(0,1,0); glScaled(1,-1,1); } if(rotate_xy) { glRotated(90,0,0,1); glTranslated(-1,0,0); } glMatrixMode(GL_MODELVIEW); igl::opengl2::draw_mesh(V,F,N,MatrixXi(),MatrixXd(),TC,TF,MatrixXd(),0,MatrixXi(),0); glMatrixMode(GL_TEXTURE); glPopMatrix(); glMatrixMode(GL_MODELVIEW); pop_object(); // Draw a nice floor glPushMatrix(); const double floor_offset = -2./bbd*(V.col(1).maxCoeff()-mid(1)); glTranslated(0,floor_offset,0); const float GREY[4] = {0.5,0.5,0.6,1.0}; const float DARK_GREY[4] = {0.2,0.2,0.3,1.0}; glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(-1,1); glBegin(GL_QUADS); glNormal3d(0,1,0); glTexCoord2d(0,1); glVertex3d(-1,0,1); glTexCoord2d(1,1); glVertex3d(1,0,1); glTexCoord2d(1,0); glVertex3d(1,0,-1); glTexCoord2d(0,0); glVertex3d(-1,0,-1); glEnd(); glDisable(GL_POLYGON_OFFSET_FILL); glDisable(GL_TEXTURE_2D); igl::opengl2::draw_floor(GREY,DARK_GREY); glPopMatrix(); pop_scene(); igl::opengl::report_gl_error(); TwDraw(); glutSwapBuffers(); if(is_animating) { glutPostRedisplay(); } }