计算圆弧夹角算法
/** 计算三点圆弧各夹角
* @p1 p2 p3 三个点,圆弧方向按照经过点p1p2p3的顺序
* @return 返回三个值, 分别表示两两之间的夹角
*/
Vector3d getArcAngle(Vector3d p1, Vector3d p2, Vector3d p3, Vector3d pc)
{
Vector3d p1c = p1 - pc;
Vector3d p2c = p2 - pc;
Vector3d p3c = p3 - pc;
double cosTheta = p1c.dot(p3c) / (Abs(p1c) * Abs(p3c));
if (cosTheta >= 1) {
cosTheta = 1;
} else if (cosTheta <= -1) {
cosTheta = -1;
}
double theta = acos(cosTheta);
double cosTheta1 = p1c.dot(p2c) / (Abs(p1c) * Abs(p2c));
if (cosTheta1 >= 1) {
cosTheta1 = 1;
} else if (cosTheta1 <= -1) {
cosTheta1 = -1;
}
double theta1 = acos(cosTheta1);
if (((p2 - p1).cross(p3 - p2).norma