C++ Matrix::cross方法代码示例

typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::RigidTransformation::correctParameters(const TransformationParameters& parameters) const
{
	TransformationParameters ortho = parameters;
	if(ortho.cols() == 4)
	{
		const Eigen::Matrix<T, 3, 1> col0 = parameters.block(0, 0, 3, 1).normalized();
		const Eigen::Matrix<T, 3, 1> col1 = parameters.block(0, 1, 3, 1).normalized();
		const Eigen::Matrix<T, 3, 1> col2 = parameters.block(0, 2, 3, 1).normalized();


		ortho.block(0, 0, 3, 1) = col1.cross(col2);
		ortho.block(0, 1, 3, 1) = col2.cross(col0);
		ortho.block(0, 2, 3, 1) = col2;
	}
	else if(ortho.cols() == 3)
	{
		// R = [ a b]
		//     [-b a]
		
		// mean of a and b
		T a = (parameters(0,0) + parameters(1,1))/2; 	
		T b = (-parameters(1,0) + parameters(0,1))/2;
		T sum = sqrt(pow(a,2) + pow(b,2));

		a = a/sum;
		b = b/sum;

		ortho(0,0) =  a; ortho(0,1) = b;
		ortho(1,0) = -b; ortho(1,1) = a;
	}


	return ortho;
}

IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  const Eigen::MatrixBase<DerivedZ> & Z,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  N.resize(F.rows(),3);
  // loop over faces
  int Frows = F.rows();
#pragma omp parallel for if (Frows>10000)
  for(int i = 0; i < Frows;i++)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v1 = V.row(F(i,1)) - V.row(F(i,0));
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v2 = V.row(F(i,2)) - V.row(F(i,0));
    N.row(i) = v1.cross(v2);//.normalized();
    typename DerivedV::Scalar r = N.row(i).norm();
    if(r == 0)
    {
      N.row(i) = Z;
    }else
    {
      N.row(i) /= r;
    }
  }
}

IGL_INLINE void igl::grad(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &V,
  const Eigen::Matrix<S, Eigen::Dynamic, Eigen::Dynamic> &F,
  const Eigen::Matrix<T, Eigen::Dynamic, 1>&X,
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &G )
{
  G = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(F.rows(),3);
  for (int i = 0; i<F.rows(); ++i)
  {
    // renaming indices of vertices of triangles for convenience
    int i1 = F(i,0);
    int i2 = F(i,1);
    int i3 = F(i,2);
    
    // #F x 3 matrices of triangle edge vectors, named after opposite vertices
    Eigen::Matrix<T, 1, 3> v32 = V.row(i3) - V.row(i2);
    Eigen::Matrix<T, 1, 3> v13 = V.row(i1) - V.row(i3);
    Eigen::Matrix<T, 1, 3> v21 = V.row(i2) - V.row(i1);
    
    // area of parallelogram is twice area of triangle
    // area of parallelogram is || v1 x v2 || 
    Eigen::Matrix<T, 1, 3> n  = v32.cross(v13); 
    
    // This does correct l2 norm of rows, so that it contains #F list of twice
    // triangle areas
    double dblA = std::sqrt(n.dot(n));
    
    // now normalize normals to get unit normals
    Eigen::Matrix<T, 1, 3> u = n / dblA;
    
    // rotate each vector 90 degrees around normal
    double norm21 = std::sqrt(v21.dot(v21));
    double norm13 = std::sqrt(v13.dot(v13));
    Eigen::Matrix<T, 1, 3> eperp21 = u.cross(v21);
    eperp21 = eperp21 / std::sqrt(eperp21.dot(eperp21));
    eperp21 *= norm21;
    Eigen::Matrix<T, 1, 3> eperp13 = u.cross(v13);
    eperp13 = eperp13 / std::sqrt(eperp13.dot(eperp13));
    eperp13 *= norm13;
    
    G.row(i) = ((X[i2] -X[i1]) *eperp13 + (X[i3] - X[i1]) *eperp21) / dblA;
  };
}

Eigen::Matrix<double, 3, 3> getRotationFromA2B(Eigen::Matrix<double, 3, 1> A, Eigen::Matrix<double, 3, 1> B) {
	Eigen::Matrix<double, 3, 1> v = A.cross(B);
	double s = v.norm();
	double c = A.dot(B);

	Eigen::Matrix<double, 3, 3> vx;
	vx <<   0, -v[2], v[1],
			v[2], 0, -v[0],
			-v[1], v[0], 0;
	return Eigen::Matrix<double, 3, 3>::Identity() + vx + ((1.0-c)/(s*s))*vx*vx;
}

///
/// @par Detailed description 
/// ... 
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
float
sasmath::
signed_angle(Eigen::Matrix<float,3,1> &coor1, Eigen::Matrix<float,3,1> &coor2, Eigen::Matrix<float,3,1> &coor3)
{

    /*
    This method calcultes the sign of the angle which is used in the calculation of a dihedral angle.
    As such, this method is not validated for other uses.  It will fail if the basis atoms for the
    dihedral (atom 2 and atom 3) overlap.
    */

    float sa = 180.0 ;
    float angle ; 
    float pi = acos(-1.0) ;

    float ada = coor1.dot(coor1) ;    
    float bdb = coor2.dot(coor2) ;    

    if(ada*bdb <= 0.0)
    {
        std::cout << "; " ;
        return sa ;
    }
    else
    {
        float adb = coor1.dot(coor2) ;    
        float argument = adb/sqrt(ada*bdb) ;
        angle = (180.0/pi) * acos(argument) ;
    }

    Eigen::Matrix<float,3,1> cp ; cp = coor1.cross(coor2) ;
    float dp = coor3.dot(cp) ;
    
    float sign = 0.0 ;
    if(dp < 0.0) 
    {
        sign = -1.0 ;
    }
    else if(dp > 0.0)
    {
        sign = 1.0 ;
    }

    sa = sign*angle ;

    return sa ;
}

 // returns the 90 deg rotation of a (around n) most similar to target b
 /// a and b should be in the same plane orthogonal to N
 static inline Eigen::Matrix<typename DerivedV::Scalar, 3, 1> K_PI_new(const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& a,
                                                                const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& b,
                                                                const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& n)
 {
   Eigen::Matrix<typename DerivedV::Scalar, 3, 1> c = (a.cross(n)).normalized();
   typename DerivedV::Scalar scorea = a.dot(b);
   typename DerivedV::Scalar scorec = c.dot(b);
   if (fabs(scorea)>=fabs(scorec))
     return a*Sign(scorea);
   else
     return c*Sign(scorec);
 }

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