C++根据两点计算角度差的两种算法

#include <iostream>
#include <cmath>

// 定义一个表示二维点的结构体，使用双精度浮点数
struct POINTF64 {
	double x;
	double y;
};
#define M_PI 3.141592653
// 计算两条线段之间的角度偏差
double AUX_CalcTwoLineAngle(POINTF64 ptA1, POINTF64 ptA2, POINTF64 ptB1, POINTF64 ptB2)
{
	// 计算线段 A 和线段 B 对应的向量
	double vecAx = ptA2.x - ptA1.x;
	double vecAy = ptA2.y - ptA1.y;
	double vecBx = ptB2.x - ptB1.x;
	double vecBy = ptB2.y - ptB1.y;

	// 计算向量的点积
	double dotProduct = vecAx * vecBx + vecAy * vecBy;

	// 计算向量的模长
	double magA = std::sqrt(vecAx * vecAx + vecAy * vecAy);
	double magB = std::sqrt(vecBx * vecBx + vecBy * vecBy);

	// 避免除零错误
	if (magA == 0 || magB == 0)
	{
		return 0.0;
	}

	// 计算两向量夹角的余弦值
	double cosAngle = dotProduct / (magA * magB);

	// 确保余弦值在 -1 到 1 的范围内，避免由于浮点数误差导致的问题
	if (cosAngle > 1.0) cosAngle = 1.0;
	if (cosAngle < -1.0) cosAngle = -1.0;

	// 计算夹角（弧度制）
	double angle = std::acos(cosAngle);

	// 将弧度转换为角度
	return angle * (180.0 / M_PI);
}

double AUX_CalcTwoLineAngle1(POINTF64 ptA1, POINTF64 ptA2, POINTF64 ptB1, POINTF64 ptB2)
{
	//-----------------------------------------------------
	// 计算旋转角度
	//-----------------------------------------------------
	double f64AngleA1A2 = 0;
	double f64AngleB1B2 = 0;
	double f64AngleErr = 0;
	f64AngleB1B2 = atan2((ptB2.y - ptB1.y), ptB2.x - ptB1.x);
	f64AngleA1A2 = atan2((ptA2.y - ptA1.y), ptA2.x - ptA1.x);
	f64AngleErr = f64AngleB1B2 - f64AngleA1A2;
	std::cout<<f64AngleErr<<std::endl;
	return f64AngleErr;
}
double AUX_CalcTwoLineAngle2(POINTF64 ptA1, POINTF64 ptA2, POINTF64 ptB1, POINTF64 ptB2)
{
	//-----------------------------------------------------
	// 计算旋转角度
	//-----------------------------------------------------
	double f64AngleA1A2 = 0;
	double f64AngleB1B2 = 0;
	double f64AngleErr = 0;
	f64AngleB1B2 = atan2((ptB2.y - ptB1.y), ptB2.x - ptB1.x);
	f64AngleA1A2 = atan2((ptA2.y - ptA1.y), ptA2.x - ptA1.x);
	f64AngleErr = f64AngleB1B2 - f64AngleA1A2;
	std::cout << f64AngleErr << std::endl;
	// 将角度偏差限制在 [-π, π] 范围内
	while (f64AngleErr > M_PI) {
		f64AngleErr -= 2 * M_PI;
	}
	while (f64AngleErr < -M_PI) {
		f64AngleErr += 2 * M_PI;
	}

	// 将弧度转换为度
	f64AngleErr = f64AngleErr * (180.0 / M_PI);

	std::cout << f64AngleErr << std::endl;
	return f64AngleErr;
}
int main() 
{
	POINTF64 ptA1 = {0.0, 0.0};
	POINTF64 ptA2 = {1.0, 0.0};

	POINTF64 ptB1 = {0.0, 0.0};
	POINTF64 ptB2 = {1, 1};

	double angle = AUX_CalcTwoLineAngle(ptA1, ptA2, ptB1, ptB2);
	std::cout << "两条线段之间的角度偏差是: " << angle << " 度" << std::endl;


	AUX_CalcTwoLineAngle2(ptA1, ptA2, ptB1, ptB2);
	return 0;
}