// SPDX-FileCopyrightText: Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
// SPDX-FileCopyrightText: Copyright 2011 Sandia Corporation
// SPDX-License-Identifier: LicenseRef-BSD-3-Clause-Sandia-USGov
#include "vtkMath.h"

#include "vtkBoxMuellerRandomSequence.h"
#include "vtkDataArray.h"
#include "vtkDebugLeaks.h"
#include "vtkMinimalStandardRandomSequence.h"
#include "vtkObjectFactory.h"
#include "vtkTypeTraits.h"

#include <cassert>
#include <cmath>
#include <limits>
#include <vector>

VTK_ABI_NAMESPACE_BEGIN
vtkStandardNewMacro(vtkMath);

class vtkMathInternal : public vtkObjectBase
{
public:
  vtkBaseTypeMacro(vtkMathInternal, vtkObjectBase);
  static vtkMathInternal* New()
  {
    // Can't use object factory macros, since they cast to vtkObject*.
    vtkMathInternal* ret = new vtkMathInternal;
    ret->InitializeObjectBase();
    return ret;
  }

  vtkMinimalStandardRandomSequence* Uniform;
  vtkBoxMuellerRandomSequence* Gaussian;
  std::vector<vtkTypeInt64> MemoizeFactorial;

private:
  vtkMathInternal();
  ~vtkMathInternal() override;
};

vtkMathInternal::vtkMathInternal()
{
  this->Gaussian = vtkBoxMuellerRandomSequence::New();

  // This line assumes the current vtkBoxMuellerRandomSequence behavior:
  // an initial vtkMinimalStandardRandomSequence is created.
  this->Uniform =
    static_cast<vtkMinimalStandardRandomSequence*>(this->Gaussian->GetUniformSequence());
  this->Uniform->SetSeedOnly(1177); // One author's home address
  this->MemoizeFactorial.resize(21, 0);
}

vtkMathInternal::~vtkMathInternal()
{
  this->Gaussian->Delete();
}

vtkSmartPointer<vtkMathInternal> vtkMath::Internal = vtkSmartPointer<vtkMathInternal>::New();

//
// Some useful macros and functions
//

//------------------------------------------------------------------------------
// Return the lowest value "i" for which 2^i >= x
int vtkMath::CeilLog2(vtkTypeUInt64 x)
{
  static const vtkTypeUInt64 t[6] = { 0xffffffff00000000ull, 0x00000000ffff0000ull,
    0x000000000000ff00ull, 0x00000000000000f0ull, 0x000000000000000cull, 0x0000000000000002ull };

  int j = 32;

  // if x is not a power of two, add 1 to final answer
  // (this is the "ceil" part of the computation)
  int y = (((x & (x - 1)) == 0) ? 0 : 1);

  // loop through the table (this unrolls nicely)
  for (int i = 0; i < 6; ++i)
  {
    int k = (((x & t[i]) == 0) ? 0 : j);
    y += k;
    x >>= k;
    j >>= 1;
  }

  return y;
}

//------------------------------------------------------------------------------
// Generate pseudo-random numbers distributed according to the uniform
// distribution between 0.0 and 1.0.
// This is used to provide portability across different systems.
double vtkMath::Random()
{
  vtkMath::Internal->Uniform->Next();
  return vtkMath::Internal->Uniform->GetValue();
}

//------------------------------------------------------------------------------
// Initialize seed value. NOTE: Random() has the bad property that
// the first random number returned after RandomSeed() is called
// is proportional to the seed value! To help solve this, call
// RandomSeed() a few times inside seed. This doesn't ruin the
// repeatability of Random().
void vtkMath::RandomSeed(int s)
{
  vtkMath::Internal->Uniform->SetSeed(s);
}

//------------------------------------------------------------------------------
// Description:
// Return the current seed used by the random number generator.
int vtkMath::GetSeed()
{
  return vtkMath::Internal->Uniform->GetSeed();
}

//------------------------------------------------------------------------------
double vtkMath::Random(double min, double max)
{
  vtkMath::Internal->Uniform->Next();
  return vtkMath::Internal->Uniform->GetRangeValue(min, max);
}

//------------------------------------------------------------------------------
double vtkMath::Gaussian()
{
  vtkMath::Internal->Gaussian->Next();
  return vtkMath::Internal->Gaussian->GetValue();
}

//------------------------------------------------------------------------------
double vtkMath::Gaussian(double mean, double std)
{
  vtkMath::Internal->Gaussian->Next();
  return vtkMath::Internal->Gaussian->GetScaledValue(mean, std);
}

//------------------------------------------------------------------------------
vtkTypeInt64 vtkMath::Factorial(int N)
{
  if (N > 20)
  {
    vtkGenericWarningMacro("Factorial(" << N << ") would overflow.");
    return vtkTypeTraits<vtkTypeInt64>::Max();
  }

  if (N == 0)
  {
    return 1;
  }

  if (vtkMath::Internal->MemoizeFactorial[N] != 0)
  {
    return vtkMath::Internal->MemoizeFactorial[N];
  }

  vtkTypeInt64 r = vtkMath::Factorial(N - 1) * N;
  vtkMath::Internal->MemoizeFactorial[N] = r;
  return r;
}

//------------------------------------------------------------------------------
// The number of combinations of n objects from a pool of m objects (m>n).
vtkTypeInt64 vtkMath::Binomial(int m, int n)
{
  double r = 1;
  for (int i = 1; i <= n; ++i)
  {
    r *= static_cast<double>(m - i + 1) / i;
  }
  return static_cast<vtkTypeInt64>(r);
}

//------------------------------------------------------------------------------
// Start iterating over "m choose n" objects.
// This function returns an array of n integers, each from 0 to m-1.
// These integers represent the n items chosen from the set [0,m[.
int* vtkMath::BeginCombination(int m, int n)
{
  if (m < n)
  {
    return nullptr;
  }

  int* r = new int[n];
  for (int i = 0; i < n; ++i)
  {
    r[i] = i;
  }
  return r;
}

//------------------------------------------------------------------------------
// Given \a m, \a n, and a valid \a combination of \a n integers in
// the range [0,m[, this function alters the integers into the next
// combination in a sequence of all combinations of \a n items from
// a pool of \a m.
// If the \a combination is the last item in the sequence on input,
// then \a combination is unaltered and 0 is returned.
// Otherwise, 1 is returned and \a combination is updated.
int vtkMath::NextCombination(int m, int n, int* combination)
{
  int status = 0;
  for (int i = n - 1; i >= 0; --i)
  {
    if (combination[i] < m - n + i)
    {
      int j = combination[i] + 1;
      while (i < n)
      {
        combination[i++] = j++;
      }
      status = 1;
      break;
    }
  }
  return status;
}

//------------------------------------------------------------------------------
// Free the "iterator" array created by vtkMath::BeginCombination.
//
void vtkMath::FreeCombination(int* combination)
{
  delete[] combination;
}

//------------------------------------------------------------------------------
// Given a unit vector v1, find two other unit vectors v2 and v3 which
// which form an orthonormal set.
template <class T1, class T2, class T3>
inline void vtkMathPerpendiculars(const T1 v1[3], T2 v2[3], T3 v3[3], double theta)
{
  double v1sq = v1[0] * v1[0];
  double v2sq = v1[1] * v1[1];
  double v3sq = v1[2] * v1[2];
  double r = std::sqrt(v1sq + v2sq + v3sq);

  // transpose the vector to avoid divide-by-zero error
  int dv1, dv2, dv3;
  if (v1sq > v2sq && v1sq > v3sq)
  {
    dv1 = 0;
    dv2 = 1;
    dv3 = 2;
  }
  else if (v2sq > v3sq)
  {
    dv1 = 1;
    dv2 = 2;
    dv3 = 0;
  }
  else
  {
    dv1 = 2;
    dv2 = 0;
    dv3 = 1;
  }

  double a = v1[dv1] / r;
  double b = v1[dv2] / r;
  double c = v1[dv3] / r;

  double tmp = std::sqrt(a * a + c * c);

  if (theta != 0.0)
  {
    double sintheta = sin(theta);
    double costheta = cos(theta);

    if (v2)
    {
      v2[dv1] = (c * costheta - a * b * sintheta) / tmp;
      v2[dv2] = sintheta * tmp;
      v2[dv3] = (-a * costheta - b * c * sintheta) / tmp;
    }

    if (v3)
    {
      v3[dv1] = (-c * sintheta - a * b * costheta) / tmp;
      v3[dv2] = costheta * tmp;
      v3[dv3] = (a * sintheta - b * c * costheta) / tmp;
    }
  }
  else
  {
    if (v2)
    {
      v2[dv1] = c / tmp;
      v2[dv2] = 0;
      v2[dv3] = -a / tmp;
    }

    if (v3)
    {
      v3[dv1] = -a * b / tmp;
      v3[dv2] = tmp;
      v3[dv3] = -b * c / tmp;
    }
  }
}

void vtkMath::Perpendiculars(const double v1[3], double v2[3], double v3[3], double theta)
{
  vtkMathPerpendiculars(v1, v2, v3, theta);
}

void vtkMath::Perpendiculars(const float v1[3], float v2[3], float v3[3], double theta)
{
  vtkMathPerpendiculars(v1, v2, v3, theta);
}

//------------------------------------------------------------------------------
// Solve linear equation Ax = b using Gaussian Elimination with Partial Pivoting
// for a 2x2 system. If the matrix is found to be singular within a small numerical
// tolerance close to machine precision then 0 is returned.
vtkTypeBool vtkMath::SolveLinearSystemGEPP2x2(
  double a00, double a01, double a10, double a11, double b0, double b1, double& x0, double& x1)
{
  // Check if any of the matrix coefficients is zero.
  // If so then swap rows/columns to form an upper triangular matrix without
  // having to use GEPP.
  bool cols_swapped = false;
  if ((a00 == 0) || (a01 == 0) || (a10 == 0) || (a11 == 0))
  {
    // zero in either row of the 2nd column?
    if ((a01 == 0) || (a11 == 0))
    {
      // swap columns
      std::swap(a00, a01);
      std::swap(a10, a11);
      cols_swapped = true;
    }
    // zero in a00?
    if (a00 == 0)
    {
      // swap rows
      std::swap(a00, a10);
      std::swap(a01, a11);
      std::swap(b0, b1);
    }
  }
  else
  {
    // None of the matrix coefficients are exactly zero.
    // Use GEPP to form upper triangular matrix, i.e. so that a10 == 0.
    // Select pivot by looking at largest absolute value in a00, a10
    if (std::abs(a00) < std::abs(a10))
    {
      // swap rows so largest coefficient in first column is in the first row
      std::swap(a00, a10);
      std::swap(a01, a11);
      std::swap(b0, b1);
    }
    // a10 = 0;            // bookkeeping only, value is no longer required
    const double f = -a10 / a00;
    a11 += a01 * f;
    b1 += b0 * f;
  }
  // Have now an exact zero in a10.
  // Need to check for singularity by looking at a11.
  // Note the choice of eps is reasonable but somewhat arbitrary.
  static const double eps = 256 * std::numeric_limits<double>::epsilon();
  if (std::abs(a11) < eps)
  {
    // matrix is singular within small numerical tolerance
    return 0;
  }
  // Solve the triangular system
  if (a11 != 0)
  {
    x1 = b1 / a11;
  }
  else
  {
    return 0;
  }
  if (a00 != 0)
  {
    x0 = (b0 - a01 * x1) / a00;
  }
  else
  {
    return 0;
  }
  // other failures in solution?
  if (!std::isfinite(x0) || !std::isfinite(x1))
  {
    return 0;
  }
  // If necessary swap solution vector rows.
  if (cols_swapped)
  {
    std::swap(x0, x1);
  }
  return 1;
}

namespace
{
constexpr double VTK_SMALL_NUMBER = 1.0e-12;
constexpr int VTK_MAX_SCRATCH_ARRAY_SIZE = 10;
constexpr int VTK_MAX_ROTATIONS = 20;
}

