求空间两向量夹角

空间三维向量的叉乘：

 

 

向量的点乘：

 

 因此结合（0）和（1）可以的得到： 

　　　　　　　　　　　　　　　　　　θ = atan2(sin(θ),cos(θ)) = atan2((A×B)∗n,A∗B) = atan2((A×B).norm(),A∗B)      { 0<θ<180 }

 

#include <iostream>
#include <Eigen/Dense>

typedef Eigen::Vector3d Point;

double getDegAngle(Point p1, Point p2, Point p3) {
    Eigen::Vector3d v1 = p2 - p1;
    Eigen::Vector3d v2 = p3 - p1;
    //one method, radian_angle belong to 0~pi
    //double radian_angle = atan2(v1.cross(v2).transpose() * (v1.cross(v2) / v1.cross(v2).norm()), v1.transpose() * v2);
    //another method, radian_angle belong to 0~2pi
    double radian_angle = atan2(v1.cross(v2).norm(), v1.transpose() * v2);
    if (v1.cross(v2).z() < 0) {
        radian_angle = 2 * M_PI - radian_angle;
    }
    return radian_angle * 180 / M_PI;
}

int main() {
    //Point p1(0, 0, 0), p2(1, 0, 0), p3(0, -1, 0);
    //Point p1(0, 0, 0), p2(1, 0, 0), p3(0, 1, 0);
    //Point p1(0, 0, 0), p2(1, 0, 0), p3(0.5, 0.5, 0);
    Point p1(0, 0, 0), p2(1, 0, 0), p3(0.5, -0.5, 0);
    std::cout << "The Degree Angle is: " << getDegAngle(p1, p2, p3) << "\n";
    return 0;
}

 

        //求两个矢量的夹角
        inline double getAngleTwoVectors(const Eigen::Vector3d & v1, const Eigen::Vector3d & v2) {
            double radian_angle = atan2(v1.cross(v2).norm(), v1.transpose() * v2);
            return radian_angle;   //[0,PI]
        }
        inline double getAngleTwoVectorsPlane(const Eigen::Vector3d & v1, const Eigen::Vector3d & v2) {
            double dot2 = v1[0]*v2[0]+v1[1]*v2[1];
            double dis1 = sqrt(v1[0] * v1[0] + v1[1] * v1[1]);
            double dis2 = sqrt(v2[0] * v2[0] + v2[1] * v2[1]);
            double radian_angle = acos(dot2/(dis1*dis2));
            return radian_angle;   //[0,PI]
        }