使用KKT条件处理机械臂碰撞问题

问题：求得的最小距离与模型中两圆柱个体的半径之和进行比较，判断机器人手臂在运动过程中是否存在潜在的碰撞？

/**
1 1 0
1 1 10
5 5 0
5 5 10
1 1

1 1 0
1 1 10
2 2 0
2 5 10
1 1

1 1 0
1 1 10
2 2 0
5 5 10
1 1

1 1 0
1 1 10
2 0 5
2 3 5
1 1
**/

#include<iostream>
#include<algorithm>
#include<cmath>
using namespace std;

int main()
{

    //cout<<"*******************************机械臂碰撞问题***********************************"
    while(1){
        cout<<"*********************************************************"<<endl;
        double A[3],B[3],C[3],D[3];
        double r_ab,r_cd;
        cout<<"请输入AB杆的两个端点坐标(中间用空格隔开):"<<endl;
        cin>>A[0]>>A[1]>>A[2]>>B[0]>>B[1]>>B[2];
        cout<<"请输入CD杆的两个端点坐标(中间用空格隔开):"<<endl;
        cin>>C[0]>>C[1]>>C[2]>>D[0]>>D[1]>>D[2];
        cout<<"请输入AB杆和CD杆的半径(中间用空格隔开):"<<endl;
        cin>>r_ab>>r_cd;
        /*
        double x1,y1,z1;
        double eta1_x = (x1-A[0])/(B[0]-A[0]);
        double eta1_y = (y1-A[1])/(B[1]-A[1]);
        double eta1_z = (z1-A[2])/(B[2]-A[2]);
        double eta2_x = (x2-C[0])/(D[0]-C[0]);
        double eta2_y = (y2-C[1])/(D[1]-C[1]);
        double eta2_z = (z2-C[2])/(D[2]-C[2]);
        */
        // eta1平方的系数
        double b = (B[0]-A[0])*(B[0]-A[0]) + (B[1]-A[1])*(B[1]-A[1]) + (B[2]-A[2])*(B[2]-A[2]);
        // eta2平方的系数
        double c = (D[0]-C[0])*(D[0]-C[0]) + (D[1]-C[1])*(D[1]-C[1]) + (D[2]-C[2])*(D[2]-C[2]);
        // eta1的系数
        double d = 2*(B[0]-A[0])*(C[0]-A[0]) + 2*(B[1]-A[1])*(C[1]-A[1]) + 2*(B[2]-A[2])*(C[2]-A[2]);
        // eta2的系数
        double e = 2*(D[0]-C[0])*(C[0]-A[0]) + 2*(D[1]-C[1])*(C[1]-A[1]) + 2*(D[2]-C[2])*(C[2]-A[2]);
        // eta1*eta2的系数
        double f = 2*(B[0]-A[0])*(D[0]-C[0]) + 2*(B[1]-A[1])*(D[1]-C[1]) + 2*(B[2]-A[2])*(D[2]-C[2]);
        // 常数
        double a = (C[0]-A[0])*(C[0]-A[0]) + (C[1]-A[1])*(C[1]-A[1]) + (C[2]-A[2])*(C[2]-A[2]);
        //cout<<a<<" "<<b<<" "<<c<<" "<<d<<" "<<e<<" "<<f<<endl;
        /*
        double l_ab = a + b*eta1_x*eta1_x + c*eta2_x*eta2_x - d*eta1_x + e*eta2_x - f*eta1_x*eta2_x;
        double lambda1,lambda2,lambda3,lambda4;
        double g1,g2,g3,g4;
        double d_eta1 = 2*b*eta1_x - d - f*eta2_x - lambda1 + lambda2;
        double d_eta2 = 2*c*eta2_x + e - f*eta1_x - lambda3 + lambda4;
        */

        double f_d[9]={0};
        double eta1,eta2;
        f_d[0] = a; //f_0_0
        f_d[1] = a+c+e; //f_0_1
        eta2 = -e/(2*c);
        f_d[2] = a + c*eta2*eta2 + e*eta2; //f_0_eta2
        eta1 = d/(2*b);
        f_d[3] = a + b*eta1*eta1 - d*eta1;//f_eta1_0
        eta1 = (d+f)/(2*b);
        f_d[4] = a + b*eta1*eta1 + c - d*eta1 + e - f*eta1;//f_eta1_1
        eta2 = (f*d-2*b*e)/(4*b*c-f*f);
        eta1 = (d+f*eta2)/(2*b);
        //cout<<eta1<<eta2<<endl;
        f_d[5] = a + b*eta1*eta1 + c*eta2*eta2 - d*eta1 + e*eta2 - f*eta1*eta2;//f_eta1_eta2
        //cout<<f_d[5]<<endl;
        f_d[6] = a + b - d;//f_1_0
        f_d[7] = a + b + c -d + e - f;//f_1_1
        eta2 = -(e-f)/(2*c);
        f_d[8] = a + b + c*eta2*eta2 - d + e*eta2 - f*eta2;//f_1_eta2
/*
        for(int i=0; i<9; i++){
            cout<<f_d[i]<<" ";
        }
        */
        //cout<<endl;
        sort(f_d,f_d+9);
        /*
        for(int i=0; i<9; i++){
            cout<<f_d[i]<<" ";
        }
        */
        double min_len;
        for(int i=0; i<9; i++){
            min_len = f_d[i];
            if(min_len>0) break;
        }
        double min_d = sqrt(min_len);
        if(min_d <= r_ab+r_cd){
            cout<<"两机械臂最短距离："<<min_d<<endl;
            cout<<"AB杆与CD杆的半径："<<r_ab<<" "<<r_cd<<endl;
            cout<<"结论：两机械臂碰撞"<<endl;
        }
        else{
            cout<<"两机械臂最短距离："<<min_d<<endl;
            cout<<"AB杆与CD杆的半径："<<r_ab<<" "<<r_cd<<endl;
            cout<<"两机械臂不碰撞"<<endl;
        }
        int flag = 1;
        cout<<"继续请输入1，退出输入0："<<endl;
        cin>>flag;
        if(flag==0) break;
    }
    return 0;
}

结论：运行图如下