//------------------------------------------------------------------------------
// Solve linear equations Ax = b using Crout's method. Input is square matrix A
// and load vector b. Solution x is written over load vector. The dimension of
// the matrix is specified in size. If error is found, method returns a 0.
vtkTypeBool vtkMath::SolveLinearSystem(double** A, double* x, int size)
{
  // if we solving something simple, just solve it
  //
  if (size == 2)
  {
    return SolveLinearSystemGEPP2x2(A[0][0], A[0][1], A[1][0], A[1][1], x[0], x[1], x[0], x[1]);
  }
  else if (size == 1)
  {
    if (A[0][0] == 0.0)
    {
      // Unable to solve linear system
      return 0;
    }

    x[0] /= A[0][0];
    return 1;
  }

  //
  // System of equations is not trivial, use Crout's method
  //

  // Check on allocation of working vectors
  //
  int *index, scratch[VTK_MAX_SCRATCH_ARRAY_SIZE];
  index = (size <= VTK_MAX_SCRATCH_ARRAY_SIZE ? scratch : new int[size]);

  //
  // Factor and solve matrix
  //
  if (vtkMath::LUFactorLinearSystem(A, index, size) == 0)
  {
    return 0;
  }
  vtkMath::LUSolveLinearSystem(A, index, x, size);

  if (size > VTK_MAX_SCRATCH_ARRAY_SIZE)
  {
    delete[] index;
  }
  return 1;
}

//------------------------------------------------------------------------------
// Invert input square matrix A into matrix AI. Note that A is modified during
// the inversion. The size variable is the dimension of the matrix. Returns 0
// if inverse not computed.
vtkTypeBool vtkMath::InvertMatrix(double** A, double** AI, int size)
{
  int iScratch[VTK_MAX_SCRATCH_ARRAY_SIZE];
  int* index = (size <= VTK_MAX_SCRATCH_ARRAY_SIZE ? iScratch : new int[size]);
  double dScratch[VTK_MAX_SCRATCH_ARRAY_SIZE];
  double* column = (size <= VTK_MAX_SCRATCH_ARRAY_SIZE ? dScratch : new double[size]);

  vtkTypeBool retVal = vtkMath::InvertMatrix(A, AI, size, index, column);

  if (size > VTK_MAX_SCRATCH_ARRAY_SIZE)
  {
    delete[] index;
    delete[] column;
  }

  return retVal;
}

//------------------------------------------------------------------------------
// Factor linear equations Ax = b using LU decomposition A = LU where L is
// lower triangular matrix and U is upper triangular matrix. Input is
// square matrix A, integer array of pivot indices index[0->n-1], and size
// of square matrix n. Output factorization LU is in matrix A. If error is
// found, method returns 0.
vtkTypeBool vtkMath::LUFactorLinearSystem(double** A, int* index, int size)
{
  double scratch[VTK_MAX_SCRATCH_ARRAY_SIZE];
  double* scale = (size <= VTK_MAX_SCRATCH_ARRAY_SIZE ? scratch : new double[size]);

  int i, j, k;
  int maxI = 0;
  double largest, temp1, temp2, sum;

  //
  // Loop over rows to get implicit scaling information
  //
  for (i = 0; i < size; ++i)
  {
    for (largest = 0.0, j = 0; j < size; ++j)
    {
      if ((temp2 = std::abs(A[i][j])) > largest)
      {
        largest = temp2;
      }
    }

    if (largest == 0.0)
    {
      vtkGenericWarningMacro(<< "Unable to factor linear system");
      if (size > VTK_MAX_SCRATCH_ARRAY_SIZE)
      {
        delete[] scale;
      }
      return 0;
    }
    scale[i] = 1.0 / largest;
  }
  //
  // Loop over all columns using Crout's method
  //
  for (j = 0; j < size; ++j)
  {
    for (i = 0; i < j; ++i)
    {
      sum = A[i][j];
      for (k = 0; k < i; ++k)
      {
        sum -= A[i][k] * A[k][j];
      }
      A[i][j] = sum;
    }
    //
    // Begin search for largest pivot element
    //
    for (largest = 0.0, i = j; i < size; ++i)
    {
      sum = A[i][j];
      for (k = 0; k < j; ++k)
      {
        sum -= A[i][k] * A[k][j];
      }
      A[i][j] = sum;

      if ((temp1 = scale[i] * std::abs(sum)) >= largest)
      {
        largest = temp1;
        maxI = i;
      }
    }
    //
    // Check for row interchange
    //
    if (j != maxI)
    {
      for (k = 0; k < size; ++k)
      {
        temp1 = A[maxI][k];
        A[maxI][k] = A[j][k];
        A[j][k] = temp1;
      }
      scale[maxI] = scale[j];
    }
    //
    // Divide by pivot element and perform elimination
    //
    index[j] = maxI;

    if (std::abs(A[j][j]) <= VTK_SMALL_NUMBER)
    {
      vtkGenericWarningMacro(<< "Unable to factor linear system");
      if (size > VTK_MAX_SCRATCH_ARRAY_SIZE)
      {
        delete[] scale;
      }
      return 0;
    }

    if (j != (size - 1))
    {
      temp1 = 1.0 / A[j][j];
      for (i = j + 1; i < size; ++i)
      {
        A[i][j] *= temp1;
      }
    }
  }

  if (size > VTK_MAX_SCRATCH_ARRAY_SIZE)
  {
    delete[] scale;
  }

  return 1;
}

//------------------------------------------------------------------------------
// Solve linear equations Ax = b using LU decomposition A = LU where L is
// lower triangular matrix and U is upper triangular matrix. Input is
// factored matrix A=LU, integer array of pivot indices index[0->n-1],
// load vector x[0->n-1], and size of square matrix n. Note that A=LU and
// index[] are generated from method LUFactorLinearSystem). Also, solution
// vector is written directly over input load vector.
void vtkMath::LUSolveLinearSystem(double** A, int* index, double* x, int size)
{
  int i, j, ii, idx;
  double sum;
  //
  // Proceed with forward and backsubstitution for L and U
  // matrices.  First, forward substitution.
  //
  for (ii = -1, i = 0; i < size; ++i)
  {
    idx = index[i];
    sum = x[idx];
    x[idx] = x[i];

    if (ii >= 0)
    {
      for (j = ii; j <= (i - 1); ++j)
      {
        sum -= A[i][j] * x[j];
      }
    }
    else if (sum != 0.0)
    {
      ii = i;
    }

    x[i] = sum;
  }
  //
  // Now, back substitution
  //
  for (i = size - 1; i >= 0; i--)
  {
    sum = x[i];
    for (j = i + 1; j < size; ++j)
    {
      sum -= A[i][j] * x[j];
    }
    x[i] = sum / A[i][i];
  }
}

#define VTK_ROTATE(a, i, j, k, l)                                                                  \
  g = a[i][j];                                                                                     \
  h = a[k][l];                                                                                     \
  a[i][j] = g - s * (h + g * tau);                                                                 \
  a[k][l] = h + s * (g - h * tau)

// Jacobi iteration for the solution of eigenvectors/eigenvalues of a nxn
// real symmetric matrix. Square nxn matrix a; size of matrix in n;
// output eigenvalues in w; and output eigenvectors in v. Resulting
// eigenvalues/vectors are sorted in decreasing order; eigenvectors are
// normalized.
// It assumes a is symmetric and uses only its upper right triangular part.
template <class T>
vtkTypeBool vtkJacobiN(T** a, int n, T* w, T** v)
{
  int i, j, k, iq, ip, numPos;
  T tresh, theta, tau, t, sm, s, h, g, c, tmp;
  T bspace[VTK_MAX_SCRATCH_ARRAY_SIZE], zspace[VTK_MAX_SCRATCH_ARRAY_SIZE];
  T* b = (n <= VTK_MAX_SCRATCH_ARRAY_SIZE) ? bspace : new T[n];
  T* z = (n <= VTK_MAX_SCRATCH_ARRAY_SIZE) ? zspace : new T[n];

  // initialize
  for (ip = 0; ip < n; ip++)
  {
    for (iq = 0; iq < n; iq++)
    {
      v[ip][iq] = 0.0;
    }
    v[ip][ip] = 1.0;
  }
  for (ip = 0; ip < n; ip++)
  {
    b[ip] = w[ip] = a[ip][ip];
    z[ip] = 0.0;
  }

  // begin rotation sequence
  for (i = 0; i < VTK_MAX_ROTATIONS; ++i)
  {
    sm = 0.0;
    for (ip = 0; ip < n - 1; ip++)
    {
      for (iq = ip + 1; iq < n; iq++)
      {
        sm += std::abs(a[ip][iq]);
      }
    }
    if (sm == 0.0)
    {
      break;
    }

    if (i < 3) // first 3 sweeps
    {
      tresh = 0.2 * sm / (n * n);
    }
    else
    {
      tresh = 0.0;
    }

    for (ip = 0; ip < n - 1; ip++)
    {
      for (iq = ip + 1; iq < n; iq++)
      {
        g = 100.0 * std::abs(a[ip][iq]);

        // after 4 sweeps
        if (i > 3 && (std::abs(w[ip]) + g) == std::abs(w[ip]) &&
          (std::abs(w[iq]) + g) == std::abs(w[iq]))
        {
          a[ip][iq] = 0.0;
        }
        else if (std::abs(a[ip][iq]) > tresh)
        {
          h = w[iq] - w[ip];
          if ((std::abs(h) + g) == std::abs(h))
          {
            t = (a[ip][iq]) / h;
          }
          else
          {
            theta = 0.5 * h / (a[ip][iq]);
            t = 1.0 / (std::abs(theta) + std::sqrt(1.0 + theta * theta));
            if (theta < 0.0)
            {
              t = -t;
            }
          }
          c = 1.0 / std::sqrt(1 + t * t);
          s = t * c;
          tau = s / (1.0 + c);
          h = t * a[ip][iq];
          z[ip] -= h;
          z[iq] += h;
          w[ip] -= h;
          w[iq] += h;
          a[ip][iq] = 0.0;

          // ip already shifted left by 1 unit
          for (j = 0; j <= ip - 1; ++j)
          {
            VTK_ROTATE(a, j, ip, j, iq);
          }
          // ip and iq already shifted left by 1 unit
          for (j = ip + 1; j <= iq - 1; ++j)
          {
            VTK_ROTATE(a, ip, j, j, iq);
          }
          // iq already shifted left by 1 unit
          for (j = iq + 1; j < n; ++j)
          {
            VTK_ROTATE(a, ip, j, iq, j);
          }
          for (j = 0; j < n; ++j)
          {
            VTK_ROTATE(v, j, ip, j, iq);
          }
        }
      }
    }

    for (ip = 0; ip < n; ip++)
    {
      b[ip] += z[ip];
      w[ip] = b[ip];
      z[ip] = 0.0;
    }
  }

  //// this is NEVER called
  if (i >= VTK_MAX_ROTATIONS)
  {
    vtkGenericWarningMacro("vtkMath::Jacobi: Error extracting eigenfunctions");
    if (n > VTK_MAX_SCRATCH_ARRAY_SIZE)
    {
      delete[] b;
      delete[] z;
    }
    return 0;
  }

  // sort eigenfunctions                 these changes do not affect accuracy
  for (j = 0; j < n - 1; ++j) // boundary incorrect
  {
    k = j;
    tmp = w[k];
    for (i = j + 1; i < n; ++i) // boundary incorrect, shifted already
    {
      if (w[i] >= tmp) // why exchange if same?
      {
        k = i;
        tmp = w[k];
      }
    }
    if (k != j)
    {
      w[k] = w[j];
      w[j] = tmp;
      for (i = 0; i < n; ++i)
      {
        tmp = v[i][j];
        v[i][j] = v[i][k];
        v[i][k] = tmp;
      }
    }
  }
  // ensure eigenvector consistency (i.e., Jacobi can compute vectors that
  // are negative of one another (.707,.707,0) and (-.707,-.707,0). This can
  // reek havoc in hyperstreamline/other stuff. We will select the most
  // positive eigenvector.
  int ceil_half_n = (n >> 1) + (n & 1);
  for (j = 0; j < n; ++j)
  {
    for (numPos = 0, i = 0; i < n; ++i)
    {
      if (v[i][j] >= 0.0)
      {
        numPos++;
      }
    }
    if (numPos < ceil_half_n)
    {
      for (i = 0; i < n; ++i)
      {
        v[i][j] *= -1.0;
      }
    }
  }

  if (n > VTK_MAX_SCRATCH_ARRAY_SIZE)
  {
    delete[] b;
    delete[] z;
  }
  return 1;
}

