三维向量求夹角(0-360)

规定Z轴的正方向是（0，0，1）

 

从a->b做法向量；

a和b的法向量与Z轴正向的积，积大于0，同向，夹角为正；

                                                     积小于0，反向，夹角为负，这里要求0-360，所以夹角为2*pi-theta；

double  Cross( Eigen::Vector3d a, Eigen::Vector3d b){
    
   
    double theta = atan2(a.cross(b).norm(), a.transpose() * b);
    //规定正方向(0,0,1)
    Eigen::Vector3d direction(0,0,1);
    double z = a.cross(b).transpose() * direction;
    return z >= 0 ? theta : 2*PI- theta;
    
}