c++计算空间向量的夹角

代码

void UtilsClass::normalization(double unnorm_array[3])
{
	// 归一化
	double max_val = std::max(abs(unnorm_array[0]), abs(unnorm_array[1]));
	max_val = std::max(max_val, abs(unnorm_array[2]));
	// std::cout << "max_val = " << max_val << std::endl;
	unnorm_array[0] = unnorm_array[0] / max_val;
	unnorm_array[1] = unnorm_array[1] / max_val;
	unnorm_array[2] = unnorm_array[2] / max_val;
	//std::cout << ba[0] << ", " << ba[1] << ", " << ba[2] << std::endl;
}

void UtilsClass::CalculateAngle(
	const Eigen::Vector4i& coordinate01, 
	const Eigen::Vector4i& coordinate02, 
	const Eigen::Vector4i& coordinate03, 
	double& angle_pi)
{
	// 02->01向量
	double ba[3] = { coordinate01(0) - coordinate02(0),
					coordinate01(1) - coordinate02(1),
					coordinate01(2) - coordinate02(2) };
	this->normalization(ba);
	// std::cout << ba[0] << ", " << ba[1] << ", " << ba[2] << std::endl;
	
	// 02->03向量
	double bc[3] = { coordinate03(0) - coordinate02(0),
					coordinate03(1) - coordinate02(1),
					coordinate03(2) - coordinate02(2) };
	this->normalization(bc);
	// std::cout << bc[0] << ", " << bc[1] << ", " << bc[2] << std::endl;

	// 空间向量夹角公式
	double amb = ba[0] * bc[0] + ba[1] * bc[1] + ba[2] * bc[2];
	double sqa = sqrt(pow(ba[0], 2) + pow(ba[1], 2) + pow(ba[2], 2));
	double sqb = sqrt(pow(bc[0], 2) + pow(bc[1], 2) + pow(bc[2], 2));
	double cos_theta = amb / (sqa * sqb);
	
	angle_pi = acos(cos_theta);
}