#undef VTK_ROTATE
#undef VTK_MAX_ROTATIONS

//------------------------------------------------------------------------------
vtkTypeBool vtkMath::JacobiN(float** a, int n, float* w, float** v)
{
  return vtkJacobiN(a, n, w, v);
}

//------------------------------------------------------------------------------
vtkTypeBool vtkMath::JacobiN(double** a, int n, double* w, double** v)
{
  return vtkJacobiN(a, n, w, v);
}

//------------------------------------------------------------------------------
// Jacobi iteration for the solution of eigenvectors/eigenvalues of a 3x3
// real symmetric matrix. Square 3x3 matrix a; output eigenvalues in w;
// and output eigenvectors in v. Resulting eigenvalues/vectors are sorted
// in decreasing order; eigenvectors are normalized.
vtkTypeBool vtkMath::Jacobi(float** a, float* w, float** v)
{
  return vtkMath::JacobiN(a, 3, w, v);
}

//------------------------------------------------------------------------------
vtkTypeBool vtkMath::Jacobi(double** a, double* w, double** v)
{
  return vtkMath::JacobiN(a, 3, w, v);
}

//------------------------------------------------------------------------------
// Estimate the condition number of a LU factored matrix. Used to judge the
// accuracy of the solution. The matrix A must have been previously factored
// using the method LUFactorLinearSystem. The condition number is the ratio
// of the infinity matrix norm (i.e., maximum value of matrix component)
// divided by the minimum diagonal value. (This works for triangular matrices
// only: see Conte and de Boor, Elementary Numerical Analysis.)
double vtkMath::EstimateMatrixCondition(const double* const* A, int size)
{
  int i;
  int j;
  double min = VTK_FLOAT_MAX, max = (-VTK_FLOAT_MAX);

  // find the maximum value
  for (i = 0; i < size; ++i)
  {
    for (j = i; j < size; ++j)
    {
      if (std::abs(A[i][j]) > max)
      {
        max = std::abs(A[i][j]);
      }
    }
  }

  // find the minimum diagonal value
  for (i = 0; i < size; ++i)
  {
    if (std::abs(A[i][i]) < min)
    {
      min = std::abs(A[i][i]);
    }
  }

  if (min == 0.0)
  {
    return VTK_FLOAT_MAX;
  }
  else
  {
    return (max / min);
  }
}

//------------------------------------------------------------------------------
// Solves for the least squares best fit matrix for the homogeneous equation X'M' = 0'.
// Uses the method described on pages 40-41 of Computer Vision by
// Forsyth and Ponce, which is that the solution is the eigenvector
// associated with the minimum eigenvalue of T(X)X, where T(X) is the
// transpose of X.
// The inputs and output are transposed matrices.
//    Dimensions: X' is numberOfSamples by xOrder,
//                M' dimension is xOrder by 1.
// M' should be pre-allocated. All matrices are row major. The resultant
// matrix M' should be pre-multiplied to X' to get 0', or transposed and
// then post multiplied to X to get 0.
// Returns success/fail.
vtkTypeBool vtkMath::SolveHomogeneousLeastSquares(
  int numberOfSamples, double** xt, int xOrder, double** mt)
{
  // check dimensional consistency
  if (numberOfSamples < xOrder)
  {
    vtkGenericWarningMacro("Insufficient number of samples. Underdetermined.");
    return 0;
  }

  // set up intermediate variables
  // Allocate matrix to hold X times transpose of X
  double** XXt = new double*[xOrder]; // size x by x
  // Allocate the array of eigenvalues and eigenvectors
  double* eigenvals = new double[xOrder];
  double** eigenvecs = new double*[xOrder];

  int i, j, k;

  // Clear the upper triangular region (and btw, allocate the eigenvecs as well)
  for (i = 0; i < xOrder; ++i)
  {
    eigenvecs[i] = new double[xOrder];
    XXt[i] = new double[xOrder];
    for (j = 0; j < xOrder; ++j)
    {
      XXt[i][j] = 0.0;
    }
  }

  // Calculate XXt upper half only, due to symmetry
  for (k = 0; k < numberOfSamples; ++k)
  {
    for (i = 0; i < xOrder; ++i)
    {
      for (j = i; j < xOrder; ++j)
      {
        XXt[i][j] += xt[k][i] * xt[k][j];
      }
    }
  }

  // now fill in the lower half of the XXt matrix
  for (i = 0; i < xOrder; ++i)
  {
    for (j = 0; j < i; ++j)
    {
      XXt[i][j] = XXt[j][i];
    }
  }

  // Compute the eigenvectors and eigenvalues
  vtkMath::JacobiN(XXt, xOrder, eigenvals, eigenvecs);

  // Smallest eigenval is at the end of the list (xOrder-1), and solution is
  // corresponding eigenvec.
  for (i = 0; i < xOrder; ++i)
  {
    mt[i][0] = eigenvecs[i][xOrder - 1];
  }

  // Clean up:
  for (i = 0; i < xOrder; ++i)
  {
    delete[] XXt[i];
    delete[] eigenvecs[i];
  }
  delete[] XXt;
  delete[] eigenvecs;
  delete[] eigenvals;

  return 1;
}

//------------------------------------------------------------------------------
// Solves for the least squares best fit matrix for the equation X'M' = Y'.
// Uses pseudoinverse to get the ordinary least squares.
// The inputs and output are transposed matrices.
//    Dimensions: X' is numberOfSamples by xOrder,
//                Y' is numberOfSamples by yOrder,
//                M' dimension is xOrder by yOrder.
// M' should be pre-allocated. All matrices are row major. The resultant
// matrix M' should be pre-multiplied to X' to get Y', or transposed and
// then post multiplied to X to get Y
// By default, this method checks for the homogeneous condition where Y==0, and
// if so, invokes SolveHomogeneousLeastSquares. For better performance when
// the system is known not to be homogeneous, invoke with checkHomogeneous=0.
// Returns success/fail.
vtkTypeBool vtkMath::SolveLeastSquares(int numberOfSamples, double** xt, int xOrder, double** yt,
  int yOrder, double** mt, int checkHomogeneous)
{
  // check dimensional consistency
  if ((numberOfSamples < xOrder) || (numberOfSamples < yOrder))
  {
    vtkGenericWarningMacro("Insufficient number of samples. Underdetermined.");
    return 0;
  }

  int i, j, k;

  bool someHomogeneous = false;
  bool allHomogeneous = true;
  double** hmt = nullptr;
  vtkTypeBool homogRC = 0;
  int* homogenFlags = new int[yOrder];
  vtkTypeBool successFlag;

  // Ok, first init some flags check and see if all the systems are homogeneous
  if (checkHomogeneous)
  {
    // If Y' is zero, it's a homogeneous system and can't be solved via
    // the pseudoinverse method. Detect this case, warn the user, and
    // invoke SolveHomogeneousLeastSquares instead. Note that it doesn't
    // really make much sense for yOrder to be greater than one in this case,
    // since that's just yOrder occurrences of a 0 vector on the RHS, but
    // we allow it anyway. N

    // Initialize homogeneous flags on a per-right-hand-side basis
    for (j = 0; j < yOrder; ++j)
    {
      homogenFlags[j] = 1;
    }
    for (i = 0; i < numberOfSamples; ++i)
    {
      for (j = 0; j < yOrder; ++j)
      {
        if (std::abs(yt[i][j]) > VTK_SMALL_NUMBER)
        {
          allHomogeneous = false;
          homogenFlags[j] = 0;
        }
      }
    }

    // If we've got one system, and it's homogeneous, do it and bail out quickly.
    if (allHomogeneous && yOrder == 1)
    {
      vtkGenericWarningMacro(
        "Detected homogeneous system (Y=0), calling SolveHomogeneousLeastSquares()");
      delete[] homogenFlags;
      return vtkMath::SolveHomogeneousLeastSquares(numberOfSamples, xt, xOrder, mt);
    }

    // Ok, we've got more than one system of equations.
    // Figure out if we need to calculate the homogeneous equation solution for
    // any of them.
    if (allHomogeneous)
    {
      someHomogeneous = true;
    }
    else
    {
      for (j = 0; j < yOrder; ++j)
      {
        if (homogenFlags[j])
        {
          someHomogeneous = true;
        }
      }
    }
  }

  // If necessary, solve the homogeneous problem
  if (someHomogeneous)
  {
    // hmt is the homogeneous equation version of mt, the general solution.
    hmt = new double*[xOrder];
    for (j = 0; j < xOrder; ++j)
    {
      // Only allocate 1 here, not yOrder, because here we're going to solve
      // just the one homogeneous equation subset of the entire problem
      hmt[j] = new double[1];
    }

    // Ok, solve the homogeneous problem
    homogRC = vtkMath::SolveHomogeneousLeastSquares(numberOfSamples, xt, xOrder, hmt);
  }

  // set up intermediate variables
  double** XXt = new double*[xOrder];  // size x by x
  double** XXtI = new double*[xOrder]; // size x by x
  double** XYt = new double*[xOrder];  // size x by y
  for (i = 0; i < xOrder; ++i)
  {
    XXt[i] = new double[xOrder];
    XXtI[i] = new double[xOrder];

    for (j = 0; j < xOrder; ++j)
    {
      XXt[i][j] = 0.0;
      XXtI[i][j] = 0.0;
    }

    XYt[i] = new double[yOrder];
    for (j = 0; j < yOrder; ++j)
    {
      XYt[i][j] = 0.0;
    }
  }

  // first find the pseudoinverse matrix
  for (k = 0; k < numberOfSamples; ++k)
  {
    for (i = 0; i < xOrder; ++i)
    {
      // first calculate the XXt matrix, only do the upper half (symmetrical)
      for (j = i; j < xOrder; ++j)
      {
        XXt[i][j] += xt[k][i] * xt[k][j];
      }

      // now calculate the XYt matrix
      for (j = 0; j < yOrder; ++j)
      {
        XYt[i][j] += xt[k][i] * yt[k][j];
      }
    }
  }

  // now fill in the lower half of the XXt matrix
  for (i = 0; i < xOrder; ++i)
  {
    for (j = 0; j < i; ++j)
    {
      XXt[i][j] = XXt[j][i];
    }
  }

  successFlag = vtkMath::InvertMatrix(XXt, XXtI, xOrder);

  // next get the inverse of XXt
  if (successFlag)
  {
    for (i = 0; i < xOrder; ++i)
    {
      for (j = 0; j < yOrder; ++j)
      {
        mt[i][j] = 0.0;
        for (k = 0; k < xOrder; ++k)
        {
          mt[i][j] += XXtI[i][k] * XYt[k][j];
        }
      }
    }
  }

  // Fix up any of the solutions that correspond to the homogeneous equation
  // problem.
  if (someHomogeneous)
  {
    for (j = 0; j < yOrder; ++j)
    {
      if (homogenFlags[j])
      {
        // Fix this one
        for (i = 0; i < xOrder; ++i)
        {
          mt[i][j] = hmt[i][0];
        }
      }
    }

    // Clean up
    for (i = 0; i < xOrder; ++i)
    {
      delete[] hmt[i];
    }
    delete[] hmt;
  }

  // clean up:
  // set up intermediate variables
  for (i = 0; i < xOrder; ++i)
  {
    delete[] XXt[i];
    delete[] XXtI[i];

    delete[] XYt[i];
  }
  delete[] XXt;
  delete[] XXtI;
  delete[] XYt;
  delete[] homogenFlags;

  if (someHomogeneous)
  {
    return homogRC && successFlag;
  }
  else
  {
    return successFlag;
  }
}

