Find center of N concentric spheres given N radii and N points on respective spheres

sphere_intersection.cpp

//
// Solution to
// http://bit.ly/2JTGuSX
//

#include "Eigen/Dense"

...

Eigen::Matrix<double, 3, 3> ATA;
Eigen::Matrix<double, 3, 1> ATb;
ATA.Zero();
ATb.Zero();
for (int i = 0; i < N-1; i++) {
    const double dx = P[i+1][0] - P[i][0];
    const double dy = P[i+1][1] - P[i][1];
    const double dz = P[i+1][2] - P[i][2];
    ATA(0,0) += dx*dx;  ATA(0,1) += dx*dy;  ATA(0,2) += dx*dz;
    ATA(1,1) += dy*dy;  ATA(1,2) += dy*dz;
    ATA(2,2) += dz*dz;
    const double dxx = P[i+1][0]*P[i+1][0] - P[i][0]*P[i][0];
    const double dyy = P[i+1][1]*P[i+1][1] - P[i][1]*P[i][1];
    const double dzz = P[i+1][2]*P[i+1][2] - P[i][2]*P[i][2];
    const double drr = R[i+1]*R[i+1] - R[i]*R[i];
    const double v = drr - (dxx + dyy + dzz);
    ATb(0) += dx*v;
    ATb(1) += dy*v;
    ATb(2) += dz*v;
}

ATA(1,0) = ATA(0,1);
ATA(2,0) = ATA(0,2);
ATA(2,1) = ATA(1,2);
ATb(0) *= -0.5;
ATb(1) *= -0.5;
ATb(2) *= -0.5;

Eigen::Matrix<double, 3, 1> c = ATA.fullPivLu().solve(ATb);