代码
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);
}