How to find the "best" position using Newton's method for n samples

44 Views Asked by At

I am trying to find the best position for n sphere intersections using Newton's method.

        Sphere equation: (x - a)2 + (y - b)2 + (z - c)2 = r2
            where a, b, c = center of sphere
            where x, y, z = sample location
            where r2 = radius squared

for a point to lie on any sphere, it needs to satisfy this equation. So, for a point to lie on (or close) to multiple spheres, it needs to satisfy this equation for multiple spheres.

I think we can make a cost function like this:

        sum((x - a_i)2 + (y - b_i)2 + (z - c_i)2 - (r_i)2) == 0;
            where a_i, b_i, c_i = center of i sphere
            where x, y, z = sample location
            where r_i = radius of i sphere

        since dot(pos - center) should equal radii squared, if dot(pos - center) - radii squared == 0, it is the same function.

Newton's method works by using taking samples at intervals, and then uses the derivative of a function in order to move it. This equation looks like this:

        s2 = s1 - (f(s1) / f`(s1))
            where   (s2) = second sample location (target)
                    f(s1) = first sample location's equation
                    f`(s1) = derivative of the sample

I have written the following function in C#, which solves the problem when I input 2 spheres:

    // For maxIterations:
    for (int i = 0; i < maxIterations; i++)
    {
        float3 vDist = float3.zero;
        float errorFunction = 0f;
        float3 derivative = float3.zero;

        // For each sphere
        for (int j = 0; j < centers.Length; j++)
        {
            // Calculate the sqDistance between the sample point and the sphere, and subtract it with the squared radii in order to get an error function
            vDist = sampleLocation - centers[j];
            errorFunction += math.dot(vDist, vDist) - (radii[j] * radii[j]);

            // The derivative of the sphere is needed in order to move the sample
            derivative += new float3((sampleLocation.x - centers[j].x) * (sampleLocation.x - centers[j].x), (sampleLocation.y - centers[j].y) * (sampleLocation.y - centers[j].y), (sampleLocation.z - centers[j].z) * (sampleLocation.z - centers[j].z));
        }

        // If the errorFunction gets within a certain threshold, the position is found
        if (errorFunction >= -threshold && errorFunction <= threshold)
        {
            print("Found position: " + sampleLocation + " in " + i + ", error: " + errorFunction);
            break;
        }

        // Move the sample based on the error function and the derivative of the sphere
        sampleLocation = sampleLocation - (errorFunction / derivative);
    }

However, when I input 3 spheres, it does not work anymore, I think because the equation becomes non-linear?

I found a good reference on stackoverflow:

https://stackoverflow.com/questions/9527432/math-3d-positioning-multilateration

but I don't quite understand the answer given there.

Could someone try to help me extend this function to work with multiple spheres?