三角测量 - 如何找到3个球体的交点(iBeacons)

时间:2014-07-11 11:43:18

标签: ios

在给定3个球面方程的情况下,计算3个球体交点的iOS Objective C方法是什么:

1) (x -x1)^2 + (y - y1)^2 + (z - z1)^2 = (r1)^2
2) (x -x2)^2 + (y - y2)^2 + (z - z2)^2 = (r2)^2
3) (x -x3)^2 + (y - y3)^2 + (z - z3)^2 = (r3)^2

//我想要的方法

-(NSArray*) findIntersectionOfThreeSpheresWithFirstSphereRadius: (float) r1 xCoordinate: (float) x1 yCoordinate:(float) y1 zCoordinate:(float) z1 andWithSecondSphereRadius: (float) r2 xCoordinate: (float) x2 yCoordinate:(float) y2 zCoordinate:(float) z2 andWithThirdSphereRadius: (float) r3 xCoordinate: (float) x3 yCoordinate:(float) y3 zCoordinate:(float) z3

1 个答案:

答案 0 :(得分:1)

我无法在任何地方找到这个问题的答案 - 所以我自己解决了。如果您觉得这个答案有帮助,请+1回答!

-(NSArray*) findIntersectionOfThreeSpheresWithFirstSphereRadius: (float) r1 xCoordinate: (float) x1 yCoordinate:(float) y1 zCoordinate:(float) z1 andWithSecondSphereRadius: (float) r2 xCoordinate: (float) x2 yCoordinate:(float) y2 zCoordinate:(float) z2 andWithThirdSphereRadius: (float) r3 xCoordinate: (float) x3 yCoordinate:(float) y3 zCoordinate:(float) z3
{
    double x1Answer;
    double y1Answer;
    double z1Answer;
    double x2Answer;
    double y2Answer;
    double z2Answer;

    double k = 0.0;
    double k2 = 0.0;
    double l = 0.0;
    double l2 = 0.0;
    double m = 0.0;
    double m2 = 0.0;
    double n = 0.0;
    double n2 = 0.0;
    double p = 0.0;
    double p2 = 0.0;
    double q = 0.0;
    double q2 = 0.0;
    double u = 0.0;
    double u2 = 0.0;
    double w = 0.0;
    double w2 = 0.0;
    double j = 0.0;
    double j2 = 0.0;
    double h = 0.0;
    double h2 = 0.0;
    double v = 0.0;
    double v2 = 0.0;
    double c = 0.0;
    double c2 = 0.0;
    double d = 0.0;
    double d2 = 0.0;
    double f = 0.0;
    double f2 = 0.0;
    double t = 0.0;
    double t2 = 0.0;
    double s = 0.0;
    double s2 = 0.0;
    double A = 0.0;
    double B = 0.0;
    double C = 0.0;
    double i1 = 0.0;
    double b1 = 0.0;
    double i2 = 0.0;
    double b2 = 0.0;
    double i3 = 0.0;
    double b3 = 0.0;
    double i4 = 0.0;
    double b4 = 0.0;
    BOOL a1SpecialCase = NO;
    BOOL a2SpecialCase = NO;
    BOOL a3SpecialCase = NO;
    BOOL a4SpecialCase = NO;
    BOOL b1SpecialCase = NO;
    BOOL b2SpecialCase = NO;
    BOOL oneAnswer = NO;
    BOOL twoAnswers = NO;


    if (z1-z2 != 0)
    {
        k2 = 2* (x2-x1)/(2*z1-2*z2);
        l2 = 2* (y2-y1)/(2*z1-2*z2);
        m2 = (x1*x1 - x2*x2 + y1*y1 - y2*y2 + z1*z1 - z2*z2 - r1*r1 + r2*r2)/(2*z1-2*z2);
    }
    else
    {
        a1SpecialCase = YES;
        k = 2* (x2-x1);
        l = 2* (y2-y1);
        m = x1*x1 - x2*x2 + y1*y1 - y2*y2 + z1*z1 - z2*z2 - r1*r1 + r2*r2;
    }

    if (y1-y2 != 0)
    {
        n2 = 2* (x2-x1)/(2*y1-2*y2);
        p2 = 2* (z2-z1)/(2*y1-2*y2);
        q2 = (x1*x1 - x2*x2 + y1*y1 - y2*y2 + z1*z1 - z2*z2 - r1*r1 + r2*r2)/(2*y1-2*y2);
    }
    else
    {
        a2SpecialCase = YES;
        n = 2* (x2-x1);
        p = 2* (z2-z1);
        q = x1*x1 - x2*x2 + y1*y1 - y2*y2 + z1*z1 - z2*z2 - r1*r1 + r2*r2;
    }

    if (z1-z3 != 0)
    {
        u2 = 2* (x3-x1)/(2*z1-2*z3);
        w2 = 2* (y3-y1)/(2*z1-2*z3);
        j2 = (x1*x1 - x3*x3 + y1*y1 - y3*y3 + z1*z1 - z3*z3 - r1*r1 + r3*r3)/(2*z1-2*z3);
    }
    else
    {
        a3SpecialCase = YES;
        u = 2* (x3-x1);
        w = 2* (y3-y1);
        j = x1*x1 - x3*x3 + y1*y1 - y3*y3 + z1*z1 - z3*z3 - r1*r1 + r3*r3;
    }

    if (y1-y3 != 0)
    {
        h2 = 2* (x3-x1)/(2*y1-2*y3);
        v2 = 2* (z3-z1)/(2*y1-2*y3);
        c2 = (x1*x1 - x3*x3 + y1*y1 - y3*y3 + z1*z1 - z3*z3 - r1*r1 + r3*r3)/(2*y1-2*y3);
    }
    else
    {
        a4SpecialCase = YES;
        h = 2* (x3-x1);
        v = 2* (z3-z1);
        c = (x1*x1 - x3*x3 + y1*y1 - y3*y3 + z1*z1 - z3*z3 - r1*r1 + r3*r3);
    }

    if ( (!a1SpecialCase&& !a2SpecialCase && !a3SpecialCase && !a4SpecialCase) || ((a1SpecialCase&&a3SpecialCase) && (a2SpecialCase&&a4SpecialCase)) ||((a1SpecialCase&&a3SpecialCase) && (!a2SpecialCase&&!a4SpecialCase)) || ((!a1SpecialCase&&!a3SpecialCase) && (a2SpecialCase&&a4SpecialCase)) )
    {
        if (l2-w2 !=0)
        {
            d2 = (u2-k2)/(l2-w2);
            f2 = (j2-m2)/(l2-w2);
        }
        else
        {
            b1SpecialCase = YES;
            d =u2-k2;
            f =j2-m2;
        }

        if (p2-v2 !=0)
        {
            t2 = (h2-n2)/(p2-v2);
            s2 = (c2-q2)/(p2-v2);
        }
        else
        {
            b2SpecialCase = YES;
            t = h2-n2;
            s = c2-q2;
        }

        if (!b1SpecialCase&&!b2SpecialCase)
        {
            A = 1+ d2*d2 + t2*t2;
            B = d2*f2- 2*x1 -d2*y1 +f2*d2 - y1*d2 +s2*t2 -z1*t2 +s2*t2 - z1*t2;
            C = x1*x1+f2*f2 - y1*f2 -y1*f2 + y1*y1 + s2*s2 - s2*z1 - z1*s2+ z1*z1 - r1*r1;

            twoAnswers = YES;
            x1Answer = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
            x2Answer = (-B - sqrt(B * B - 4 * A * C)) / (2 * A);

            y1Answer = d2*x1Answer + f2;
            y2Answer = d2*x2Answer + f2;

            z1Answer = t2*x1Answer +s2;
            z2Answer = t2*x2Answer +s2;

        }
        else if (b1SpecialCase&&b2SpecialCase)
        {
            //Figure Out Later if this ever gets called
        }
        else if (b1SpecialCase)
        {
            oneAnswer = YES;
            x1Answer = -f/d;
            z1Answer = -f*t*t/d + s2;
            if (a1SpecialCase)
            {
                y1Answer = -m+k*f/d;
            }
            else
            {
                y1Answer = (z1Answer-m2-k2*x1Answer)/l2;
            }
        }
        else if (b2SpecialCase)
        {
            oneAnswer = YES;
            x1Answer = -s/t;
            y1Answer = d2*x1Answer + f2;
            if (a2SpecialCase)
            {
                z1Answer = (-1-n*x1Answer)/p;
            }
            else
            {
                z1Answer = (y1Answer-q2-n2*x1Answer)/p2;
            }
        }

    }
    else
    {
        if ( (!a1SpecialCase&&!a3SpecialCase))
        {
            if (a2SpecialCase)
            {

                if (l2-w2 !=0)
                {
                    d2 = (u2-k2)/(l2-w2);
                    f2 = (j2-m2)/(l2-w2);
                    i1 = -q/p;
                    b1 = -n/p;

                    A = 1+ b1*b1 + d2*d2;
                    B = -2*x1 + f2*d2- d2*y1 +f2*d2 - y1*d2 + b1*i1 - b1*z1 +b1*i1 -z1*b1;
                    C = x1*x1 + f2*f2 - f2*y1 - y1*f2 + y1*y1 +i1*i1 - i1*z1- z1*i1 +z1*z1- r1*r1;

                    twoAnswers = YES;
                    x1Answer = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
                    x2Answer = (-B - sqrt(B * B - 4 * A * C)) / (2 * A);

                    y1Answer = d2*x1Answer + f2;
                    y2Answer = d2*x2Answer + f2;

                    z1Answer = m2+l2*y1Answer + x1Answer*k2;
                    z2Answer = m2+l2*y2Answer + x2Answer*k2;
                }
                else
                {
                    b1SpecialCase = YES;
                    oneAnswer = YES;
                    x1Answer = -f/d;
                    z1Answer = (-n*x1Answer - q)/p;
                    y1Answer = (z1Answer-m2-k2*x1Answer)/l2;
                }
            }
            else if (a4SpecialCase)
            {

                if (l2-w2 !=0)
                {
                    d2 = (u2-k2)/(l2-w2);
                    f2 = (j2-m2)/(l2-w2);
                    i2 = -h/v;
                    b2 = -c/p;

                    A = 1+ b2*b2 + d2*d2;
                    B = -2*x1 + f2*d2- d2*y1 +f2*d2 - y1*d2 + b2*i2 - b2*z1 +b2*i2 -z1*b2;
                    C = x1*x1 + f2*f2 - f2*y1 - y1*f2 + y1*y1 +i2*i2 - i2*z1- z1*i2 +z1*z1- r1*r1;

                    twoAnswers = YES;
                    x1Answer = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
                    x2Answer = (-B - sqrt(B * B - 4 * A * C)) / (2 * A);

                    y1Answer = d2*x1Answer + f2;
                    y2Answer = d2*x2Answer + f2;

                    z1Answer = m2+l2*y1Answer + x1Answer*k2;
                    z2Answer = m2+l2*y2Answer + x2Answer*k2;
                }
                else
                {
                    b1SpecialCase = YES;
                    oneAnswer = YES;
                    x1Answer = -f/d;
                    z1Answer = (-n*x1Answer - q)/p;
                    y1Answer = (z1Answer-m2-k2*x1Answer)/l2;
                }
            }


        }
        else if (!a2SpecialCase&&!a4SpecialCase)
        {
            if (a1SpecialCase)
            {
                if (p2-v2 !=0)
                {
                    t2 = (h2-n2)/(p2-v2);
                    s2 = (c2-q2)/(p2-v2);
                    b3 = -k/l;
                    i3 = m/l;
                    A = 1+ b3*b3 + t2*t2;
                    B = -2*x1 -b3*i3-b3*y1-b3*i3 -y1*b3 + s2*t2 - z1*t2+ s2*t2-z1*t2;
                    C = x1*x1 + i3*i3 - i3*y1 - y1*i3 + y1*y1 +s2*s2 - z1*s2- z1*s2 +z1*z1- r1*r1;

                    twoAnswers = YES;
                    x1Answer = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
                    x2Answer = (-B - sqrt(B * B - 4 * A * C)) / (2 * A);

                    y1Answer = (-m-k*x1Answer)/l;
                    y2Answer = (-m-k*x2Answer)/l;

                    z1Answer = u2*x1Answer+w2*y1Answer +j2;
                    z2Answer = u2*x2Answer+w2*y2Answer +j2;
                }
                else
                {
                    b2SpecialCase = YES;
                    oneAnswer = YES;
                    x1Answer = -s/t;
                    y1Answer = (-m-k*x1Answer)/l;
                    z1Answer = u2*x1Answer+w2*y1Answer+j2;
                }
            }
            else if (a3SpecialCase)
            {
                if (p2-v2 !=0)
                {
                    t2 = (h2-n2)/(p2-v2);
                    s2 = (c2-q2)/(p2-v2);
                    b4 = -u/w;
                    i4 = j/w;
                    A = 1+ b4*b4 + t2*t2;
                    B = -2*x1 -b4*i4-b4*y1-b4*i4 -y1*b3 + s2*t2 - z1*t2+ s2*t2-z1*t2;
                    C = x1*x1 + i4*i4 - i4*y1 - y1*i4 + y1*y1 +s2*s2 - z1*s2- z1*s2 +z1*z1- r1*r1;

                    twoAnswers = YES;
                    x1Answer = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
                    x2Answer = (-B - sqrt(B * B - 4 * A * C)) / (2 * A);

                    y1Answer = (-j-u*x1Answer)/w;
                    y2Answer = (-j-u*x2Answer)/w;

                    z1Answer = k2*x1Answer+l2*y1Answer;
                    z2Answer = k2*x2Answer+l2*y2Answer;
                }
                else
                {
                    b2SpecialCase = YES;
                    oneAnswer = YES;
                    x1Answer = -s/t;
                    y1Answer = (-j-u*x1Answer)/w;
                    z1Answer = k2*x1Answer+l2*y1Answer+m2;
                }
            }

        }
        else if  (a1SpecialCase&&a2SpecialCase&&a3SpecialCase)
        {
            if (u!=0)
            {
                oneAnswer = YES;
                y1Answer = (k*j/u -m)/(l-k*w/u);
                x1Answer = (-m-l*y1Answer)/k;
                z1Answer = y1Answer-c2-h2*x1Answer;
            }
            else
            {
                oneAnswer = YES;
                y1Answer =-j/w;
                x1Answer = (-m-l*y1Answer)/k;
                z1Answer = y1Answer-c2-h2*x1Answer;
            }

        }
        else if  (a2SpecialCase&&a3SpecialCase&&a4SpecialCase)
        {
            if (n!=0)
            {
                z1Answer = (h*q/n -c)/(v-h*p/n);
                x1Answer = (-c-v*z1Answer)/h;
                y1Answer = (z1Answer-m2-k2*x1Answer)/l2;
            }
            else
            {
                z1Answer = -1/p;
                x1Answer = (-c-v*z1Answer)/h;
                y1Answer = (z1Answer-m2-k2*x1Answer)/l2;
            }
        }
        else if  (a3SpecialCase&&a4SpecialCase&&a1SpecialCase)
        {
            if (u!=0)
            {
                oneAnswer = YES;
                y1Answer = (k*j/u -m)/(l-k*w/u);
                x1Answer = (-m-l*y1Answer)/k;
                z1Answer = (y1Answer-q2-n2*x1Answer)/p2;
            }
            else
            {
                oneAnswer = YES;
                y1Answer =-j/w;
                x1Answer = (-m-l*y1Answer)/k;
                z1Answer = (y1Answer-q2-n2*x1Answer)/p2;
            }
        }
        else if  (a4SpecialCase&&a1SpecialCase&&a2SpecialCase)
        {
            if (n!=0)
            {
                oneAnswer = YES;
                z1Answer = (h*q/n -c)/(v-h*p/n);
                x1Answer = (-c-v*z1Answer)/h;
                y1Answer = (z1Answer- u2*x1Answer-j2)/w;
            }
            else
            {
                oneAnswer = YES;
                z1Answer = -1/p;
                x1Answer = (-c-v*z1Answer)/h;
                y1Answer = (z1Answer- u2*x1Answer-j2)/w;
            }
        }

    }


    if (!isnan(x1Answer)&&!isnan(y1Answer)&&!isnan(z1Answer))
    {
        NSLog(@"Answer: One Answer :%i Coordinate1 (%f, %f, %f) and Coordinate 2 (%f, %f, %f) and r1: %f, r2: %f, r3: %f", oneAnswer, x1Answer, y1Answer, z1Answer, x2Answer, y2Answer, z2Answer, r1, r2, r3);
        NSArray *answer;
        if (twoAnswers)
        {
            answer = [NSArray arrayWithObjects:[NSNumber numberWithDouble: (x1Answer+x2Answer)/2.0],[NSNumber numberWithDouble: (y1Answer+ y2Answer)/2], [NSNumber numberWithDouble:(z1Answer+ z1Answer)/2], nil];
        }
        else if (oneAnswer)
        {
            answer = [NSArray arrayWithObjects:[NSNumber numberWithDouble: x1Answer],[NSNumber numberWithDouble: y1Answer], [NSNumber numberWithDouble:z1Answer], nil];
        }
        return answer;
    }
    else
    {
        return nil;
    }


}