求空间两向量夹角

#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~pi
    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;
}