text: // David Eberly, Geometric Tools, Redmond WA 98052
  // Copyright (c) 1998-2016
  // Distributed under the Boost Software License, Version 1.0.
  // http://www.boost.org/LICENSE_1_0.txt
  // http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
  // File Version: 3.0.0 (2016/06/19)
  
  #pragma once
  
  #include <Mathematics/GteMatrix.h>
  #include <Mathematics/GteVector3.h>
  #include <Mathematics/GteHypersphere.h>
  #include <Mathematics/GteSymmetricEigensolver.h>
  
  namespace gte
  {
  
  // The quadratic fit is
  //
  //   0 = C[0] + C[1]*X + C[2]*Y + C[3]*Z + C[4]*X^2 + C[5]*Y^2
  //       + C[6]*Z^2 + C[7]*X*Y + C[8]*X*Z + C[9]*Y*Z
  //
  // subject to Length(C) = 1.  Minimize E(C) = C^t M C with Length(C) = 1
  // and M = (sum_i V_i)(sum_i V_i)^t where
  //
  //   V = (1, X, Y, Z, X^2, Y^2, Z^2, X*Y, X*Z, Y*Z)
  //         
  // The minimum value is the smallest eigenvalue of M and C is a corresponding
  // unit length eigenvector.
  //
  // Input:
  //   n = number of points to fit
  //   p[0..n-1] = array of points to fit
  //
  // Output:
  //   c[0..9] = coefficients of quadratic fit (the eigenvector)
  //   return value of function is nonnegative and a measure of the fit
  //   (the minimum eigenvalue; 0 = exact fit, positive otherwise)
  
  // Canonical forms.  The quadratic equation can be factored into
  // P^T A P + B^T P + K = 0 where P = (X,Y,Z), K = C[0], B = (C[1],C[2],C[3]),
  // and A is a 3x3 symmetric matrix with A00 = C[4], A11 = C[5], A22 = C[6],
  // A01 = C[7]/2, A02 = C[8]/2, and A12 = C[9]/2.  Matrix A = R^T D R where
  // R is orthogonal and D is diagonal (using an eigendecomposition).  Define
  // V = R P = (v0,v1,v2), E = R B = (e0,e1,e2), D = diag(d0,d1,d2), and f = K
  // to obtain
  //
  //   d0 v0^2 + d1 v1^2 + d2 v^2 + e0 v0 + e1 v1 + e2 v2 + f = 0
  //
  // The characterization depends on the signs of the d_i.
  
  template <typename Real>
  class ApprQuadratic3
  {
  public:
      Real operator()(int numPoints, Vector3<Real> const* points,
          Real coefficidents[10]);
  };
  
  
  // If you think your points are nearly spherical, use this.  Sphere is of form
  // C'[0]+C'[1]*X+C'[2]*Y+C'[3]*Z+C'[4]*(X^2+Y^2+Z^2) where Length(C') = 1.
  // Function returns C = (C'[0]/C'[4],C'[1]/C'[4],C'[2]/C'[4],C'[3]/C'[4]), so
  // fitted sphere is C[0]+C[1]*X+C[2]*Y+C[3]*Z+X^2+Y^2+Z^2.  Center is
  // (xc,yc,zc) = -0.5*(C[1],C[2],C[3]) and radius is rad =
  // sqrt(xc*xc+yc*yc+zc*zc-C[0]).
  template <typename Real>
  class ApprQuadraticSphere3
  {
  public:
      Real operator()(int numPoints, Vector3<Real> const* points,
          Sphere3<Real>& sphere);
  };
  
  
  template <typename Real>
  Real ApprQuadratic3<Real>::operator()(int numPoints,
      Vector3<Real> const* points, Real coefficients[10])
  {
      Matrix<10, 10, Real> A;
      for (int i = 0; i < numPoints; ++i)
      {
          Real x = points[i][0];
          Real y = points[i][1];
          Real z = points[i][2];
          Real x2 = x*x;
          Real y2 = y*y;
          Real z2 = z*z;
          Real xy = x*y;
          Real xz = x*z;
          Real yz = y*z;
          Real x3 = x*x2;
          Real xy2 = x*y2;
          Real xz2 = x*z2;
          Real x2y = x*xy;
          Real x2z = x*xz;
          Real xyz = x*y*z;
          Real y3 = y*y2;
          Real yz2 = y*z2;
          Real y2z = y*yz;
          Real z3 = z*z2;
          Real x4 = x*x3;
          Real x2y2 = x*xy2;
          Real x2z2 = x*xz2;
          Real x3y = x*x2y;
          Real x3z = x*x2z;
          Real x2yz = x*xyz;
          Real y4 = y*y3;
          Real y2z2 = y*yz2;
          Real xy3 = x*y3;
          Real xy2z = x*y2z;
          Real y3z = y*y2z;
          Real z4 = z*z3;
          Real xyz2 = x*yz2;
          Real xz3 = x*z3;
          Real yz3 = y*z3;
  
          A(0, 1) += x;
          A(0, 2) += y;
          A(0, 3) += z;
          A(0, 4) += x2;
          A(0, 5) += y2;
          A(0, 6) += z2;
          A(0, 7) += xy;
          A(0, 8) += xz;
          A(0, 9) += yz;
          A(1, 4) += x3;
          A(1, 5) += xy2;
          A(1, 6) += xz2;
          A(1, 7) += x2y;
          A(1, 8) += x2z;
          A(1, 9) += xyz;
          A(2, 5) += y3;
          A(2, 6) += yz2;
          A(2, 9) += y2z;
          A(3, 6) += z3;
          A(4, 4) += x4;
          A(4, 5) += x2y2;
          A(4, 6) += x2z2;
          A(4, 7) += x3y;
          A(4, 8) += x3z;
          A(4, 9) += x2yz;
          A(5, 5) += y4;
          A(5, 6) += y2z2;
          A(5, 7) += xy3;
          A(5, 8) += xy2z;
          A(5, 9) += y3z;
          A(6, 6) += z4;
          A(6, 7) += xyz2;
          A(6, 8) += xz3;
          A(6, 9) += yz3;
          A(9, 9) += y2z2;
      }
  
      A(0, 0) = static_cast<Real>(numPoints);
      A(1, 1) = A(0, 4);
      A(1, 2) = A(0, 7);
      A(1, 3) = A(0, 8);
      A(2, 2) = A(0, 5);
      A(2, 3) = A(0, 9);
      A(2, 4) = A(1, 7);
      A(2, 7) = A(1, 5);
      A(2, 8) = A(1, 9);
      A(3, 3) = A(0, 6);
      A(3, 4) = A(1, 8);
      A(3, 5) = A(2, 9);
      A(3, 7) = A(1, 9);
      A(3, 8) = A(1, 6);
      A(3, 9) = A(2, 6);
      A(7, 7) = A(4, 5);
      A(7, 8) = A(4, 9);
      A(7, 9) = A(5, 8);
      A(8, 8) = A(4, 6);
      A(8, 9) = A(6, 7);
      A(9, 9) = A(5, 6);
  
      for (int row = 0; row < 10; ++row)
      {
          for (int col = 0; col < row; ++col)
          {
              A(row, col) = A(col, row);
          }
      }
  
      Real invNumPoints = ((Real)1) / static_cast<Real>(numPoints);
      for (int row = 0; row < 10; ++row)
      {
          for (int col = 0; col < 10; ++col)
          {
              A(row, col) *= invNumPoints;
          }
      }
  
      SymmetricEigensolver<Real> es(10, 1024);
      es.Solve(&A[0], +1);
      es.GetEigenvector(0, &coefficients[0]);
  
      // For an exact fit, numeric round-off errors might make the minimum
      // eigenvalue just slightly negative.  Return the absolute value since
      // the application might rely on the return value being nonnegative.
      return std::abs(es.GetEigenvalue(0));
  }
  
  template <typename Real>
  Real ApprQuadraticSphere3<Real>::operator()(int numPoints,
      Vector3<Real> const* points, Sphere3<Real>& sphere)
  {
      Matrix<5, 5, Real> A;
      for (int i = 0; i < numPoints; ++i)
      {
          Real x = points[i][0];
          Real y = points[i][1];
          Real z = points[i][2];
          Real x2 = x*x;
          Real y2 = y*y;
          Real z2 = z*z;
          Real xy = x*y;
          Real xz = x*z;
          Real yz = y*z;
          Real r2 = x2 + y2 + z2;
          Real xr2 = x*r2;
          Real yr2 = y*r2;
          Real zr2 = z*r2;
          Real r4 = r2*r2;
  
          A(0, 1) += x;
          A(0, 2) += y;
          A(0, 3) += z;
          A(0, 4) += r2;
          A(1, 1) += x2;
          A(1, 2) += xy;
          A(1, 3) += xz;
          A(1, 4) += xr2;
          A(2, 2) += y2;
          A(2, 3) += yz;
          A(2, 4) += yr2;
          A(3, 3) += z2;
          A(3, 4) += zr2;
          A(4, 4) += r4;
      }
  
      A(0, 0) = static_cast<Real>(numPoints);
  
      for (int row = 0; row < 5; ++row)
      {
          for (int col = 0; col < row; ++col)
          {
              A(row, col) = A(col, row);
          }
      }
  
      Real invNumPoints = ((Real)1) / static_cast<Real>(numPoints);
      for (int row = 0; row < 5; ++row)
      {
          for (int col = 0; col < 5; ++col)
          {
              A(row, col) *= invNumPoints;
          }
      }
  
      SymmetricEigensolver<Real> es(5, 1024);
      es.Solve(&A[0], +1);
      Vector<5, Real> evector;
      es.GetEigenvector(0, &evector[0]);
  
      Real inv = ((Real)1) / evector[4];  // TODO: Guard against zero divide?
      Real coefficients[4];
      for (int row = 0; row < 4; ++row)
      {
          coefficients[row] = inv * evector[row];
      }
  
      sphere.center[0] = ((Real)-0.5) * coefficients[1];
      sphere.center[1] = ((Real)-0.5) * coefficients[2];
      sphere.center[2] = ((Real)-0.5) * coefficients[3];
      sphere.radius = sqrt(std::abs(Dot(sphere.center, sphere.center) -
          coefficients[0]));
  
      // For an exact fit, numeric round-off errors might make the minimum
      // eigenvalue just slightly negative.  Return the absolute value since
      // the application might rely on the return value being nonnegative.
      return std::abs(es.GetEigenvalue(0));
  }
  
  
  }
  