三点确定圆弧_根据三点获取圆弧夹角及圆弧插补算法

admin1

计算圆弧夹角算法

/** 计算三点圆弧各夹角

* @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).normalized()).dot(

(p1c.cross(p3 - p1)).normalized()) < 0) {

theta = theta - 2 * pi;

//再判断第一个点的夹角是否大于180

if ((p1c.cross(p2c)).dot(p1c.cross(p3c)) > 0) {

theta1 -= 2 * pi;

}

}

double theta2 = theta - theta1;

Vector3d v(theta, theta1, theta2);

return v;

}

圆弧位置插补算法

/*

* 圆弧轨迹位置插补

* @p1 圆弧起点

* @p2 圆弧中点

* @p3 圆弧终点

* @t 中间点系数

* @return 返回中间的位置坐标

*/

Vector3d circlePositionInterpolation(Vector3d p1, Vector3d p2, Vector3d p3, double t)

{

double pNorm = Abs((p1 - p2).cross(p2 - p3));

double R = (Abs(p1 - p2)*Abs(p2 - p3)*Abs(p3 - p1)) / (2.0 * pNorm);

double alfa = ((Abs(p2 - p3)*Abs(p2 - p3)) * ((p1 - p2).dot(p1 - p3))) / (2 * pNorm*pNorm);

double beta = ((Abs(p1 - p3)*Abs(p1 - p3)) * ((p2 - p1).dot(p2 - p3))) / (2 * pNorm*pNorm);

double gama = ((Abs(p1 - p2)*Abs(p1 - p2)) * ((p3 - p1).dot(p3 - p2))) / (2 * pNorm*pNorm);

Vector3d pc = alfa * p1 + beta * p2 + gama * p3;

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).normalized()).dot((p1c.cross(p3 - p1)).normalized()) < 0) {

theta = theta - 2*pi;

//再判断第一个点的夹角是否大于180

if ((p1c.cross(p2c)).dot(p1c.cross(p3c)) > 0) {

theta1 -= 2*pi;

}

}

Vector3d temp;

if (theta == pi) {

temp = p1c.cross(p2c).cross(p1c);

} else {

temp = p1c.cross(p3c).cross(p1c);

}

Vector3d pt = pc + p1c*cos(t * theta) + temp.normalized() * R * sin(t * theta);

return pt;

}