//=============================================================================
// Thread safe versions of math methods.
//=============================================================================

// Invert input square matrix A into matrix AI. Note that A is modified during
// the inversion. The size variable is the dimension of the matrix. Returns 0
// if inverse not computed.
// -----------------------
// For thread safe behavior, temporary arrays tmp1SIze and tmp2Size
// of length size must be passed in.
vtkTypeBool vtkMath::InvertMatrix(
  double** A, double** AI, int size, int* tmp1Size, double* tmp2Size)
{
  int i, j;

  //
  // Factor matrix; then begin solving for inverse one column at a time.
  // Note: tmp1Size returned value is used later, tmp2Size is just working
  // memory whose values are not used in LUSolveLinearSystem
  //
  if (vtkMath::LUFactorLinearSystem(A, tmp1Size, size, tmp2Size) == 0)
  {
    return 0;
  }

  for (j = 0; j < size; ++j)
  {
    for (i = 0; i < size; ++i)
    {
      tmp2Size[i] = 0.0;
    }
    tmp2Size[j] = 1.0;

    vtkMath::LUSolveLinearSystem(A, tmp1Size, tmp2Size, size);

    for (i = 0; i < size; ++i)
    {
      AI[i][j] = tmp2Size[i];
    }
  }

  return 1;
}

