Exemple #1
0
Eigen::Matrix<T,4,1> blend(T t, Eigen::Matrix<T,4,1> p00, Eigen::Matrix<T,4,1> pNorm,
		   Eigen::Matrix<T,4,1> p10){
  T r0 = p00.mag();
  T r1 = p10.mag();
  T rs = linear(t,r0,r1);
  T dotp = dot(p00,p10);
  dotp = abs(dotp);
  T theta = acos(dotp/r0/r1);
  Eigen::Quaternion<T> dq0;
  p00.normalize();
  p10.normalize();
  Eigen::Quaternion<T> q00(p00);
  Eigen::Quaternion<T> q10(p10);	
  if (theta < 0.01) {
    //just give it a little nudge in the right direction
    T o = 0.10;
    Eigen::Quaternion<T> dq(cos(o/2.), sin(o/2.)*pNorm[0], sin(o/2.)*pNorm[1], sin(o/2.)*pNorm[2]);	
    Eigen::Matrix<T,4,1> p01 = dq.rotate(p00);
    Eigen::Quaternion<T> q01(p01);		
    dq0 = Eigen::Quaternion<T>::slerp(q01,q10,t);	
  } 
  else {
    dq0 = Eigen::Quaternion<T>::slerp(q00,q10,t);	
  }	
  T dott = p00.dot(p10);
  T dotq0 = q00.dot(q10);	
  T dotq1 = dq0.dot(q10);
  return Eigen::Matrix<T,4,1>(rs*dq0[1],rs*dq0[2],rs*dq0[3]);	
  //return Eigen::Matrix<T,4,1>(p01);
}
IGL_INLINE Eigen::Matrix<Scalar, 3, 3> igl::rotation_matrix_from_directions(
  const Eigen::Matrix<Scalar, 3, 1> v0,
  const Eigen::Matrix<Scalar, 3, 1> v1)
{
  Eigen::Matrix<Scalar, 3, 3> rotM;
  const double epsilon=1e-8;
  Scalar dot=v0.normalized().dot(v1.normalized());
  ///control if there is no rotation
  if ((v0-v1).norm()<epsilon)
  {
    rotM = Eigen::Matrix<Scalar, 3, 3>::Identity();
    return rotM;
  }
  if ((v0+v1).norm()<epsilon)
  {
    rotM = -Eigen::Matrix<Scalar, 3, 3>::Identity();
    rotM(0,0) = 1.;
    std::cerr<<"igl::rotation_matrix_from_directions: rotating around x axis by 180o"<<std::endl;
    return rotM;
  }
  ///find the axis of rotation
  Eigen::Matrix<Scalar, 3, 1> axis;
  axis=v0.cross(v1);
  axis.normalize();

  ///construct rotation matrix
  Scalar u=axis(0);
  Scalar v=axis(1);
  Scalar w=axis(2);
  Scalar phi=acos(dot);
  Scalar rcos = cos(phi);
  Scalar rsin = sin(phi);

  rotM(0,0) =      rcos + u*u*(1-rcos);
  rotM(1,0) =  w * rsin + v*u*(1-rcos);
  rotM(2,0) = -v * rsin + w*u*(1-rcos);
  rotM(0,1) = -w * rsin + u*v*(1-rcos);
  rotM(1,1) =      rcos + v*v*(1-rcos);
  rotM(2,1) =  u * rsin + w*v*(1-rcos);
  rotM(0,2) =  v * rsin + u*w*(1-rcos);
  rotM(1,2) = -u * rsin + v*w*(1-rcos);
  rotM(2,2) =      rcos + w*w*(1-rcos);

  return rotM;
}