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?