// Factor linear equations Ax = b using LU decomposition A = LU where L is
// lower triangular matrix and U is upper triangular matrix. Input is
// square matrix A, integer array of pivot indices index[0->n-1], and size
// of square matrix n. Output factorization LU is in matrix A. If error is
// found, method returns 0.
//------------------------------------------------------------------
// For thread safe, temporary memory array tmpSize of length size
// must be passed in.
vtkTypeBool vtkMath::LUFactorLinearSystem(double** A, int* index, int size, double* tmpSize)
{
  int i, j, k;
  int maxI = 0;
  double largest, temp1, temp2, sum;

  //
  // Loop over rows to get implicit scaling information
  //
  for (i = 0; i < size; ++i)
  {
    for (largest = 0.0, j = 0; j < size; ++j)
    {
      if ((temp2 = std::abs(A[i][j])) > largest)
      {
        largest = temp2;
      }
    }

    if (largest == 0.0)
    {
      vtkGenericWarningMacro(<< "Unable to factor linear system");
      return 0;
    }
    tmpSize[i] = 1.0 / largest;
  }
  //
  // Loop over all columns using Crout's method
  //
  for (j = 0; j < size; ++j)
  {
    for (i = 0; i < j; ++i)
    {
      sum = A[i][j];
      for (k = 0; k < i; ++k)
      {
        sum -= A[i][k] * A[k][j];
      }
      A[i][j] = sum;
    }
    //
    // Begin search for largest pivot element
    //
    for (largest = 0.0, i = j; i < size; ++i)
    {
      sum = A[i][j];
      for (k = 0; k < j; ++k)
      {
        sum -= A[i][k] * A[k][j];
      }
      A[i][j] = sum;

      if ((temp1 = tmpSize[i] * std::abs(sum)) >= largest)
      {
        largest = temp1;
        maxI = i;
      }
    }
    //
    // Check for row interchange
    //
    if (j != maxI)
    {
      for (k = 0; k < size; ++k)
      {
        std::swap(A[maxI][k], A[j][k]);
      }
      tmpSize[maxI] = tmpSize[j];
    }
    //
    // Divide by pivot element and perform elimination
    //
    index[j] = maxI;

    if (std::abs(A[j][j]) <= VTK_SMALL_NUMBER)
    {
      vtkGenericWarningMacro(<< "Unable to factor linear system");
      return 0;
    }

    if (j != (size - 1))
    {
      temp1 = 1.0 / A[j][j];
      for (i = j + 1; i < size; ++i)
      {
        A[i][j] *= temp1;
      }
    }
  }

  return 1;
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
// All of the following methods are for dealing with 3x3 matrices
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// helper function, swap two 3-vectors
template <class T>
inline void vtkSwapVectors3(T v1[3], T v2[3])
{
  for (int i = 0; i < 3; ++i)
  {
    std::swap(v1[i], v2[i]);
  }
}

//------------------------------------------------------------------------------
// Unrolled LU factorization of a 3x3 matrix with pivoting.
template <class T>
inline void vtkLUFactor3x3(T A[3][3], int index[3])
{
  int i, maxI;
  T tmp, largest;
  T scale[3];

  // Loop over rows to get implicit scaling information

  for (i = 0; i < 3; ++i)
  {
    largest = std::abs(A[i][0]);
    if ((tmp = std::abs(A[i][1])) > largest)
    {
      largest = tmp;
    }
    if ((tmp = std::abs(A[i][2])) > largest)
    {
      largest = tmp;
    }
    scale[i] = T(1.0) / largest;
  }

  // Loop over all columns using Crout's method

  // first column
  largest = scale[0] * std::abs(A[0][0]);
  maxI = 0;
  if ((tmp = scale[1] * std::abs(A[1][0])) >= largest)
  {
    largest = tmp;
    maxI = 1;
  }
  if ((tmp = scale[2] * std::abs(A[2][0])) >= largest)
  {
    maxI = 2;
  }
  if (maxI != 0)
  {
    vtkSwapVectors3(A[maxI], A[0]);
    scale[maxI] = scale[0];
  }
  index[0] = maxI;

  A[1][0] /= A[0][0];
  A[2][0] /= A[0][0];

  // second column
  A[1][1] -= A[1][0] * A[0][1];
  A[2][1] -= A[2][0] * A[0][1];
  largest = scale[1] * std::abs(A[1][1]);
  maxI = 1;
  if ((tmp = scale[2] * std::abs(A[2][1])) >= largest)
  {
    maxI = 2;
    vtkSwapVectors3(A[2], A[1]);
    scale[2] = scale[1];
  }
  index[1] = maxI;
  A[2][1] /= A[1][1];

  // third column
  A[1][2] -= A[1][0] * A[0][2];
  A[2][2] -= A[2][0] * A[0][2] + A[2][1] * A[1][2];
  index[2] = 2;
}

//------------------------------------------------------------------------------
void vtkMath::LUFactor3x3(float A[3][3], int index[3])
{
  vtkLUFactor3x3(A, index);
}

//------------------------------------------------------------------------------
void vtkMath::LUFactor3x3(double A[3][3], int index[3])
{
  vtkLUFactor3x3(A, index);
}

//------------------------------------------------------------------------------
// Backsubstitution with an LU-decomposed matrix.
template <class T1, class T2>
inline void vtkLUSolve3x3(const T1 A[3][3], const int index[3], T2 x[3])
{
  T2 sum;

  // forward substitution

  sum = x[index[0]];
  x[index[0]] = x[0];
  x[0] = sum;

  sum = x[index[1]];
  x[index[1]] = x[1];
  x[1] = sum - A[1][0] * x[0];

  sum = x[index[2]];
  x[index[2]] = x[2];
  x[2] = sum - A[2][0] * x[0] - A[2][1] * x[1];

  // back substitution

  x[2] = x[2] / A[2][2];
  x[1] = (x[1] - A[1][2] * x[2]) / A[1][1];
  x[0] = (x[0] - A[0][1] * x[1] - A[0][2] * x[2]) / A[0][0];
}

//------------------------------------------------------------------------------
void vtkMath::LUSolve3x3(const float A[3][3], const int index[3], float x[3])
{
  vtkLUSolve3x3(A, index, x);
}

//------------------------------------------------------------------------------
void vtkMath::LUSolve3x3(const double A[3][3], const int index[3], double x[3])
{
  vtkLUSolve3x3(A, index, x);
}

//------------------------------------------------------------------------------
// this method solves Ay = x for y
template <class T1, class T2, class T3>
inline void vtkLinearSolve3x3(const T1 A[3][3], const T2 x[3], T3 y[3])
{
  double a1 = A[0][0];
  double b1 = A[0][1];
  double c1 = A[0][2];
  double a2 = A[1][0];
  double b2 = A[1][1];
  double c2 = A[1][2];
  double a3 = A[2][0];
  double b3 = A[2][1];
  double c3 = A[2][2];

  // Compute the adjoint
  double d1 = vtkMath::Determinant2x2(b2, b3, c2, c3);
  double d2 = -vtkMath::Determinant2x2(a2, a3, c2, c3);
  double d3 = vtkMath::Determinant2x2(a2, a3, b2, b3);

  double e1 = -vtkMath::Determinant2x2(b1, b3, c1, c3);
  double e2 = vtkMath::Determinant2x2(a1, a3, c1, c3);
  double e3 = -vtkMath::Determinant2x2(a1, a3, b1, b3);

  double f1 = vtkMath::Determinant2x2(b1, b2, c1, c2);
  double f2 = -vtkMath::Determinant2x2(a1, a2, c1, c2);
  double f3 = vtkMath::Determinant2x2(a1, a2, b1, b2);

  // Compute the determinant
  double det = a1 * d1 + b1 * d2 + c1 * d3;

  // Multiply by the adjoint
  double v1 = d1 * x[0] + e1 * x[1] + f1 * x[2];
  double v2 = d2 * x[0] + e2 * x[1] + f2 * x[2];
  double v3 = d3 * x[0] + e3 * x[1] + f3 * x[2];

  // Divide by the determinant
  y[0] = v1 / det;
  y[1] = v2 / det;
  y[2] = v3 / det;
}

//------------------------------------------------------------------------------
void vtkMath::LinearSolve3x3(const float A[3][3], const float x[3], float y[3])
{
  vtkLinearSolve3x3(A, x, y);
}

//------------------------------------------------------------------------------
void vtkMath::LinearSolve3x3(const double A[3][3], const double x[3], double y[3])
{
  vtkLinearSolve3x3(A, x, y);
}

//------------------------------------------------------------------------------
template <class T1, class T2, class T3>
inline void vtkMultiply3x3(const T1 A[3][3], const T2 v[3], T3 u[3])
{
  T3 x = A[0][0] * v[0] + A[0][1] * v[1] + A[0][2] * v[2];
  T3 y = A[1][0] * v[0] + A[1][1] * v[1] + A[1][2] * v[2];
  T3 z = A[2][0] * v[0] + A[2][1] * v[1] + A[2][2] * v[2];

  u[0] = x;
  u[1] = y;
  u[2] = z;
}

//------------------------------------------------------------------------------
void vtkMath::Multiply3x3(const float A[3][3], const float v[3], float u[3])
{
  vtkMultiply3x3(A, v, u);
}

//------------------------------------------------------------------------------
void vtkMath::Multiply3x3(const double A[3][3], const double v[3], double u[3])
{
  vtkMultiply3x3(A, v, u);
}

//------------------------------------------------------------------------------
template <class T, class T2, class T3>
inline void vtkMultiplyMatrix3x3(const T A[3][3], const T2 B[3][3], T3 C[3][3])
{
  T3 D[3][3];

  for (int i = 0; i < 3; ++i)
  {
    D[0][i] = A[0][0] * B[0][i] + A[0][1] * B[1][i] + A[0][2] * B[2][i];
    D[1][i] = A[1][0] * B[0][i] + A[1][1] * B[1][i] + A[1][2] * B[2][i];
    D[2][i] = A[2][0] * B[0][i] + A[2][1] * B[1][i] + A[2][2] * B[2][i];
  }

  for (int j = 0; j < 3; ++j)
  {
    C[j][0] = D[j][0];
    C[j][1] = D[j][1];
    C[j][2] = D[j][2];
  }
}

//------------------------------------------------------------------------------
void vtkMath::Multiply3x3(const float A[3][3], const float B[3][3], float C[3][3])
{
  vtkMultiplyMatrix3x3(A, B, C);
}

//------------------------------------------------------------------------------
void vtkMath::Multiply3x3(const double A[3][3], const double B[3][3], double C[3][3])
{
  vtkMultiplyMatrix3x3(A, B, C);
}

//------------------------------------------------------------------------------
void vtkMath::MultiplyMatrix(const double* const* A, const double* const* B, unsigned int rowA,
  unsigned int colA, unsigned int rowB, unsigned int colB, double** C)
{
  // we need colA == rowB
  if (colA != rowB)
  {
    vtkGenericWarningMacro("Number of columns of A must match number of rows of B.");
  }

  // output matrix is rowA*colB

  // output row
  for (unsigned int i = 0; i < rowA; ++i)
  {
    // output col
    for (unsigned int j = 0; j < colB; ++j)
    {
      C[i][j] = 0;
      // sum for this point
      for (unsigned int k = 0; k < colA; ++k)
      {
        C[i][j] += A[i][k] * B[k][j];
      }
    }
  }
}

//------------------------------------------------------------------------------
template <class T1, class T2>
inline void vtkTranspose3x3(const T1 A[3][3], T2 AT[3][3])
{
  T2 tmp;
  tmp = A[1][0];
  AT[1][0] = A[0][1];
  AT[0][1] = tmp;
  tmp = A[2][0];
  AT[2][0] = A[0][2];
  AT[0][2] = tmp;
  tmp = A[2][1];
  AT[2][1] = A[1][2];
  AT[1][2] = tmp;

  AT[0][0] = A[0][0];
  AT[1][1] = A[1][1];
  AT[2][2] = A[2][2];
}

//------------------------------------------------------------------------------
void vtkMath::Transpose3x3(const float A[3][3], float AT[3][3])
{
  vtkTranspose3x3(A, AT);
}

//------------------------------------------------------------------------------
void vtkMath::Transpose3x3(const double A[3][3], double AT[3][3])
{
  vtkTranspose3x3(A, AT);
}

//------------------------------------------------------------------------------
template <class T1, class T2>
inline void vtkInvert3x3(const T1 A[3][3], T2 AI[3][3])
{
  double a1 = A[0][0];
  double b1 = A[0][1];
  double c1 = A[0][2];
  double a2 = A[1][0];
  double b2 = A[1][1];
  double c2 = A[1][2];
  double a3 = A[2][0];
  double b3 = A[2][1];
  double c3 = A[2][2];

  // Compute the adjoint
  double d1 = vtkMath::Determinant2x2(b2, b3, c2, c3);
  double d2 = -vtkMath::Determinant2x2(a2, a3, c2, c3);
  double d3 = vtkMath::Determinant2x2(a2, a3, b2, b3);

  double e1 = -vtkMath::Determinant2x2(b1, b3, c1, c3);
  double e2 = vtkMath::Determinant2x2(a1, a3, c1, c3);
  double e3 = -vtkMath::Determinant2x2(a1, a3, b1, b3);

  double f1 = vtkMath::Determinant2x2(b1, b2, c1, c2);
  double f2 = -vtkMath::Determinant2x2(a1, a2, c1, c2);
  double f3 = vtkMath::Determinant2x2(a1, a2, b1, b2);

  // Divide by the determinant
  double det = a1 * d1 + b1 * d2 + c1 * d3;

  AI[0][0] = d1 / det;
  AI[1][0] = d2 / det;
  AI[2][0] = d3 / det;

  AI[0][1] = e1 / det;
  AI[1][1] = e2 / det;
  AI[2][1] = e3 / det;

  AI[0][2] = f1 / det;
  AI[1][2] = f2 / det;
  AI[2][2] = f3 / det;
}

//------------------------------------------------------------------------------
void vtkMath::Invert3x3(const float A[3][3], float AI[3][3])
{
  vtkInvert3x3(A, AI);
}

//------------------------------------------------------------------------------
void vtkMath::Invert3x3(const double A[3][3], double AI[3][3])
{
  vtkInvert3x3(A, AI);
}

//------------------------------------------------------------------------------
template <class T>
inline void vtkIdentity3x3(T A[3][3])
{
  for (int i = 0; i < 3; ++i)
  {
    A[i][0] = A[i][1] = A[i][2] = T(0.0);
    A[i][i] = 1.0;
  }
}

//------------------------------------------------------------------------------
void vtkMath::Identity3x3(float A[3][3])
{
  vtkIdentity3x3(A);
}

//------------------------------------------------------------------------------
void vtkMath::Identity3x3(double A[3][3])
{
  vtkIdentity3x3(A);
}

//------------------------------------------------------------------------------
// Multiplying two quaternions
template <class T>
inline void vtkQuaternionMultiplication(const T q1[4], const T q2[4], T q[4])
{
  T ww = q1[0] * q2[0];
  T wx = q1[0] * q2[1];
  T wy = q1[0] * q2[2];
  T wz = q1[0] * q2[3];

  T xw = q1[1] * q2[0];
  T xx = q1[1] * q2[1];
  T xy = q1[1] * q2[2];
  T xz = q1[1] * q2[3];

  T yw = q1[2] * q2[0];
  T yx = q1[2] * q2[1];
  T yy = q1[2] * q2[2];
  T yz = q1[2] * q2[3];

  T zw = q1[3] * q2[0];
  T zx = q1[3] * q2[1];
  T zy = q1[3] * q2[2];
  T zz = q1[3] * q2[3];

  q[0] = ww - xx - yy - zz;
  q[1] = wx + xw + yz - zy;
  q[2] = wy - xz + yw + zx;
  q[3] = wz + xy - yx + zw;
}

//------------------------------------------------------------------------------
void vtkMath::MultiplyQuaternion(const float q1[4], const float q2[4], float q[4])
{
  vtkQuaternionMultiplication(q1, q2, q);
}

//------------------------------------------------------------------------------
void vtkMath::MultiplyQuaternion(const double q1[4], const double q2[4], double q[4])
{
  vtkQuaternionMultiplication(q1, q2, q);
}

//----------------------------------------------------------------------------
void vtkMath::RotateVectorByNormalizedQuaternion(const float v[3], const float q[4], float r[3])
{
  float f = std::sqrt(q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
  float a[3];
  if (f != 0.0)
  {
    a[0] = q[1] / f;
    a[1] = q[2] / f;
    a[2] = q[3] / f;

    // atan2() provides a more accurate angle result than acos()
    float t = 2.0 * atan2(f, q[0]);

    float cosT = cos(t);
    float sinT = sin(t);
    float dotKV = a[0] * v[0] + a[1] * v[1] + a[2] * v[2];
    float crossKV[3];
    vtkMath::Cross(a, v, crossKV);

    r[0] = v[0] * cosT + crossKV[0] * sinT + a[0] * dotKV * (1.0 - cosT);
    r[1] = v[1] * cosT + crossKV[1] * sinT + a[1] * dotKV * (1.0 - cosT);
    r[2] = v[2] * cosT + crossKV[2] * sinT + a[2] * dotKV * (1.0 - cosT);
  }
  else
  {
    r[0] = v[0];
    r[1] = v[1];
    r[2] = v[2];
  }
}

void vtkMath::RotateVectorByNormalizedQuaternion(const double v[3], const double q[4], double r[3])
{
  double f = std::sqrt(q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
  double a[3];
  if (f != 0.0)
  {
    a[0] = q[1] / f;
    a[1] = q[2] / f;
    a[2] = q[3] / f;

    // atan2() provides a more accurate angle result than acos()
    double t = 2.0 * atan2(f, q[0]);

    double cosT = cos(t);
    double sinT = sin(t);
    double dotKV = a[0] * v[0] + a[1] * v[1] + a[2] * v[2];
    double crossKV[3];
    vtkMath::Cross(a, v, crossKV);

    r[0] = v[0] * cosT + crossKV[0] * sinT + a[0] * dotKV * (1.0 - cosT);
    r[1] = v[1] * cosT + crossKV[1] * sinT + a[1] * dotKV * (1.0 - cosT);
    r[2] = v[2] * cosT + crossKV[2] * sinT + a[2] * dotKV * (1.0 - cosT);
  }
  else
  {
    r[0] = v[0];
    r[1] = v[1];
    r[2] = v[2];
  }
}

void vtkMath::RotateVectorByWXYZ(const float v[3], const float q[4], float r[3])
{
  float cosT = cos(q[0]);
  float sinT = sin(q[0]);
  float dotKV = q[1] * v[0] + q[2] * v[1] + q[3] * v[2];
  float crossKV[3];
  vtkMath::Cross(&(q[1]), v, crossKV);

  r[0] = v[0] * cosT + crossKV[0] * sinT + q[1] * dotKV * (1.0 - cosT);
  r[1] = v[1] * cosT + crossKV[1] * sinT + q[2] * dotKV * (1.0 - cosT);
  r[2] = v[2] * cosT + crossKV[2] * sinT + q[3] * dotKV * (1.0 - cosT);
}

void vtkMath::RotateVectorByWXYZ(const double v[3], const double q[4], double r[3])
{
  double cosT = cos(q[0]);
  double sinT = sin(q[0]);
  double dotKV = q[1] * v[0] + q[2] * v[1] + q[3] * v[2];
  double crossKV[3];
  vtkMath::Cross(&(q[1]), v, crossKV);

  r[0] = v[0] * cosT + crossKV[0] * sinT + q[1] * dotKV * (1.0 - cosT);
  r[1] = v[1] * cosT + crossKV[1] * sinT + q[2] * dotKV * (1.0 - cosT);
  r[2] = v[2] * cosT + crossKV[2] * sinT + q[3] * dotKV * (1.0 - cosT);
}

//------------------------------------------------------------------------------
//  The orthogonalization is done via quaternions in order to avoid
//  having to use a singular value decomposition algorithm.
template <class T1, class T2>
inline void vtkOrthogonalize3x3(const T1 A[3][3], T2 B[3][3])
{
  int i;

  // copy the matrix
  for (i = 0; i < 3; ++i)
  {
    B[0][i] = A[0][i];
    B[1][i] = A[1][i];
    B[2][i] = A[2][i];
  }

  // Pivot the matrix to improve accuracy
  T2 scale[3];
  int index[3];
  T2 largest;

  // Loop over rows to get implicit scaling information
  for (i = 0; i < 3; ++i)
  {
    T2 x1 = std::abs(B[i][0]);
    T2 x2 = std::abs(B[i][1]);
    T2 x3 = std::abs(B[i][2]);
    largest = (x2 > x1 ? x2 : x1);
    largest = (x3 > largest ? x3 : largest);
    scale[i] = 1;
    if (largest != 0)
    {
      scale[i] /= largest;
    }
  }

  // first column
  T2 x1 = std::abs(B[0][0]) * scale[0];
  T2 x2 = std::abs(B[1][0]) * scale[1];
  T2 x3 = std::abs(B[2][0]) * scale[2];
  index[0] = 0;
  largest = x1;
  if (x2 >= largest)
  {
    largest = x2;
    index[0] = 1;
  }
  if (x3 >= largest)
  {
    index[0] = 2;
  }
  if (index[0] != 0)
  {
    vtkSwapVectors3(B[index[0]], B[0]);
    scale[index[0]] = scale[0];
  }

  // second column
  T2 y2 = std::abs(B[1][1]) * scale[1];
  T2 y3 = std::abs(B[2][1]) * scale[2];
  index[1] = 1;
  largest = y2;
  if (y3 >= largest)
  {
    index[1] = 2;
    vtkSwapVectors3(B[2], B[1]);
  }

  // third column
  index[2] = 2;

  // A quaternion can only describe a pure rotation, not
  // a rotation with a flip, therefore the flip must be
  // removed before the matrix is converted to a quaternion.
  bool flip = false;
  if (vtkDeterminant3x3(B) < 0)
  {
    flip = true;
    for (i = 0; i < 3; ++i)
    {
      B[0][i] = -B[0][i];
      B[1][i] = -B[1][i];
      B[2][i] = -B[2][i];
    }
  }

  // Do orthogonalization using a quaternion intermediate
  // (this, essentially, does the orthogonalization via
  // diagonalization of an appropriately constructed symmetric
  // 4x4 matrix rather than by doing SVD of the 3x3 matrix)
  T2 quat[4];
  vtkMath::Matrix3x3ToQuaternion(B, quat);
  vtkMath::QuaternionToMatrix3x3(quat, B);

  // Put the flip back into the orthogonalized matrix.
  if (flip)
  {
    for (i = 0; i < 3; ++i)
    {
      B[0][i] = -B[0][i];
      B[1][i] = -B[1][i];
      B[2][i] = -B[2][i];
    }
  }

  // Undo the pivoting
  if (index[1] != 1)
  {
    vtkSwapVectors3(B[index[1]], B[1]);
  }
  if (index[0] != 0)
  {
    vtkSwapVectors3(B[index[0]], B[0]);
  }
}

//------------------------------------------------------------------------------
void vtkMath::Orthogonalize3x3(const float A[3][3], float B[3][3])
{
  vtkOrthogonalize3x3(A, B);
}

//------------------------------------------------------------------------------
void vtkMath::Orthogonalize3x3(const double A[3][3], double B[3][3])
{
  vtkOrthogonalize3x3(A, B);
}

//------------------------------------------------------------------------------
float vtkMath::Norm(const float* x, int n)
{
  double sum = 0;
  for (int i = 0; i < n; ++i)
  {
    sum += x[i] * x[i];
  }

  return std::sqrt(sum);
}

//------------------------------------------------------------------------------
double vtkMath::Norm(const double* x, int n)
{
  double sum = 0;
  for (int i = 0; i < n; ++i)
  {
    sum += x[i] * x[i];
  }

  return std::sqrt(sum);
}

//------------------------------------------------------------------------------
bool vtkMath::ProjectVector(const float a[3], const float b[3], float projection[3])
{
  float bSquared = vtkMath::Dot(b, b);

  if (bSquared == 0)
  {
    projection[0] = 0;
    projection[1] = 0;
    projection[2] = 0;
    return false;
  }

  float scale = vtkMath::Dot(a, b) / bSquared;

  for (int i = 0; i < 3; ++i)
  {
    projection[i] = b[i];
  }
  vtkMath::MultiplyScalar(projection, scale);

  return true;
}

//------------------------------------------------------------------------------
bool vtkMath::ProjectVector(const double a[3], const double b[3], double projection[3])
{
  double bSquared = vtkMath::Dot(b, b);

  if (bSquared == 0)
  {
    projection[0] = 0;
    projection[1] = 0;
    projection[2] = 0;
    return false;
  }

  double scale = vtkMath::Dot(a, b) / bSquared;

  for (int i = 0; i < 3; ++i)
  {
    projection[i] = b[i] * scale;
  }

  return true;
}

//------------------------------------------------------------------------------
bool vtkMath::ProjectVector2D(const float a[2], const float b[2], float projection[2])
{
  float bSquared = vtkMath::Dot2D(b, b);

  if (bSquared == 0)
  {
    projection[0] = 0;
    projection[1] = 0;
    return false;
  }

  float scale = vtkMath::Dot2D(a, b) / bSquared;

  for (int i = 0; i < 2; ++i)
  {
    projection[i] = b[i];
  }
  vtkMath::MultiplyScalar2D(projection, scale);

  return true;
}

//------------------------------------------------------------------------------
bool vtkMath::ProjectVector2D(const double a[2], const double b[2], double projection[2])
{
  double bSquared = vtkMath::Dot2D(b, b);

  if (bSquared == 0)
  {
    projection[0] = 0;
    projection[1] = 0;
    return false;
  }

  double scale = vtkMath::Dot2D(a, b) / bSquared;

  for (int i = 0; i < 2; ++i)
  {
    projection[i] = b[i];
  }
  vtkMath::MultiplyScalar2D(projection, scale);

  return true;
}

//------------------------------------------------------------------------------
// Extract the eigenvalues and eigenvectors from a 3x3 matrix.
// The eigenvectors (the columns of V) will be normalized.
// The eigenvectors are aligned optimally with the x, y, and z
// axes respectively.
template <class T1, class T2>
inline void vtkDiagonalize3x3(const T1 A[3][3], T2 w[3], T2 V[3][3])
{
  int i, j, k, maxI;
  T2 tmp, maxVal;

  // do the matrix[3][3] to **matrix conversion for Jacobi
  T2 C[3][3];
  T2 *ATemp[3], *VTemp[3];
  for (i = 0; i < 3; ++i)
  {
    C[i][0] = A[i][0];
    C[i][1] = A[i][1];
    C[i][2] = A[i][2];
    ATemp[i] = C[i];
    VTemp[i] = V[i];
  }

  // diagonalize using Jacobi
  vtkMath::JacobiN(ATemp, 3, w, VTemp);

  // if all the eigenvalues are the same, return identity matrix
  if (w[0] == w[1] && w[0] == w[2])
  {
    vtkMath::Identity3x3(V);
    return;
  }

  // transpose temporarily, it makes it easier to sort the eigenvectors
  vtkMath::Transpose3x3(V, V);

  // if two eigenvalues are the same, re-orthogonalize to optimally line
  // up the eigenvectors with the x, y, and z axes
  for (i = 0; i < 3; ++i)
  {
    if (w[(i + 1) % 3] == w[(i + 2) % 3]) // two eigenvalues are the same
    {
      // find maximum element of the independent eigenvector
      maxVal = std::abs(V[i][0]);
      maxI = 0;
      for (j = 1; j < 3; ++j)
      {
        if (maxVal < (tmp = std::abs(V[i][j])))
        {
          maxVal = tmp;
          maxI = j;
        }
      }
      // swap the eigenvector into its proper position
      if (maxI != i)
      {
        tmp = w[maxI];
        w[maxI] = w[i];
        w[i] = tmp;
        vtkSwapVectors3(V[i], V[maxI]);
      }
      // maximum element of eigenvector should be positive
      if (V[maxI][maxI] < 0)
      {
        V[maxI][0] = -V[maxI][0];
        V[maxI][1] = -V[maxI][1];
        V[maxI][2] = -V[maxI][2];
      }

      // re-orthogonalize the other two eigenvectors
      j = (maxI + 1) % 3;
      k = (maxI + 2) % 3;

      V[j][0] = 0.0;
      V[j][1] = 0.0;
      V[j][2] = 0.0;
      V[j][j] = 1.0;
      vtkMath::Cross(V[maxI], V[j], V[k]);
      vtkMath::Normalize(V[k]);
      vtkMath::Cross(V[k], V[maxI], V[j]);

      // transpose vectors back to columns
      vtkMath::Transpose3x3(V, V);
      return;
    }
  }

  // the three eigenvalues are different, just sort the eigenvectors
  // to align them with the x, y, and z axes

  // find the vector with the largest x element, make that vector
  // the first vector
  maxVal = std::abs(V[0][0]);
  maxI = 0;
  for (i = 1; i < 3; ++i)
  {
    if (maxVal < (tmp = std::abs(V[i][0])))
    {
      maxVal = tmp;
      maxI = i;
    }
  }
  // swap eigenvalue and eigenvector
  if (maxI != 0)
  {
    tmp = w[maxI];
    w[maxI] = w[0];
    w[0] = tmp;
    vtkSwapVectors3(V[maxI], V[0]);
  }
  // do the same for the y element
  if (std::abs(V[1][1]) < std::abs(V[2][1]))
  {
    tmp = w[2];
    w[2] = w[1];
    w[1] = tmp;
    vtkSwapVectors3(V[2], V[1]);
  }

  // ensure that the sign of the eigenvectors is correct
  for (i = 0; i < 2; ++i)
  {
    if (V[i][i] < 0)
    {
      V[i][0] = -V[i][0];
      V[i][1] = -V[i][1];
      V[i][2] = -V[i][2];
    }
  }
  // set sign of final eigenvector to ensure that determinant is positive
  if (vtkMath::Determinant3x3(V) < 0)
  {
    V[2][0] = -V[2][0];
    V[2][1] = -V[2][1];
    V[2][2] = -V[2][2];
  }

  // transpose the eigenvectors back again
  vtkMath::Transpose3x3(V, V);
}

//------------------------------------------------------------------------------
void vtkMath::Diagonalize3x3(const float A[3][3], float w[3], float V[3][3])
{
  vtkDiagonalize3x3(A, w, V);
}

//------------------------------------------------------------------------------
void vtkMath::Diagonalize3x3(const double A[3][3], double w[3], double V[3][3])
{
  vtkDiagonalize3x3(A, w, V);
}

//------------------------------------------------------------------------------
// Perform singular value decomposition on the matrix A:
//    A = U * W * VT
// where U and VT are orthogonal W is diagonal (the diagonal elements
// are returned in vector w).
// The matrices U and VT will both have positive determinants.
// The scale factors w are ordered according to how well the
// corresponding eigenvectors (in VT) match the x, y and z axes
// respectively.
//
// The singular value decomposition is used to decompose a linear
// transformation into a rotation, followed by a scale, followed
// by a second rotation.  The scale factors w will be negative if
// the determinant of matrix A is negative.
//
// Contributed by David Gobbi (dgobbi@irus.rri.on.ca)
template <class T1, class T2>
inline void vtkSingularValueDecomposition3x3(const T1 A[3][3], T2 U[3][3], T2 w[3], T2 VT[3][3])
{
  int i;
  T2 B[3][3];

  // copy so that A can be used for U or VT without risk
  for (i = 0; i < 3; ++i)
  {
    B[0][i] = A[0][i];
    B[1][i] = A[1][i];
    B[2][i] = A[2][i];
  }

  // temporarily flip if determinant is negative
  T2 d = vtkMath::Determinant3x3(B);
  if (d < 0)
  {
    for (i = 0; i < 3; ++i)
    {
      B[0][i] = -B[0][i];
      B[1][i] = -B[1][i];
      B[2][i] = -B[2][i];
    }
  }

  // orthogonalize, diagonalize, etc.
  vtkMath::Orthogonalize3x3(B, U);
  vtkMath::Transpose3x3(B, B);
  vtkMath::Multiply3x3(B, U, VT);
  vtkMath::Diagonalize3x3(VT, w, VT);
  vtkMath::Multiply3x3(U, VT, U);
  vtkMath::Transpose3x3(VT, VT);

  // re-create the flip
  if (d < 0)
  {
    w[0] = -w[0];
    w[1] = -w[1];
    w[2] = -w[2];
  }

  /* paranoia check: recombine to ensure that the SVD is correct
  vtkMath::Transpose3x3(B, B);

  if (d < 0)
  {
    for (i = 0; i < 3; ++i)
    {
      B[0][i] = -B[0][i];
      B[1][i] = -B[1][i];
      B[2][i] = -B[2][i];
    }
  }

  int j;
  T2 maxerr = 0;
  T2 tmp;
  T2 M[3][3];
  T2 W[3][3];
  vtkMath::Identity3x3(W);
  W[0][0] = w[0]; W[1][1] = w[1]; W[2][2] = w[2];
  vtkMath::Identity3x3(M);
  vtkMath::Multiply3x3(M, U, M);
  vtkMath::Multiply3x3(M, W, M);
  vtkMath::Multiply3x3(M, VT, M);

  for (i = 0; i < 3; ++i)
  {
    for (j = 0; j < 3; ++j)
    {
      if ((tmp = std::abs(B[i][j] - M[i][j])) > maxerr)
      {
        maxerr = tmp;
      }
    }
  }

  vtkGenericWarningMacro("SingularValueDecomposition max error = " << maxerr);
  */
}

//------------------------------------------------------------------------------
void vtkMath::SingularValueDecomposition3x3(
  const float A[3][3], float U[3][3], float w[3], float VT[3][3])
{
  vtkSingularValueDecomposition3x3(A, U, w, VT);
}

//------------------------------------------------------------------------------
void vtkMath::SingularValueDecomposition3x3(
  const double A[3][3], double U[3][3], double w[3], double VT[3][3])
{
  vtkSingularValueDecomposition3x3(A, U, w, VT);
}

//------------------------------------------------------------------------------
void vtkMath::RGBToHSV(float r, float g, float b, float* h, float* s, float* v)
{
  double dh, ds, dv;
  vtkMath::RGBToHSV(r, g, b, &dh, &ds, &dv);
  *h = static_cast<float>(dh);
  *s = static_cast<float>(ds);
  *v = static_cast<float>(dv);
}

//------------------------------------------------------------------------------
void vtkMath::RGBToHSV(double r, double g, double b, double* h, double* s, double* v)
{
  const double onethird = 1.0 / 3.0;
  const double onesixth = 1.0 / 6.0;
  const double twothird = 2.0 / 3.0;

  double cmax = r;
  double cmin = r;
  if (g > cmax)
  {
    cmax = g;
  }
  else if (g < cmin)
  {
    cmin = g;
  }
  if (b > cmax)
  {
    cmax = b;
  }
  else if (b < cmin)
  {
    cmin = b;
  }
  *v = cmax;

  if (*v > 0.0)
  {
    *s = (cmax - cmin) / cmax;
  }
  else
  {
    *s = 0.0;
  }
  if (*s > 0)
  {
    if (r == cmax)
    {
      *h = onesixth * (g - b) / (cmax - cmin);
    }
    else if (g == cmax)
    {
      *h = onethird + onesixth * (b - r) / (cmax - cmin);
    }
    else
    {
      *h = twothird + onesixth * (r - g) / (cmax - cmin);
    }
    if (*h < 0.0)
    {
      *h += 1.0;
    }
  }
  else
  {
    *h = 0.0;
  }
}

//------------------------------------------------------------------------------
void vtkMath::HSVToRGB(float h, float s, float v, float* r, float* g, float* b)
{
  double dr, dg, db;
  vtkMath::HSVToRGB(h, s, v, &dr, &dg, &db);
  *r = static_cast<float>(dr);
  *g = static_cast<float>(dg);
  *b = static_cast<float>(db);
}

//------------------------------------------------------------------------------
void vtkMath::HSVToRGB(double h, double s, double v, double* r, double* g, double* b)
{
  const double onethird = 1.0 / 3.0;
  const double onesixth = 1.0 / 6.0;
  const double twothird = 2.0 / 3.0;
  const double fivesixth = 5.0 / 6.0;

  // compute RGB from HSV
  if (h > onesixth && h <= onethird) // green/red
  {
    *g = 1.0;
    *r = (onethird - h) / onesixth;
    *b = 0.0;
  }
  else if (h > onethird && h <= 0.5) // green/blue
  {
    *g = 1.0;
    *b = (h - onethird) / onesixth;
    *r = 0.0;
  }
  else if (h > 0.5 && h <= twothird) // blue/green
  {
    *b = 1.0;
    *g = (twothird - h) / onesixth;
    *r = 0.0;
  }
  else if (h > twothird && h <= fivesixth) // blue/red
  {
    *b = 1.0;
    *r = (h - twothird) / onesixth;
    *g = 0.0;
  }
  else if (h > fivesixth && h <= 1.0) // red/blue
  {
    *r = 1.0;
    *b = (1.0 - h) / onesixth;
    *g = 0.0;
  }
  else // red/green
  {
    *r = 1.0;
    *g = h / onesixth;
    *b = 0.0;
  }

  // add Saturation to the equation.
  *r = (s * *r + (1.0 - s));
  *g = (s * *g + (1.0 - s));
  *b = (s * *b + (1.0 - s));

  *r *= v;
  *g *= v;
  *b *= v;
}

//------------------------------------------------------------------------------
void vtkMath::LabToXYZ(double L, double a, double b, double* x, double* y, double* z)
{
  // LAB to XYZ
  double var_Y = (L + 16) / 116;
  double var_X = a / 500 + var_Y;
  double var_Z = var_Y - b / 200;

  if (pow(var_Y, 3) > 0.008856)
  {
    var_Y = pow(var_Y, 3);
  }
  else
  {
    var_Y = (var_Y - 16.0 / 116.0) / 7.787;
  }

  if (pow(var_X, 3) > 0.008856)
  {
    var_X = pow(var_X, 3);
  }
  else
  {
    var_X = (var_X - 16.0 / 116.0) / 7.787;
  }

  if (pow(var_Z, 3) > 0.008856)
  {
    var_Z = pow(var_Z, 3);
  }
  else
  {
    var_Z = (var_Z - 16.0 / 116.0) / 7.787;
  }
  const double ref_X = 0.9505;
  const double ref_Y = 1.000;
  const double ref_Z = 1.089;
  *x = ref_X * var_X; // ref_X = 0.9505  Observer= 2 deg Illuminant= D65
  *y = ref_Y * var_Y; // ref_Y = 1.000
  *z = ref_Z * var_Z; // ref_Z = 1.089
}

//------------------------------------------------------------------------------
void vtkMath::XYZToLab(double x, double y, double z, double* L, double* a, double* b)
{
  const double ref_X = 0.9505;
  const double ref_Y = 1.000;
  const double ref_Z = 1.089;
  double var_X = x / ref_X; // ref_X = 0.9505  Observer= 2 deg, Illuminant= D65
  double var_Y = y / ref_Y; // ref_Y = 1.000
  double var_Z = z / ref_Z; // ref_Z = 1.089

  if (var_X > 0.008856)
  {
    var_X = pow(var_X, 1.0 / 3.0);
  }
  else
  {
    var_X = (7.787 * var_X) + (16.0 / 116.0);
  }
  if (var_Y > 0.008856)
  {
    var_Y = pow(var_Y, 1.0 / 3.0);
  }
  else
  {
    var_Y = (7.787 * var_Y) + (16.0 / 116.0);
  }
  if (var_Z > 0.008856)
  {
    var_Z = pow(var_Z, 1.0 / 3.0);
  }
  else
  {
    var_Z = (7.787 * var_Z) + (16.0 / 116.0);
  }

  *L = (116 * var_Y) - 16;
  *a = 500 * (var_X - var_Y);
  *b = 200 * (var_Y - var_Z);
}

//------------------------------------------------------------------------------
void vtkMath::XYZToRGB(double x, double y, double z, double* r, double* g, double* b)
{
  // double ref_X = 0.9505;        //Observer = 2 deg Illuminant = D65
  // double ref_Y = 1.000;
  // double ref_Z = 1.089;

  *r = x * 3.2406 + y * -1.5372 + z * -0.4986;
  *g = x * -0.9689 + y * 1.8758 + z * 0.0415;
  *b = x * 0.0557 + y * -0.2040 + z * 1.0570;

  // The following performs a "gamma correction" specified by the sRGB color
  // space.  sRGB is defined by a canonical definition of a display monitor and
  // has been standardized by the International Electrotechnical Commission (IEC
  // 61966-2-1).  The nonlinearity of the correction is designed to make the
  // colors more perceptually uniform.  This color space has been adopted by
  // several applications including Adobe Photoshop and Microsoft Windows color
  // management.  OpenGL is agnostic on its RGB color space, but it is reasonable
  // to assume it is close to this one.
  if (*r > 0.0031308)
  {
    *r = 1.055 * (pow(*r, (1 / 2.4))) - 0.055;
  }
  else
  {
    *r = 12.92 * (*r);
  }
  if (*g > 0.0031308)
  {
    *g = 1.055 * (pow(*g, (1 / 2.4))) - 0.055;
  }
  else
  {
    *g = 12.92 * (*g);
  }
  if (*b > 0.0031308)
  {
    *b = 1.055 * (pow(*b, (1 / 2.4))) - 0.055;
  }
  else
  {
    *b = 12.92 * (*b);
  }

  // Clip colors. ideally we would do something that is perceptually closest
  // (since we can see colors outside of the display gamut), but this seems to
  // work well enough.
  double maxVal = *r;
  if (maxVal < *g)
  {
    maxVal = *g;
  }
  if (maxVal < *b)
  {
    maxVal = *b;
  }
  if (maxVal > 1.0)
  {
    *r /= maxVal;
    *g /= maxVal;
    *b /= maxVal;
  }
  if (*r < 0)
  {
    *r = 0;
  }
  if (*g < 0)
  {
    *g = 0;
  }
  if (*b < 0)
  {
    *b = 0;
  }
}

//------------------------------------------------------------------------------
void vtkMath::RGBToXYZ(double r, double g, double b, double* x, double* y, double* z)
{
  // The following performs a "gamma correction" specified by the sRGB color
  // space.  sRGB is defined by a canonical definition of a display monitor and
  // has been standardized by the International Electrotechnical Commission (IEC
  // 61966-2-1).  The nonlinearity of the correction is designed to make the
  // colors more perceptually uniform.  This color space has been adopted by
  // several applications including Adobe Photoshop and Microsoft Windows color
  // management.  OpenGL is agnostic on its RGB color space, but it is reasonable
  // to assume it is close to this one.
  if (r > 0.04045)
  {
    r = pow((r + 0.055) / 1.055, 2.4);
  }
  else
  {
    r = r / 12.92;
  }
  if (g > 0.04045)
  {
    g = pow((g + 0.055) / 1.055, 2.4);
  }
  else
  {
    g = g / 12.92;
  }
  if (b > 0.04045)
  {
    b = pow((b + 0.055) / 1.055, 2.4);
  }
  else
  {
    b = b / 12.92;
  }

  // Observer. = 2 deg, Illuminant = D65
  *x = r * 0.4124 + g * 0.3576 + b * 0.1805;
  *y = r * 0.2126 + g * 0.7152 + b * 0.0722;
  *z = r * 0.0193 + g * 0.1192 + b * 0.9505;
}

//------------------------------------------------------------------------------
void vtkMath::RGBToLab(double red, double green, double blue, double* L, double* a, double* b)
{
  double x, y, z;
  vtkMath::RGBToXYZ(red, green, blue, &x, &y, &z);
  vtkMath::XYZToLab(x, y, z, L, a, b);
}

//------------------------------------------------------------------------------
void vtkMath::LabToRGB(double L, double a, double b, double* red, double* green, double* blue)
{
  double x, y, z;
  vtkMath::LabToXYZ(L, a, b, &x, &y, &z);
  vtkMath::XYZToRGB(x, y, z, red, green, blue);
}

//------------------------------------------------------------------------------
void vtkMath::ClampValues(double* values, int nb_values, const double range[2])
{
  if (!values || nb_values <= 0 || !range)
  {
    return;
  }

  const double* values_end = values + nb_values;
  while (values < values_end)
  {
    *values = vtkMath::ClampValue(*values, range[0], range[1]);
    ++values;
  }
}

//------------------------------------------------------------------------------
void vtkMath::ClampValues(
  const double* values, int nb_values, const double range[2], double* clamped_values)
{
  if (!values || nb_values <= 0 || !range || !clamped_values)
  {
    return;
  }

  const double* values_end = values + nb_values;
  while (values < values_end)
  {
    *clamped_values = vtkMath::ClampValue(*values, range[0], range[1]);
    ++values;
    ++clamped_values;
  }
}

//------------------------------------------------------------------------------
int vtkMath::GetScalarTypeFittingRange(
  double range_min, double range_max, double scale, double shift)
{
  class TypeRange
  {
  public:
    int Type;
    double Min;
    double Max;
  };

  const TypeRange FloatTypes[] = { { VTK_FLOAT, VTK_FLOAT_MIN, VTK_FLOAT_MAX },
    { VTK_DOUBLE, VTK_DOUBLE_MIN, VTK_DOUBLE_MAX } };

  const TypeRange IntTypes[] = { { VTK_BIT, VTK_BIT_MIN, VTK_BIT_MAX },
    { VTK_CHAR, VTK_CHAR_MIN, VTK_CHAR_MAX },
    { VTK_SIGNED_CHAR, VTK_SIGNED_CHAR_MIN, VTK_SIGNED_CHAR_MAX },
    { VTK_UNSIGNED_CHAR, VTK_UNSIGNED_CHAR_MIN, VTK_UNSIGNED_CHAR_MAX },
    { VTK_SHORT, VTK_SHORT_MIN, VTK_SHORT_MAX },
    { VTK_UNSIGNED_SHORT, VTK_UNSIGNED_SHORT_MIN, VTK_UNSIGNED_SHORT_MAX },
    { VTK_INT, VTK_INT_MIN, VTK_INT_MAX },
    { VTK_UNSIGNED_INT, VTK_UNSIGNED_INT_MIN, VTK_UNSIGNED_INT_MAX },
    { VTK_LONG, static_cast<double>(VTK_LONG_MIN), static_cast<double>(VTK_LONG_MAX) },
    { VTK_UNSIGNED_LONG, static_cast<double>(VTK_UNSIGNED_LONG_MIN),
      static_cast<double>(VTK_UNSIGNED_LONG_MAX) },
    { VTK_LONG_LONG, static_cast<double>(VTK_LONG_LONG_MIN),
      static_cast<double>(VTK_LONG_LONG_MAX) },
    { VTK_UNSIGNED_LONG_LONG, static_cast<double>(VTK_UNSIGNED_LONG_LONG_MIN),
      static_cast<double>(VTK_UNSIGNED_LONG_LONG_MAX) } };

  // If the range, scale or shift are decimal number, just browse
  // the decimal types

  double intpart;

  int range_min_is_int = (modf(range_min, &intpart) == 0.0);
  int range_max_is_int = (modf(range_max, &intpart) == 0.0);
  int scale_is_int = (modf(scale, &intpart) == 0.0);
  int shift_is_int = (modf(shift, &intpart) == 0.0);

  range_min = range_min * scale + shift;
  range_max = range_max * scale + shift;

  if (range_min_is_int && range_max_is_int && scale_is_int && shift_is_int)
  {
    for (unsigned int i = 0; i < sizeof(IntTypes) / sizeof(TypeRange); ++i)
    {
      if (IntTypes[i].Min <= range_min && range_max <= IntTypes[i].Max)
      {
        return IntTypes[i].Type;
      }
    }
  }

  for (unsigned int i = 0; i < sizeof(FloatTypes) / sizeof(TypeRange); ++i)
  {
    if (FloatTypes[i].Min <= range_min && range_max <= FloatTypes[i].Max)
    {
      return FloatTypes[i].Type;
    }
  }

  return -1;
}

//------------------------------------------------------------------------------
vtkTypeBool vtkMath::GetAdjustedScalarRange(vtkDataArray* array, int comp, double range[2])
{
  if (!array || comp < 0 || comp >= array->GetNumberOfComponents())
  {
    return 0;
  }

  array->GetRange(range, comp);

  switch (array->GetDataType())
  {
    case VTK_UNSIGNED_CHAR:
      range[0] = static_cast<double>(array->GetDataTypeMin());
      range[1] = static_cast<double>(array->GetDataTypeMax());
      break;

    case VTK_UNSIGNED_SHORT:
      range[0] = static_cast<double>(array->GetDataTypeMin());
      if (range[1] <= 4095.0)
      {
        if (range[1] > VTK_UNSIGNED_CHAR_MAX)
        {
          range[1] = 4095.0;
        }
      }
      else
      {
        range[1] = static_cast<double>(array->GetDataTypeMax());
      }
      break;
    default:
      assert("check: impossible case." && 0); // reaching this line is a bug.
      break;
  }

  return 1;
}

//------------------------------------------------------------------------------
vtkTypeBool vtkMath::ExtentIsWithinOtherExtent(const int extent1[6], const int extent2[6])
{
  if (!extent1 || !extent2)
  {
    return 0;
  }

  for (int i = 0; i < 6; i += 2)
  {
    if (extent1[i] < extent2[i] || extent1[i] > extent2[i + 1] || extent1[i + 1] < extent2[i] ||
      extent1[i + 1] > extent2[i + 1])
    {
      return 0;
    }
  }

  return 1;
}

//------------------------------------------------------------------------------

vtkTypeBool vtkMath::BoundsIsWithinOtherBounds(
  const double bounds1[6], const double bounds2[6], const double delta[3])
{
  if (!bounds1 || !bounds2)
  {
    return 0;
  }
  for (int i = 0; i < 6; i += 2)
  {
    if (bounds1[i] + delta[i / 2] < bounds2[i] || bounds1[i] - delta[i / 2] > bounds2[i + 1] ||
      bounds1[i + 1] + delta[i / 2] < bounds2[i] || bounds1[i + 1] - delta[i / 2] > bounds2[i + 1])
    {
      return 0;
    }
  }
  return 1;
}

//------------------------------------------------------------------------------
vtkTypeBool vtkMath::PointIsWithinBounds(
  const double point[3], const double bounds[6], const double delta[3])
{
  if (!point || !bounds || !delta)
  {
    return 0;
  }
  return /*i = 0*/ point[0] + delta[0] >= bounds[0] && point[0] - delta[0] <= bounds[1] &&
    /*i = 1*/ point[1] + delta[1] >= bounds[2] && point[1] - delta[1] <= bounds[3] &&
    /*i = 2*/ point[2] + delta[2] >= bounds[4] && point[2] - delta[2] <= bounds[5];
}

//------------------------------------------------------------------------------
int vtkMath::PlaneIntersectsAABB(
  const double bounds[6], const double normal[3], const double point[3])
{
  if (!bounds || !point || !normal)
  {
    return -2;
  }

  double nPoint[3];
  double pPoint[3];

  // X Component
  if (normal[0] >= 0)
  {
    nPoint[0] = bounds[0];
    pPoint[0] = bounds[1];
  }
  else
  {
    nPoint[0] = bounds[1];
    pPoint[0] = bounds[0];
  }

  // Y Component
  if (normal[1] >= 0)
  {
    nPoint[1] = bounds[2];
    pPoint[1] = bounds[3];
  }
  else
  {
    nPoint[1] = bounds[3];
    pPoint[1] = bounds[2];
  }

  // Z Component
  if (normal[2] >= 0)
  {
    nPoint[2] = bounds[4];
    pPoint[2] = bounds[5];
  }
  else
  {
    nPoint[2] = bounds[5];
    pPoint[2] = bounds[4];
  }

  // Compute distances from nPoint/pPoint to the plane
  // Distance = unit_N  *  (P_x - P_plane)
  //          = a * px_1 + b * px_2 + c * px_3 - d
  double const d = vtkMath::Dot(normal, point);

  if ((nPoint[0] * normal[0] + nPoint[1] * normal[1] + nPoint[2] * normal[2] - d) > 0)
  {
    return 1;
  }
  else if ((pPoint[0] * normal[0] + pPoint[1] * normal[1] + pPoint[2] * normal[2] - d) < 0)
  {
    return -1;
  }

  return 0;
}

//------------------------------------------------------------------------------
double vtkMath::AngleBetweenVectors(const double v1[3], const double v2[3])
{
  double cross[3];
  vtkMath::Cross(v1, v2, cross);
  return atan2(vtkMath::Norm(cross), vtkMath::Dot(v1, v2));
}

//------------------------------------------------------------------------------
double vtkMath::SignedAngleBetweenVectors(
  const double v1[3], const double v2[3], const double vn[3])
{
  double cross[3];
  vtkMath::Cross(v1, v2, cross);
  double angle = atan2(vtkMath::Norm(cross), vtkMath::Dot(v1, v2));
  return vtkMath::Dot(cross, vn) >= 0 ? angle : -angle;
}

//------------------------------------------------------------------------------
double vtkMath::GaussianAmplitude(double variance, double distanceFromMean)
{
  return 1. / (std::sqrt(2. * vtkMath::Pi() * variance)) *
    exp(-(pow(distanceFromMean, 2)) / (2. * variance));
}

//------------------------------------------------------------------------------
double vtkMath::GaussianAmplitude(double mean, double variance, double position)
{
  double distanceToMean = std::abs(mean - position);
  return GaussianAmplitude(variance, distanceToMean);
}

//------------------------------------------------------------------------------
double vtkMath::GaussianWeight(double variance, double distanceFromMean)
{
  return exp(-(pow(distanceFromMean, 2)) / (2. * variance));
}

//------------------------------------------------------------------------------
double vtkMath::GaussianWeight(double mean, double variance, double position)
{
  double distanceToMean = std::abs(mean - position);
  return GaussianWeight(variance, distanceToMean);
}

//------------------------------------------------------------------------------
double vtkMath::Solve3PointCircle(
  const double p1[3], const double p2[3], const double p3[3], double center[3])
{
  double v21[3], v32[3], v13[3];
  double v12[3], v23[3], v31[3];
  for (int i = 0; i < 3; ++i)
  {
    v21[i] = p1[i] - p2[i];
    v32[i] = p2[i] - p3[i];
    v13[i] = p3[i] - p1[i];
    v12[i] = -v21[i];
    v23[i] = -v32[i];
    v31[i] = -v13[i];
  }

  double norm12 = vtkMath::Norm(v12);
  double norm23 = vtkMath::Norm(v23);
  double norm13 = vtkMath::Norm(v13);

  double crossv21v32[3];
  vtkMath::Cross(v21, v32, crossv21v32);
  double normCross = vtkMath::Norm(crossv21v32);

  double radius = (norm12 * norm23 * norm13) / (2. * normCross);

  double alpha = ((norm23 * norm23) * vtkMath::Dot(v21, v31)) / (2. * normCross * normCross);
  double beta = ((norm13 * norm13) * vtkMath::Dot(v12, v32)) / (2. * normCross * normCross);
  double gamma = ((norm12 * norm12) * vtkMath::Dot(v13, v23)) / (2. * normCross * normCross);

  for (int i = 0; i < 3; ++i)
  {
    center[i] = alpha * p1[i] + beta * p2[i] + gamma * p3[i];
  }
  return radius;
}

//------------------------------------------------------------------------------
void vtkMath::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "Seed: " << vtkMath::Internal->Uniform->GetSeed() << "\n";
}

//------------------------------------------------------------------------------
double vtkMath::Inf()
{
  return std::numeric_limits<double>::infinity();
}

//------------------------------------------------------------------------------
double vtkMath::NegInf()
{
  return -std::numeric_limits<double>::infinity();
}

//------------------------------------------------------------------------------
double vtkMath::Nan()
{
  return std::numeric_limits<double>::quiet_NaN();
}

//------------------------------------------------------------------------------
#ifndef VTK_MATH_ISINF_IS_INLINE
vtkTypeBool vtkMath::IsInf(double x)
{
  return (!vtkMath::IsNan(x) && !((x < vtkMath::Inf()) && (x > vtkMath::NegInf())));
}
#endif

//------------------------------------------------------------------------------
#ifndef VTK_MATH_ISNAN_IS_INLINE
vtkTypeBool vtkMath::IsNan(double x)
{
  return !((x <= 0.0) || (x >= 0.0));
}
#endif

//------------------------------------------------------------------------------
#ifndef VTK_MATH_ISFINITE_IS_INLINE
bool vtkMath::IsFinite(double x)
{
  return !vtkMath::IsNan(x) && !vtkMath::IsInf(x);
}
#endif

//------------------------------------------------------------------------------
int vtkMath::QuadraticRoot(double a, double b, double c, double min, double max, double* u)
{
  if (a == 0.0) // then its close to 0
  {
    if (b != 0.0) // not close to 0
    {
      u[0] = -c / b;
      if (u[0] > min && u[0] < max) // its in the interval
      {
        return 1; // 1 soln found
      }
      else // its not in the interval
      {
        return 0;
      }
    }
    else
    {
      return 0;
    }
  }
  double d = b * b - 4 * a * c; // discriminant
  if (d <= 0.0)                 // single or no root
  {
    if (d == 0.0) // close to 0
    {
      u[0] = -b / a;
      if (u[0] > min && u[0] < max) // its in the interval
      {
        return 1;
      }
      else // its not in the interval
      {
        return 0;
      }
    }
    else // no root d must be below 0
    {
      return 0;
    }
  }
  double q = -0.5 * (b + copysign(sqrt(d), b));
  u[0] = c / q;
  u[1] = q / a;

  if ((u[0] > min && u[0] < max) && (u[1] > min && u[1] < max))
  {
    return 2;
  }
  else if (u[0] > min && u[0] < max) // then one wasn't in interval
  {
    return 1;
  }
  else if (u[1] > min && u[1] < max)
  { // make it easier, make u[0] be the valid one always
    std::swap(u[0], u[1]);
    return 1;
  }
  return 0;
}
VTK_ABI_NAMESPACE_END
