C++ Quaterniond::slerp方法代码示例

// 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 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());
//.........这里部分代码省略.........

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

//.........这里部分代码省略.........

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();
//.........这里部分代码省略.........