关于sqrt对负数开根号acos对大于1的数计算反三角函数值的问题

空间里面有一个实心球，两个点，问两点不经过实心球的路径的最小值.

题目意思很简单，我们可能会用到余弦定理，但是，余弦定理有误差，我们可能会得到一些奇怪的数字，浮点数的误差导致余弦定理计算出来了小于-1或者大于1的其他数字，当我们对这样的数字进行反三角函数运算时，会得到nan，当然sqrt 有时候也会碰到对负数开方，于是我们要重写这两个函数

#include<bits/stdc++.h>
using namespace std;
const double eps = 1e-12;
const double INF = 1e18;
const double PI = acos(-1.0);
double ACOS(double x)
{
    if(x>1)
        x=1.0;
    if(x<-1)
        x=-1.0;
    return acos(x);
}
double SQRT(double x)
{
    if(x<0)
        return 0;
    return sqrt(x);
}

struct Point3D
{
    double x, y, z;
    Point3D() {};
    Point3D(double xx, double yy,double zz)
    {
        x = xx;
        y = yy;
        z = zz;
    }
};
typedef struct Point2D
{
    Point2D() {};
    double x;
    double y;
    Point2D(double xx, double yy)
    {
        x = xx;
        y = yy;
    }
} Point;

struct Circle3D
{
    Point3D o;
    double r;
};
struct Circle
{
    Point2D o;
    double r;
};


struct Line
{
    Point s, e;
    Line() {};
    Line(Point a, Point b)
    {
        s = a;
        e = b;
    };
};


double dis(Point a, Point b)
{
    return SQRT(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
}


double dis(Point3D a, Point3D b)
{
    return SQRT(pow(a.x - b.x, 2) + pow(a.y - b.y, 2) + pow(a.z - b.z, 2));
}

double solve(Point3D s, Point3D t, Circle3D o)
{
    double so=dis(s,o.o);
    double to=dis(t,o.o);
    double st=dis(s,t);
    double sita_sot=ACOS((so*so+to*to-st*st)/2/so/to);
    double sita_sto=ACOS((st*st+to*to-so*so)/2/st/to);
    double sita_tso=ACOS((st*st+so*so-to*to)/2/st/so);


    double p=(so+to+st)/2;
    double d=SQRT(p*(p-so)*(p-to)*(p-st))*2/st;


    if(sita_sto<PI/2&&sita_tso<PI/2&&d<o.r)
    {
        double sita_soa1=ACOS(o.r/so);
        double sita_toa2=ACOS(o.r/to);

        double sita=sita_sot-sita_soa1-sita_toa2;

        return sita*o.r + so*sin(sita_soa1) + to*sin(sita_toa2);

    }
    else
    {
        return dis(s, t);
    }
}


double solve(Point s, Point t, Circle o)
{
    double so=dis(s,o.o);
    double to=dis(t,o.o);
    double st=dis(s,t);
    double sita_sot=ACOS((so*so+to*to-st*st)/2/so/to);
    double sita_sto=ACOS((st*st+to*to-so*so)/2/st/to);
    double sita_tso=ACOS((st*st+so*so-to*to)/2/st/so);


    double p=(so+to+st)/2;
    double d=SQRT(p*(p-so)*(p-to)*(p-st))*2/st;


    if(sita_sto<PI/2&&sita_tso<PI/2&&d<o.r)
    {
        double sita_soa1=ACOS(o.r/so);
        double sita_toa2=ACOS(o.r/to);

        double sita=sita_sot-sita_soa1-sita_toa2;

        return sita*o.r + so*sin(sita_soa1) + to*sin(sita_toa2);

    }
    else
    {
        return dis(s, t);
    }
}
double solve(double a,double b,double c,double r)
{
    Point s, t;
    Circle o;
    double arcsita = (pow(b, 2) + pow(c, 2) - pow(a, 2)) / 2 / b / c;
    double sita = ACOS(arcsita);

    t.x = c * cos(sita);
    t.y = c * sin(sita);

    o.o.x = 0;
    o.o.y = 0;
    o.r = r;

    s.x = b;
    s.y = 0;

    return solve(s, t, o);
}


int main()
{
    int T;
    scanf("%d", &T);
    while (T--)
    {
        Circle3D o;
        Point3D s, t;
        scanf("%lf%lf%lf%lf", &o.o.x, &o.o.y, &o.o.z,&o.r);
        scanf("%lf%lf%lf", &s.x, &s.y, &s.z);
        scanf("%lf%lf%lf", &t.x, &t.y, &t.z);
        double a = dis(s, t);
        double b = dis(s, o.o);
        double c = dis(t, o.o);

        if(fabs(a)<eps)
        {
            printf("0.0000000000\n");
        }
        else
        {
            printf("%.8lf\n", solve(a,b,c,o.r));
        }
    }
}