求解三维空间中两向量之间的夹角

问题描述：已知三维空间中的三个点$P_1$，$P_2$和$P_3$，求向量$\overrightarrow{P_1{P}_2}$和$\overrightarrow{P_1{P}_3}$之间的夹角，要求必须能够计算出[0, 2$\pi$)之间的数值，而不仅仅是只能求出锐角，并用C++或Python或MATLAB语言进行算法实现。

问题分析：为了求解出这个问题，首先需要引入三维向量的点乘和叉乘的知识。最后，根据点乘和叉乘推导出两个空间向量之间夹角的求解公式。
空间三维向量的叉乘：
$\overrightarrow{C}=\overrightarrow{A}\times \overrightarrow{B}$，$\overrightarrow{C}$也是一个空间三维向量，方向通过右手定则来判断，即一个垂直于向量 $\overrightarrow{A}$和向量 $\overrightarrow{B}$所在平面的向量。
$$|\overrightarrow{C}|=|\overrightarrow{A}\times \overrightarrow{B}|=|\overrightarrow{A}|\ast |\overrightarrow{B}|\ast sin(\theta )\text{\hspace{1em}(0)}$$

向量$\overrightarrow{A}\ast \overrightarrow{B}$是一个数，它的大小是：
$$\overrightarrow{A}\ast \overrightarrow{B}=|\overrightarrow{A}|\ast |\overrightarrow{B}|\ast cos(\theta )\text{\hspace{1em}(1)}$$
现在，将$\overrightarrow{C}=\overrightarrow{A}\times \overrightarrow{B}$ 和 $\overrightarrow{C}$ 代入上述公式(1)，则有如下的表达式：
$$\overrightarrow{C}\ast \overrightarrow{C}=(\overrightarrow{A}\times \overrightarrow{B})\ast \overrightarrow{C}=|\overrightarrow{A}\times \overrightarrow{B}|\ast |\overrightarrow{C}|\ast cos(\theta )\text{\hspace{1em}(2)}$$
因此，有：
$$cos(\theta )=\frac{(\overrightarrow{A}\times \overrightarrow{B})\ast \overrightarrow{C}}{|\overrightarrow{A}\times \overrightarrow{B}|\ast |\overrightarrow{C}|}=cos(0)=1\text{\hspace{1em}(3)}$$
注意：在公式(3)中，之所以为$cos(\theta )=cos(0)$，因为$\overrightarrow{A}\times \overrightarrow{B}=\overrightarrow{C}$。
由公式(3)结合公式(0)，有：
$$|\overrightarrow{C}|=|\overrightarrow{A}\times \overrightarrow{B}|=\frac{(\overrightarrow{A}\times \overrightarrow{B})\ast \overrightarrow{C}}{|\overrightarrow{C}|}=(\overrightarrow{A}\times \overrightarrow{B})\ast \overrightarrow{n}=|\overrightarrow{A}|\ast |\overrightarrow{B}|\ast sin(\theta )\text{\hspace{1em}(4)}$$
注意：$\overrightarrow{n}$是$\overrightarrow{C}$的单位向量，即$\overrightarrow{A}\times \overrightarrow{B}$的单位法向量。
接下来，我们再来推导如何求解 $\theta$ 的公式：
综合公式(4)和公式(2)，可得：
$$\theta =atan2(sin(\theta ),cos(\theta ))=atan2((\overrightarrow{A}\times \overrightarrow{B})\ast \overrightarrow{n},\overrightarrow{A}\ast \overrightarrow{B})=atan2((\overrightarrow{A}\times \overrightarrow{B}).norm(),\overrightarrow{A}\ast \overrightarrow{B})\text{\hspace{1em}(5)}$$
但是，存在一个问题，即公式(5)返回的数值是一个范围在 0 到 $\pi$ 之间的数值，而不是我们想要的 0 到 2$\pi$的数值，即存在旋转的方向问题，当旋转的角度超过 $180^{\circ }$ 时，就会就会计算出一个反向旋转的角度小于 $180^{\circ }$的角度值。为此，我们需要判断旋转的方向，即在向量叉乘的过程中得到的法向量的 $z$ 值是正的还是负的。通过这个来判断法向量的方向问题，通常当法向量的 $z$ 值如果是负的，那么需要通过 $2\pi -\theta$ 来得到真实的旋转角度，此时得到的旋转角度是一个大于 $\pi$ 的数值。

---

公式推导完毕之后，我们就可以通过C++编程来实现该求解算法。我们会使用到Eigen线性代数库，这是一个在数值计算和机器人以及计算机视觉、图像处理等领域非常重要的一个基础库，很多重量级的开源算法库都是在Eigen基础上构建的。Eigen也非常好用，甚至直接从官网下载源代码解压就可以用，因为这个库只有头文件，不需要编译源码，解压即安装。
CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
set(CMAKE_CXX_STANDARD 14)
project(Demo)
find_package(Eigen3 REQUIRED)
add_executable(${PROJECT_NAME} "main.cpp")

main.cpp

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

测试结果：

The Degree Angle is: 270
The Degree Angle is: 90
The Degree Angle is: 45
The Degree Angle is: 315