//计算向量角度 void calculateVectorAngle(double dir1[3], double dir2[3], double* sinVal, double* cosVal, double* tanVal) { // 计算点积 double dotProduct = 0.0; for (int i = 0; i < 3; ++i) { dotProduct += dir1[i] * dir2[i]; } // 计算向量模长 double mag1 = 0.0; double mag2 = 0.0; for (int i = 0; i < 3; ++i) { mag1 += dir1[i] * dir1[i]; mag2 += dir2[i] * dir2[i]; } mag1 = std::sqrt(mag1); mag2 = std::sqrt(mag2); // 计算余弦值 double cosValue = dotProduct / (mag1 * mag2); // 处理浮点精度问题，确保cos值在[-1, 1]范围内 if (cosValue > 1.0) cosValue = 1.0; if (cosValue < -1.0) cosValue = -1.0; // 计算正弦值 double sinValue = std::sqrt(1.0 - cosValue * cosValue); // 计算正切值 double tanValue; if (std::abs(cosValue) < 1e-10) // 避免除以零 { tanValue = (cosValue >= 0) ? 1e10 : -1e10; // 使用一个大数表示无穷大 } else { tanValue = sinValue / cosValue; } // 确定角度的正确象限 // 计算叉积的z分量（用于确定角度方向） double crossZ = dir1[0] * dir2[1] - dir1[1] * dir2[0]; // 如果叉积的z分量为负，说明角度大于180度，正弦值应为负 if (crossZ < 0) { sinValue = -sinValue; // 正切值也需要相应调整 if (std::abs(cosValue) >= 1e-10) { tanValue = sinValue / cosValue; } } // 通过指针将结果赋值给输出参数 *sinVal = sinValue; *cosVal = cosValue; *tanVal = tanValue; }