// 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();
}
}
