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

2024年4月16日 125次阅读 来源: 小强的机器人工坊

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

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

向量 A → ∗ B → \overrightarrow{A}*\overrightarrow{B} A ∗B 是一个数，它的大小是：
(1) A → ∗ B → = ∣ A → ∣ ∗ ∣ B → ∣ ∗ c o s ( θ ) \overrightarrow{A}*\overrightarrow{B}=|\overrightarrow{A}|*|\overrightarrow{B}|*cos(\theta) \tag{1} A ∗B =∣A ∣∗∣B ∣∗cos(θ)(1)
现在，将 C → = A → × B → \overrightarrow{C}=\overrightarrow{A} \times \overrightarrow{B} C =A ×B 和 C → \overrightarrow{C} C 代入上述公式(1)，则有如下的表达式：
(2) C → ∗ C → = ( A → × B → ) ∗ C → = ∣ A → × B → ∣ ∗ ∣ C → ∣ ∗ c o s ( θ ) \overrightarrow{C} * \overrightarrow{C}=(\overrightarrow{A} \times \overrightarrow{B}) * \overrightarrow{C}=|\overrightarrow{A} \times \overrightarrow{B}| * |\overrightarrow{C}| * cos(\theta) \tag{2} C ∗C =(A ×B )∗C =∣A ×B ∣∗∣C ∣∗cos(θ)(2)
因此，有：
(3) c o s ( θ ) = ( A → × B → ) ∗ C → ∣ A → × B → ∣ ∗ ∣ C → ∣ = c o s ( 0 ) = 1 cos(\theta)=\frac{(\overrightarrow{A} \times \overrightarrow{B}) * \overrightarrow{C}}{|\overrightarrow{A} \times \overrightarrow{B}| * |\overrightarrow{C}|} = cos(0) = 1 \tag{3} cos(θ)=∣A ×B ∣∗∣C ∣(A ×B )∗C ​=cos(0)=1(3)
注意：在公式(3)中，之所以为 c o s ( θ ) = c o s ( 0 ) cos(\theta)=cos(0) cos(θ)=cos(0)，因为 A → × B → = C → \overrightarrow{A} \times \overrightarrow{B}=\overrightarrow{C} A ×B =C 。
由公式(3)结合公式(0)，有：
(4) ∣ C → ∣ = ∣ A → × B → ∣ = ( A → × B → ) ∗ C → ∣ C → ∣ = ( A → × B → ) ∗ n → = ∣ A → ∣ ∗ ∣ B → ∣ ∗ s i n ( θ ) |\overrightarrow{C}|=|\overrightarrow{A} \times \overrightarrow{B}|=\frac{(\overrightarrow{A} \times \overrightarrow{B}) * \overrightarrow{C}}{|\overrightarrow{C}|}=(\overrightarrow{A} \times \overrightarrow{B})*\overrightarrow{n}=|\overrightarrow{A}|*|\overrightarrow{B}|*sin(\theta) \tag{4} ∣C ∣=∣A ×B ∣=∣C ∣(A ×B )∗C ​=(A ×B )∗n =∣A ∣∗∣B ∣∗sin(θ)(4)
注意： n → \overrightarrow{n} n 是 C → \overrightarrow{C} C 的单位向量，即 A → × B → \overrightarrow{A} \times \overrightarrow{B} A ×B 的单位法向量。
接下来，我们再来推导如何求解 θ \theta θ 的公式：
综合公式(4)和公式(2)，可得：
(5) θ = a t a n 2 ( s i n ( θ ) , c o s ( θ ) ) = a t a n 2 ( ( A → × B → ) ∗ n → , A → ∗ B → ) = a t a n 2 ( ( A → × B → ) . n o r m ( ) , A → ∗ B → ) \theta = atan2(sin(\theta), cos(\theta))=atan2((\overrightarrow{A} \times \overrightarrow{B})*\overrightarrow{n}, \overrightarrow{A}*\overrightarrow{B}) =atan2((\overrightarrow{A} \times \overrightarrow{B}).norm(), \overrightarrow{A}*\overrightarrow{B}) \tag{5} θ=atan2(sin(θ),cos(θ))=atan2((A ×B )∗n ,A ∗B )=atan2((A ×B ).norm(),A ∗B )(5)
但是，存在一个问题，即公式(5)返回的数值是一个范围在 0 到 π \pi π 之间的数值，而不是我们想要的 0 到 2 π \pi π的数值，即存在旋转的方向问题，当旋转的角度超过 18 0 ∘ 180^{\circ} 180∘ 时，就会就会计算出一个反向旋转的角度小于 18 0 ∘ 180^{\circ} 180∘的角度值。为此，我们需要判断旋转的方向，即在向量叉乘的过程中得到的法向量的 z z z 值是正的还是负的。通过这个来判断法向量的方向问题，通常当法向量的 z z z 值如果是负的，那么需要通过 2 π − θ 2\pi-\theta 2π−θ 来得到真实的旋转角度，此时得到的旋转角度是一个大于 π \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
    原文作者：小强的机器人工坊
    原文地址: https://blog.csdn.net/l1216766050/article/details/99713052
    本文转自网络文章，转载此文章仅为分享知识，如有侵权，请联系博主进行删除。
点赞