///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2002-2012, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
// 
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// *       Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// *       Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
// *       Neither the name of Industrial Light & Magic nor the names of
// its contributors may be used to endorse or promote products derived
// from this software without specific prior written permission. 
// 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////





//----------------------------------------------------------------------------
//
//	Implementation of non-template items declared in ImathMatrixAlgo.h
//
//----------------------------------------------------------------------------

#include "ImathMatrixAlgo.h"
#include <cmath>
#include <algorithm>

#if defined(OPENEXR_DLL)
    #define EXPORT_CONST __declspec(dllexport)
#else
    #define EXPORT_CONST const
#endif

IMATH_INTERNAL_NAMESPACE_SOURCE_ENTER

EXPORT_CONST M33f identity33f ( 1, 0, 0,
				0, 1, 0,
				0, 0, 1);

EXPORT_CONST M33d identity33d ( 1, 0, 0,
				0, 1, 0,
				0, 0, 1);

EXPORT_CONST M44f identity44f ( 1, 0, 0, 0,
				0, 1, 0, 0,
				0, 0, 1, 0,
				0, 0, 0, 1);

EXPORT_CONST M44d identity44d ( 1, 0, 0, 0,
				0, 1, 0, 0,
				0, 0, 1, 0,
				0, 0, 0, 1);

namespace
{

class KahanSum
{
public:
    KahanSum() : _total(0), _correction(0) {}

    void
    operator+= (const double val)
    {
        const double y = val - _correction;
        const double t = _total + y;
        _correction = (t - _total) - y;
        _total = t;
    }

    double get() const
    {
        return _total;
    }

private:
    double _total;
    double _correction;
};

}

template <typename T>
M44d
procrustesRotationAndTranslation (const Vec3<T>* A, const Vec3<T>* B, const T* weights, const size_t numPoints, const bool doScale)
{
    if (numPoints == 0)
        return M44d();

    // Always do the accumulation in double precision:
    V3d Acenter (0.0);
    V3d Bcenter (0.0);
    double weightsSum = 0.0;

    if (weights == 0)
    {
        for (int i = 0; i < numPoints; ++i)
        {
            Acenter += (V3d) A[i];
            Bcenter += (V3d) B[i];
        }
        weightsSum = (double) numPoints;
    }
    else
    {
        for (int i = 0; i < numPoints; ++i)
        {
            const double w = weights[i];
            weightsSum += w;

            Acenter += w * (V3d) A[i];
            Bcenter += w * (V3d) B[i];
        }
    }

    if (weightsSum == 0)
        return M44d();

    Acenter /= weightsSum;
    Bcenter /= weightsSum;

    //
    // Find Q such that |Q*A - B|  (actually A-Acenter and B-Bcenter, weighted)
    // is minimized in the least squares sense.
    // From Golub/Van Loan, p.601
    //
    // A,B are 3xn
    // Let C = B A^T   (where A is 3xn and B^T is nx3, so C is 3x3)
    // Compute the SVD: C = U D V^T  (U,V rotations, D diagonal).
    // Throw away the D part, and return Q = U V^T
    M33d C (0.0);
    if (weights == 0)
    {
        for (int i = 0; i < numPoints; ++i)
            C += outerProduct ((V3d) B[i] - Bcenter, (V3d) A[i] - Acenter);
    }
    else
    {
        for (int i = 0; i < numPoints; ++i)
        {
            const double w = weights[i];
            C += outerProduct (w * ((V3d) B[i] - Bcenter), (V3d) A[i] - Acenter);
        }
    }

    M33d U, V;
    V3d S;
    jacobiSVD (C, U, S, V, IMATH_INTERNAL_NAMESPACE::limits<double>::epsilon(), true);

    // We want Q.transposed() here since we are going to be using it in the
    // Imath style (multiplying vectors on the right, v' = v*A^T):
    const M33d Qt = V * U.transposed();

    double s = 1.0;
    if (doScale && numPoints > 1)
    {
        // Finding a uniform scale: let us assume the Q is completely fixed
        // at this point (solving for both simultaneously seems much harder).  
        // We are trying to compute (again, per Golub and van Loan)
        //    min || s*A*Q - B ||_F
        // Notice that we've jammed a uniform scale in front of the Q.  
        // Now, the Frobenius norm (the least squares norm over matrices)
        // has the neat property that it is equivalent to minimizing the trace
        // of M^T*M (see your friendly neighborhood linear algebra text for a
        // derivation).  Thus, we can expand this out as
        //   min tr (s*A*Q - B)^T*(s*A*Q - B)
        // = min tr(Q^T*A^T*s*s*A*Q) + tr(B^T*B) - 2*tr(Q^T*A^T*s*B)  by linearity of the trace
        // = min s^2 tr(A^T*A) + tr(B^T*B) - 2*s*tr(Q^T*A^T*B)        using the fact that the trace is invariant
        //                                                            under similarity transforms Q*M*Q^T
        // If we differentiate w.r.t. s and set this to 0, we get
        // 0 = 2*s*tr(A^T*A) - 2*tr(Q^T*A^T*B)
        // so
        // 2*s*tr(A^T*A) = 2*s*tr(Q^T*A^T*B)
        // s = tr(Q^T*A^T*B) / tr(A^T*A)

        KahanSum traceATA;
        if (weights == 0)
        {
            for (int i = 0; i < numPoints; ++i)
                traceATA += ((V3d) A[i] - Acenter).length2();
        }
        else
        {
            for (int i = 0; i < numPoints; ++i)
                traceATA += ((double) weights[i]) * ((V3d) A[i] - Acenter).length2();
        }

        KahanSum traceBATQ;
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
                traceBATQ += Qt[j][i] * C[i][j];

        s = traceBATQ.get() / traceATA.get();
    }

    // Q is the rotation part of what we want to return.
    // The entire transform is:
    //    (translate origin to Bcenter) * Q * (translate Acenter to origin)
    //                last                                first
    // The effect of this on a point is:
    //    (translate origin to Bcenter) * Q * (translate Acenter to origin) * point
    //  = (translate origin to Bcenter) * Q * (-Acenter + point)
    //  = (translate origin to Bcenter) * (-Q*Acenter + Q*point)
    //  = (translate origin to Bcenter) * (translate Q*Acenter to origin) * Q*point
    //  = (translate Q*Acenter to Bcenter) * Q*point
    // So what we want to return is:
    //    (translate Q*Acenter to Bcenter) * Q
    //
    // In block form, this is:
    //   [ 1 0 0  | ] [       0 ] [ 1 0 0  |  ]   [ 1 0 0  | ] [           |   ]   [                 ]
    //   [ 0 1 0 tb ] [  s*Q  0 ] [ 0 1 0 -ta ] = [ 0 1 0 tb ] [  s*Q  -s*Q*ta ] = [   Q   tb-s*Q*ta ]
    //   [ 0 0 1  | ] [       0 ] [ 0 0 1  |  ]   [ 0 0 1  | ] [           |   ]   [                 ]
    //   [ 0 0 0  1 ] [ 0 0 0 1 ] [ 0 0 0  1  ]   [ 0 0 0  1 ] [ 0 0 0     1   ]   [ 0 0 0    1      ]
    // (ofc the whole thing is transposed for Imath).  
    const V3d translate = Bcenter - s*Acenter*Qt;

    return M44d (s*Qt.x[0][0], s*Qt.x[0][1], s*Qt.x[0][2], T(0),
                 s*Qt.x[1][0], s*Qt.x[1][1], s*Qt.x[1][2], T(0),
                 s*Qt.x[2][0], s*Qt.x[2][1], s*Qt.x[2][2], T(0),
                 translate.x, translate.y, translate.z, T(1));
} // procrustesRotationAndTranslation

template <typename T>
M44d
procrustesRotationAndTranslation (const Vec3<T>* A, const Vec3<T>* B, const size_t numPoints, const bool doScale)
{
    return procrustesRotationAndTranslation (A, B, (const T*) 0, numPoints, doScale);
} // procrustesRotationAndTranslation


template IMATH_EXPORT M44d procrustesRotationAndTranslation (const V3d* from, const V3d* to, const size_t numPoints, const bool doScale);
template IMATH_EXPORT M44d procrustesRotationAndTranslation (const V3f* from, const V3f* to, const size_t numPoints, const bool doScale);
template IMATH_EXPORT M44d procrustesRotationAndTranslation (const V3d* from, const V3d* to, const double* weights, const size_t numPoints, const bool doScale);
template IMATH_EXPORT M44d procrustesRotationAndTranslation (const V3f* from, const V3f* to, const float* weights, const size_t numPoints, const bool doScale);


namespace
{

// Applies the 2x2 Jacobi rotation
//   [  c s 0 ]    [ 1  0 0 ]    [  c 0 s ]
//   [ -s c 0 ] or [ 0  c s ] or [  0 1 0 ]
//   [  0 0 1 ]    [ 0 -s c ]    [ -s 0 c ]
// from the right; that is, computes
//   J * A
// for the Jacobi rotation J and the matrix A.  This is efficient because we
// only need to touch exactly the 2 columns that are affected, so we never
// need to explicitly construct the J matrix.  
template <typename T, int j, int k>
void
jacobiRotateRight (IMATH_INTERNAL_NAMESPACE::Matrix33<T>& A,
                   const T c,
                   const T s)
{
    for (int i = 0; i < 3; ++i)
    {
        const T tau1 = A[i][j];
        const T tau2 = A[i][k];
        A[i][j] = c * tau1 - s * tau2;
        A[i][k] = s * tau1 + c * tau2;
    }
}

template <typename T>
void
jacobiRotateRight (IMATH_INTERNAL_NAMESPACE::Matrix44<T>& A,
                   const int j,
                   const int k,
                   const T c,
                   const T s)
{
    for (int i = 0; i < 4; ++i)
    {
        const T tau1 = A[i][j];
        const T tau2 = A[i][k];
        A[i][j] = c * tau1 - s * tau2;
        A[i][k] = s * tau1 + c * tau2;
    }
}

// This routine solves the 2x2 SVD:
//     [  c1   s1 ] [ w   x ] [  c2  s2 ]   [ d1    0 ]
//     [          ] [       ] [         ] = [         ]
//     [ -s1   c1 ] [ y   z ] [ -s2  c2 ]   [  0   d2 ]
// where
//      [ w   x ]
//  A = [       ]
//      [ y   z ]
// is the subset of A consisting of the [j,k] entries, A([j k], [j k]) in
// Matlab parlance.  The method is the 'USVD' algorithm described in the
// following paper:
//    'Computation of the Singular Value Decomposition using Mesh-Connected Processors'
//    by Richard P. Brent, Franklin T. Luk, and Charles Van Loan
// It breaks the computation into two steps: the first symmetrizes the matrix,
// and the second diagonalizes the symmetric matrix.  
template <typename T, int j, int k, int l>
bool
twoSidedJacobiRotation (IMATH_INTERNAL_NAMESPACE::Matrix33<T>& A,
                        IMATH_INTERNAL_NAMESPACE::Matrix33<T>& U,
                        IMATH_INTERNAL_NAMESPACE::Matrix33<T>& V,
                        const T tol)
{
    // Load everything into local variables to make things easier on the
    // optimizer:
    const T w = A[j][j];
    const T x = A[j][k];
    const T y = A[k][j];
    const T z = A[k][k];

    // We will keep track of whether we're actually performing any rotations,
    // since if the matrix is already diagonal we'll end up with the identity
    // as our Jacobi rotation and we can short-circuit.
    bool changed = false;

    // The first step is to symmetrize the 2x2 matrix,
    //   [ c  s ]^T [ w x ] = [ p q ]
    //   [ -s c ]   [ y z ]   [ q r ]
    T mu_1 = w + z;
    T mu_2 = x - y;

    T c, s;
    if (std::abs(mu_2) <= tol*std::abs(mu_1))  // Already symmetric (to tolerance)
    {                                          // Note that the <= is important here
        c = T(1);                              // because we want to bypass the computation
        s = T(0);                              // of rho if mu_1 = mu_2 = 0.

        const T p = w;
        const T r = z;
        mu_1 = r - p;
        mu_2 = x + y;
    }
    else
    {
        const T rho = mu_1 / mu_2;
        s = T(1) / std::sqrt (T(1) + rho*rho);  // TODO is there a native inverse square root function?
        if (rho < 0)
            s = -s;
        c = s * rho;

        mu_1 = s * (x + y) + c * (z - w);   // = r - p
        mu_2 = T(2) * (c * x - s * z);      // = 2*q

        changed = true;
    }

    // The second stage diagonalizes,
    //   [ c2   s2 ]^T [ p q ] [ c2  s2 ]  = [ d1   0 ]
    //   [ -s2  c2 ]   [ q r ] [ -s2 c2 ]    [  0  d2 ]
    T c_2, s_2;
    if (std::abs(mu_2) <= tol*std::abs(mu_1))
    {
       c_2 = T(1);
       s_2 = T(0);
    }
    else
    {
        const T rho_2 = mu_1 / mu_2;
        T t_2 = T(1) / (std::abs(rho_2) + std::sqrt(1 + rho_2*rho_2));
        if (rho_2 < 0)
            t_2 = -t_2;
        c_2 = T(1) / std::sqrt (T(1) + t_2*t_2);
        s_2 = c_2 * t_2;

        changed = true;
    }

    const T c_1 = c_2 * c - s_2 * s;
    const T s_1 = s_2 * c + c_2 * s;

    if (!changed)
    {
        // We've decided that the off-diagonal entries are already small
        // enough, so we'll set them to zero.  This actually appears to result
        // in smaller errors than leaving them be, possibly because it prevents
        // us from trying to do extra rotations later that we don't need.
        A[k][j] = 0;
        A[j][k] = 0;
        return false;
    }

    const T d_1 = c_1*(w*c_2 - x*s_2) - s_1*(y*c_2 - z*s_2);
    const T d_2 = s_1*(w*s_2 + x*c_2) + c_1*(y*s_2 + z*c_2);

    // For the entries we just zeroed out, we'll just set them to 0, since
    // they should be 0 up to machine precision.  
    A[j][j] = d_1;
    A[k][k] = d_2;
    A[k][j] = 0;
    A[j][k] = 0;

    // Rotate the entries that _weren't_ involved in the 2x2 SVD:
    {
        // Rotate on the left by
        //    [  c1 s1 0 ]^T      [  c1 0 s1 ]^T      [ 1   0  0 ]^T
        //    [ -s1 c1 0 ]    or  [   0 1  0 ]    or  [ 0  c1 s1 ]
        //    [   0  0 1 ]        [ -s1 0 c1 ]        [ 0 -s1 c1 ]
        // This has the effect of adding the (weighted) ith and jth _rows_ to
        // each other.  
        const T tau1 = A[j][l];
        const T tau2 = A[k][l];
        A[j][l] = c_1 * tau1 - s_1 * tau2;
        A[k][l] = s_1 * tau1 + c_1 * tau2;
    }

    {
        // Rotate on the right by
        //    [  c2 s2 0 ]      [  c2 0 s2 ]      [ 1   0  0 ]
        //    [ -s2 c2 0 ]  or  [   0 1  0 ]  or  [ 0  c2 s2 ]
        //    [   0  0 1 ]      [ -s2 0 c2 ]      [ 0 -s2 c2 ]
        // This has the effect of adding the (weighted) ith and jth _columns_ to
        // each other.  
        const T tau1 = A[l][j];
        const T tau2 = A[l][k];
        A[l][j] = c_2 * tau1 - s_2 * tau2;
        A[l][k] = s_2 * tau1 + c_2 * tau2;
    }

    // Now apply the rotations to U and V:
    // Remember that we have 
    //    R1^T * A * R2 = D
    // This is in the 2x2 case, but after doing a bunch of these
    // we will get something like this for the 3x3 case:
    //   ... R1b^T * R1a^T * A * R2a * R2b * ... = D
    //   -----------------       ---------------
    //        = U^T                    = V
    // So,
    //   U = R1a * R1b * ...
    //   V = R2a * R2b * ...
    jacobiRotateRight<T, j, k> (U, c_1, s_1);
    jacobiRotateRight<T, j, k> (V, c_2, s_2);

    return true;
}

template <typename T>
bool
twoSidedJacobiRotation (IMATH_INTERNAL_NAMESPACE::Matrix44<T>& A,
                        int j,
                        int k,
                        IMATH_INTERNAL_NAMESPACE::Matrix44<T>& U,
                        IMATH_INTERNAL_NAMESPACE::Matrix44<T>& V,
                        const T tol)
{
    // Load everything into local variables to make things easier on the
    // optimizer:
    const T w = A[j][j];
    const T x = A[j][k];
    const T y = A[k][j];
    const T z = A[k][k];

    // We will keep track of whether we're actually performing any rotations,
    // since if the matrix is already diagonal we'll end up with the identity
    // as our Jacobi rotation and we can short-circuit.
    bool changed = false;

    // The first step is to symmetrize the 2x2 matrix,
    //   [ c  s ]^T [ w x ] = [ p q ]
    //   [ -s c ]   [ y z ]   [ q r ]
    T mu_1 = w + z;
    T mu_2 = x - y;

    T c, s;
    if (std::abs(mu_2) <= tol*std::abs(mu_1))  // Already symmetric (to tolerance)
    {                                          // Note that the <= is important here
        c = T(1);                              // because we want to bypass the computation
        s = T(0);                              // of rho if mu_1 = mu_2 = 0.

        const T p = w;
        const T r = z;
        mu_1 = r - p;
        mu_2 = x + y;
    }
    else
    {
        const T rho = mu_1 / mu_2;
        s = T(1) / std::sqrt (T(1) + rho*rho);  // TODO is there a native inverse square root function?
        if (rho < 0)
            s = -s;
        c = s * rho;

        mu_1 = s * (x + y) + c * (z - w);   // = r - p
        mu_2 = T(2) * (c * x - s * z);      // = 2*q

        changed = true;
    }

    // The second stage diagonalizes,
    //   [ c2   s2 ]^T [ p q ] [ c2  s2 ]  = [ d1   0 ]
    //   [ -s2  c2 ]   [ q r ] [ -s2 c2 ]    [  0  d2 ]
    T c_2, s_2;
    if (std::abs(mu_2) <= tol*std::abs(mu_1))
    {
       c_2 = T(1);
       s_2 = T(0);
    }
    else
    {
        const T rho_2 = mu_1 / mu_2;
        T t_2 = T(1) / (std::abs(rho_2) + std::sqrt(1 + rho_2*rho_2));
        if (rho_2 < 0)
            t_2 = -t_2;
        c_2 = T(1) / std::sqrt (T(1) + t_2*t_2);
        s_2 = c_2 * t_2;

        changed = true;
    }

    const T c_1 = c_2 * c - s_2 * s;
    const T s_1 = s_2 * c + c_2 * s;

    if (!changed)
    {
        // We've decided that the off-diagonal entries are already small
        // enough, so we'll set them to zero.  This actually appears to result
        // in smaller errors than leaving them be, possibly because it prevents
        // us from trying to do extra rotations later that we don't need.
        A[k][j] = 0;
        A[j][k] = 0;
        return false;
    }

    const T d_1 = c_1*(w*c_2 - x*s_2) - s_1*(y*c_2 - z*s_2);
    const T d_2 = s_1*(w*s_2 + x*c_2) + c_1*(y*s_2 + z*c_2);

    // For the entries we just zeroed out, we'll just set them to 0, since
    // they should be 0 up to machine precision.  
    A[j][j] = d_1;
    A[k][k] = d_2;
    A[k][j] = 0;
    A[j][k] = 0;

    // Rotate the entries that _weren't_ involved in the 2x2 SVD:
    for (int l = 0; l < 4; ++l)
    {
        if (l == j || l == k)
            continue;

        // Rotate on the left by
        //    [ 1               ]
        //    [   .             ]
        //    [     c2   s2     ]  j
        //    [        1        ]
        //    [    -s2   c2     ]  k
        //    [             .   ]
        //    [               1 ]
        //          j    k
        //
        // This has the effect of adding the (weighted) ith and jth _rows_ to
        // each other.  
        const T tau1 = A[j][l];
        const T tau2 = A[k][l];
        A[j][l] = c_1 * tau1 - s_1 * tau2;
        A[k][l] = s_1 * tau1 + c_1 * tau2;
    }

    for (int l = 0; l < 4; ++l)
    {
        // We set the A[j/k][j/k] entries already
        if (l == j || l == k)
            continue;

        // Rotate on the right by
        //    [ 1               ]
        //    [   .             ]
        //    [     c2   s2     ]  j
        //    [        1        ]
        //    [    -s2   c2     ]  k
        //    [             .   ]
        //    [               1 ]
        //          j    k
        //
        // This has the effect of adding the (weighted) ith and jth _columns_ to
        // each other.  
        const T tau1 = A[l][j];
        const T tau2 = A[l][k];
        A[l][j] = c_2 * tau1 - s_2 * tau2;
        A[l][k] = s_2 * tau1 + c_2 * tau2;
    }

    // Now apply the rotations to U and V:
    // Remember that we have 
    //    R1^T * A * R2 = D
    // This is in the 2x2 case, but after doing a bunch of these
    // we will get something like this for the 3x3 case:
    //   ... R1b^T * R1a^T * A * R2a * R2b * ... = D
    //   -----------------       ---------------
    //        = U^T                    = V
    // So,
    //   U = R1a * R1b * ...
    //   V = R2a * R2b * ...
    jacobiRotateRight (U, j, k, c_1, s_1);
    jacobiRotateRight (V, j, k, c_2, s_2);

    return true;
}

template <typename T>
void
swapColumns (IMATH_INTERNAL_NAMESPACE::Matrix33<T>& A, int j, int k)
{
    for (int i = 0; i < 3; ++i)
        std::swap (A[i][j], A[i][k]);
}

template <typename T>
T
maxOffDiag (const IMATH_INTERNAL_NAMESPACE::Matrix33<T>& A)
{
    T result = 0;
    result = std::max (result, std::abs (A[0][1]));
    result = std::max (result, std::abs (A[0][2]));
    result = std::max (result, std::abs (A[1][0]));
    result = std::max (result, std::abs (A[1][2]));
    result = std::max (result, std::abs (A[2][0]));
    result = std::max (result, std::abs (A[2][1]));
    return result;
}

template <typename T>
T
maxOffDiag (const IMATH_INTERNAL_NAMESPACE::Matrix44<T>& A)
{
    T result = 0;
    for (int i = 0; i < 4; ++i)
    {
        for (int j = 0; j < 4; ++j)
        {
            if (i != j)
                result = std::max (result, std::abs (A[i][j]));
        }
    }

    return result;
}

template <typename T>
void
twoSidedJacobiSVD (IMATH_INTERNAL_NAMESPACE::Matrix33<T> A,
                   IMATH_INTERNAL_NAMESPACE::Matrix33<T>& U,
                   IMATH_INTERNAL_NAMESPACE::Vec3<T>& S,
                   IMATH_INTERNAL_NAMESPACE::Matrix33<T>& V,
                   const T tol,
                   const bool forcePositiveDeterminant)
{
    // The two-sided Jacobi SVD works by repeatedly zeroing out
    // off-diagonal entries of the matrix, 2 at a time.  Basically,
    // we can take our 3x3 matrix,
    //    [* * *]
    //    [* * *]
    //    [* * *]
    // and use a pair of orthogonal transforms to zero out, say, the
    // pair of entries (0, 1) and (1, 0):
    //  [ c1 s1  ] [* * *] [ c2 s2  ]   [*   *]
    //  [-s1 c1  ] [* * *] [-s2 c2  ] = [  * *]
    //  [       1] [* * *] [       1]   [* * *]
    // When we go to zero out the next pair of entries (say, (0, 2) and (2, 0))
    // then we don't expect those entries to stay 0:
    //  [ c1 s1  ] [*   *] [ c2 s2  ]   [* *  ]
    //  [-s1 c1  ] [  * *] [-s2 c2  ] = [* * *]
    //  [       1] [* * *] [       1]   [  * *]
    // However, if we keep doing this, we'll find that the off-diagonal entries
    // converge to 0 fairly quickly (convergence should be roughly cubic).  The 
    // result is a diagonal A matrix and a bunch of orthogonal transforms:
    //               [* * *]                [*    ]
    //  L1 L2 ... Ln [* * *] Rn ... R2 R1 = [  *  ]
    //               [* * *]                [    *]
    //  ------------ ------- ------------   -------
    //      U^T         A         V            S
    // This turns out to be highly accurate because (1) orthogonal transforms
    // are extremely stable to compute and apply (this is why QR factorization
    // works so well, FWIW) and because (2) by applying everything to the original
    // matrix A instead of computing (A^T * A) we avoid any precision loss that
    // would result from that.  
    U.makeIdentity();
    V.makeIdentity();

    const int maxIter = 20;  // In case we get really unlucky, prevents infinite loops
    const T absTol = tol * maxOffDiag (A);  // Tolerance is in terms of the maximum
    if (absTol != 0)                        // _off-diagonal_ entry.
    {
        int numIter = 0;
        do
        {
            ++numIter;
            bool changed = twoSidedJacobiRotation<T, 0, 1, 2> (A, U, V, tol);
            changed = twoSidedJacobiRotation<T, 0, 2, 1> (A, U, V, tol) || changed;
            changed = twoSidedJacobiRotation<T, 1, 2, 0> (A, U, V, tol) || changed;
            if (!changed)
                break;
        } while (maxOffDiag(A) > absTol && numIter < maxIter);
    }

    // The off-diagonal entries are (effectively) 0, so whatever's left on the
    // diagonal are the singular values:
    S.x = A[0][0];
    S.y = A[1][1];
    S.z = A[2][2];

    // Nothing thus far has guaranteed that the singular values are positive,
    // so let's go back through and flip them if not (since by contract we are
    // supposed to return all positive SVs):
    for (int i = 0; i < 3; ++i)
    {
        if (S[i] < 0)
        {
            // If we flip S[i], we need to flip the corresponding column of U
            // (we could also pick V if we wanted; it doesn't really matter):
            S[i] = -S[i];
            for (int j = 0; j < 3; ++j)
                U[j][i] = -U[j][i];
        }
    }

    // Order the singular values from largest to smallest; this requires
    // exactly two passes through the data using bubble sort:
    for (int i = 0; i < 2; ++i)
    {
        for (int j = 0; j < (2 - i); ++j)
        {
            // No absolute values necessary since we already ensured that
            // they're positive:
            if (S[j] < S[j+1])
            {
                // If we swap singular values we also have to swap
                // corresponding columns in U and V:
                std::swap (S[j], S[j+1]);
                swapColumns (U, j, j+1);
                swapColumns (V, j, j+1);
            }
        }
    }

    if (forcePositiveDeterminant)
    {
        // We want to guarantee that the returned matrices always have positive
        // determinant.  We can do this by adding the appropriate number of
        // matrices of the form:
        //       [ 1       ]
        //  L =  [    1    ]
        //       [      -1 ]
        // Note that L' = L and L*L = Identity.  Thus we can add:
        //   U*L*L*S*V = (U*L)*(L*S)*V
        // if U has a negative determinant, and
        //   U*S*L*L*V = U*(S*L)*(L*V)
        // if V has a neg. determinant.
        if (U.determinant() < 0)
        {
            for (int i = 0; i < 3; ++i)
                U[i][2] = -U[i][2];
            S.z = -S.z;
        }
   
        if (V.determinant() < 0)
        {
            for (int i = 0; i < 3; ++i)
                V[i][2] = -V[i][2];
            S.z = -S.z;
        }
    }
}

template <typename T>
void
twoSidedJacobiSVD (IMATH_INTERNAL_NAMESPACE::Matrix44<T> A,
                   IMATH_INTERNAL_NAMESPACE::Matrix44<T>& U,
                   IMATH_INTERNAL_NAMESPACE::Vec4<T>& S,
                   IMATH_INTERNAL_NAMESPACE::Matrix44<T>& V,
                   const T tol,
                   const bool forcePositiveDeterminant)
{
    // Please see the Matrix33 version for a detailed description of the algorithm.
    U.makeIdentity();
    V.makeIdentity();

    const int maxIter = 20;  // In case we get really unlucky, prevents infinite loops
    const T absTol = tol * maxOffDiag (A);  // Tolerance is in terms of the maximum
    if (absTol != 0)                        // _off-diagonal_ entry.
    {
        int numIter = 0;
        do
        {
            ++numIter;
            bool changed = twoSidedJacobiRotation (A, 0, 1, U, V, tol);
            changed = twoSidedJacobiRotation (A, 0, 2, U, V, tol) || changed;
            changed = twoSidedJacobiRotation (A, 0, 3, U, V, tol) || changed;
            changed = twoSidedJacobiRotation (A, 1, 2, U, V, tol) || changed;
            changed = twoSidedJacobiRotation (A, 1, 3, U, V, tol) || changed;
            changed = twoSidedJacobiRotation (A, 2, 3, U, V, tol) || changed;
            if (!changed)
                break;
        } while (maxOffDiag(A) > absTol && numIter < maxIter);
    }

    // The off-diagonal entries are (effectively) 0, so whatever's left on the
    // diagonal are the singular values:
    S[0] = A[0][0];
    S[1] = A[1][1];
    S[2] = A[2][2];
    S[3] = A[3][3];

    // Nothing thus far has guaranteed that the singular values are positive,
    // so let's go back through and flip them if not (since by contract we are
    // supposed to return all positive SVs):
    for (int i = 0; i < 4; ++i)
    {
        if (S[i] < 0)
        {
            // If we flip S[i], we need to flip the corresponding column of U
            // (we could also pick V if we wanted; it doesn't really matter):
            S[i] = -S[i];
            for (int j = 0; j < 4; ++j)
                U[j][i] = -U[j][i];
        }
    }

    // Order the singular values from largest to smallest using insertion sort:
    for (int i = 1; i < 4; ++i)
    {
        const IMATH_INTERNAL_NAMESPACE::Vec4<T> uCol (U[0][i], U[1][i], U[2][i], U[3][i]);
        const IMATH_INTERNAL_NAMESPACE::Vec4<T> vCol (V[0][i], V[1][i], V[2][i], V[3][i]);
        const T sVal = S[i];

        int j = i - 1;
        while (std::abs (S[j]) < std::abs (sVal))
        {
            for (int k = 0; k < 4; ++k)
                U[k][j+1] = U[k][j];
            for (int k = 0; k < 4; ++k)
                V[k][j+1] = V[k][j];
            S[j+1] = S[j];

            --j;
            if (j < 0)
                break;
        }

        for (int k = 0; k < 4; ++k)
            U[k][j+1] = uCol[k];
        for (int k = 0; k < 4; ++k)
            V[k][j+1] = vCol[k];
        S[j+1] = sVal;
    }

    if (forcePositiveDeterminant)
    {
        // We want to guarantee that the returned matrices always have positive
        // determinant.  We can do this by adding the appropriate number of
        // matrices of the form:
        //       [ 1          ]
        //  L =  [    1       ]
        //       [       1    ]
        //       [         -1 ]
        // Note that L' = L and L*L = Identity.  Thus we can add:
        //   U*L*L*S*V = (U*L)*(L*S)*V
        // if U has a negative determinant, and
        //   U*S*L*L*V = U*(S*L)*(L*V)
        // if V has a neg. determinant.
        if (U.determinant() < 0)
        {
            for (int i = 0; i < 4; ++i)
                U[i][3] = -U[i][3];
            S[3] = -S[3];
        }
   
        if (V.determinant() < 0)
        {
            for (int i = 0; i < 4; ++i)
                V[i][3] = -V[i][3];
            S[3] = -S[3];
        }
    }
}

}

template <typename T>
void
jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix33<T>& A,
           IMATH_INTERNAL_NAMESPACE::Matrix33<T>& U,
           IMATH_INTERNAL_NAMESPACE::Vec3<T>& S,
           IMATH_INTERNAL_NAMESPACE::Matrix33<T>& V,
           const T tol,
           const bool forcePositiveDeterminant)
{
    twoSidedJacobiSVD (A, U, S, V, tol, forcePositiveDeterminant);
}

template <typename T>
void
jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix44<T>& A,
           IMATH_INTERNAL_NAMESPACE::Matrix44<T>& U,
           IMATH_INTERNAL_NAMESPACE::Vec4<T>& S,
           IMATH_INTERNAL_NAMESPACE::Matrix44<T>& V,
           const T tol,
           const bool forcePositiveDeterminant)
{
    twoSidedJacobiSVD (A, U, S, V, tol, forcePositiveDeterminant);
}

template IMATH_EXPORT void jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix33<float>& A,
                                      IMATH_INTERNAL_NAMESPACE::Matrix33<float>& U,
                                      IMATH_INTERNAL_NAMESPACE::Vec3<float>& S,
                                      IMATH_INTERNAL_NAMESPACE::Matrix33<float>& V,
                                      const float tol,
                                      const bool forcePositiveDeterminant);
template IMATH_EXPORT void jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix33<double>& A,
                                      IMATH_INTERNAL_NAMESPACE::Matrix33<double>& U,
                                      IMATH_INTERNAL_NAMESPACE::Vec3<double>& S,
                                      IMATH_INTERNAL_NAMESPACE::Matrix33<double>& V,
                                      const double tol,
                                      const bool forcePositiveDeterminant);
template IMATH_EXPORT void jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix44<float>& A,
                                      IMATH_INTERNAL_NAMESPACE::Matrix44<float>& U,
                                      IMATH_INTERNAL_NAMESPACE::Vec4<float>& S,
                                      IMATH_INTERNAL_NAMESPACE::Matrix44<float>& V,
                                      const float tol,
                                      const bool forcePositiveDeterminant);
template IMATH_EXPORT void jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix44<double>& A,
                                      IMATH_INTERNAL_NAMESPACE::Matrix44<double>& U,
                                      IMATH_INTERNAL_NAMESPACE::Vec4<double>& S,
                                      IMATH_INTERNAL_NAMESPACE::Matrix44<double>& V,
                                      const double tol,
                                      const bool forcePositiveDeterminant);

namespace
{

template <int j, int k, typename TM>
inline 
void
jacobiRotateRight (TM& A,
                   const typename TM::BaseType s,
                   const typename TM::BaseType tau)
{
    typedef typename TM::BaseType T;

    for (unsigned int i = 0; i < TM::dimensions(); ++i)
    {
        const T nu1 = A[i][j];
        const T nu2 = A[i][k];
        A[i][j] -= s * (nu2 + tau * nu1);
        A[i][k] += s * (nu1 - tau * nu2);
   }
}

template <int j, int k, int l, typename T>
bool
jacobiRotation (Matrix33<T>& A,
                Matrix33<T>& V,
                Vec3<T>& Z,
                const T tol)
{
    // Load everything into local variables to make things easier on the
    // optimizer:
    const T x = A[j][j];
    const T y = A[j][k];
    const T z = A[k][k];

    // The first stage diagonalizes,
    //   [ c  s ]^T [ x y ] [ c -s ]  = [ d1   0 ]
    //   [ -s c ]   [ y z ] [ s  c ]    [  0  d2 ]
    const T mu1 = z - x;
    const T mu2 = 2 * y;

    if (std::abs(mu2) <= tol*std::abs(mu1))
    {
        // We've decided that the off-diagonal entries are already small
        // enough, so we'll set them to zero.  This actually appears to result
        // in smaller errors than leaving them be, possibly because it prevents
        // us from trying to do extra rotations later that we don't need.
        A[j][k] = 0;
        return false;
    }
    const T rho = mu1 / mu2;
    const T t = (rho < 0 ? T(-1) : T(1)) / (std::abs(rho) + std::sqrt(1 + rho*rho));
    const T c = T(1) / std::sqrt (T(1) + t*t);
    const T s = t * c;
    const T tau = s / (T(1) + c);
    const T h = t * y;

    // Update diagonal elements.
    Z[j] -= h;
    Z[k] += h;
    A[j][j] -= h;
    A[k][k] += h;

    // For the entries we just zeroed out, we'll just set them to 0, since
    // they should be 0 up to machine precision.  
    A[j][k] = 0;

    // We only update upper triagnular elements of A, since
    // A is supposed to be symmetric.
    T& offd1 = l < j ? A[l][j] : A[j][l];
    T& offd2 = l < k ? A[l][k] : A[k][l];
    const T nu1 = offd1;
    const T nu2 = offd2;
    offd1 = nu1 - s * (nu2 + tau * nu1);
    offd2 = nu2 + s * (nu1 - tau * nu2); 

    // Apply rotation to V
    jacobiRotateRight<j, k> (V, s, tau);

    return true;
}

template <int j, int k, int l1, int l2, typename T>
bool
jacobiRotation (Matrix44<T>& A,
                Matrix44<T>& V,
                Vec4<T>& Z,
                const T tol)
{
    const T x = A[j][j];
    const T y = A[j][k];
    const T z = A[k][k];

    const T mu1 = z - x;
    const T mu2 = T(2) * y;

    // Let's see if rho^(-1) = mu2 / mu1 is less than tol
    // This test also checks if rho^2 will overflow 
    // when tol^(-1) < sqrt(limits<T>::max()).
    if (std::abs(mu2) <= tol*std::abs(mu1))
    {
        A[j][k] = 0;
        return true;
    }

    const T rho = mu1 / mu2;
    const T t = (rho < 0 ? T(-1) : T(1)) / (std::abs(rho) + std::sqrt(1 + rho*rho));
    const T c = T(1) / std::sqrt (T(1) + t*t);
    const T s = c * t;
    const T tau = s / (T(1) + c);
    const T h = t * y;

    Z[j] -= h;
    Z[k] += h;
    A[j][j] -= h;
    A[k][k] += h;
    A[j][k] = 0;

    {
        T& offd1 = l1 < j ? A[l1][j] : A[j][l1];
        T& offd2 = l1 < k ? A[l1][k] : A[k][l1];
        const T nu1 = offd1;
        const T nu2 = offd2;
        offd1 -= s * (nu2 + tau * nu1);
        offd2 += s * (nu1 - tau * nu2); 
    }

    {
        T& offd1 = l2 < j ? A[l2][j] : A[j][l2];
        T& offd2 = l2 < k ? A[l2][k] : A[k][l2];
        const T nu1 = offd1;
        const T nu2 = offd2;
        offd1 -= s * (nu2 + tau * nu1);
        offd2 += s * (nu1 - tau * nu2); 
    }

    jacobiRotateRight<j, k> (V, s, tau);

    return true;
}

template <typename TM>
inline
typename TM::BaseType
maxOffDiagSymm (const TM& A)
{
    typedef typename TM::BaseType T;
    T result = 0;
    for (unsigned int i = 0; i < TM::dimensions(); ++i)
        for (unsigned int j = i+1; j < TM::dimensions(); ++j)
            result = std::max (result, std::abs (A[i][j]));

   return result;
}

} // namespace

template <typename T>
void
jacobiEigenSolver (Matrix33<T>& A,
                   Vec3<T>& S,
                   Matrix33<T>& V,
                   const T tol)
{
    V.makeIdentity();
    for(int i = 0; i < 3; ++i) {
        S[i] = A[i][i];
    }

    const int maxIter = 20;  // In case we get really unlucky, prevents infinite loops
    const T absTol = tol * maxOffDiagSymm (A);  // Tolerance is in terms of the maximum
    if (absTol != 0)                        // _off-diagonal_ entry.
    {
        int numIter = 0;
        do
        {
            // Z is for accumulating small changes (h) to diagonal entries
            // of A for one sweep. Adding h's directly to A might cause
            // a cancellation effect when h is relatively very small to 
            // the corresponding diagonal entry of A and 
            // this will increase numerical errors
            Vec3<T> Z(0, 0, 0);
            ++numIter;
            bool changed = jacobiRotation<0, 1, 2> (A, V, Z, tol);
            changed = jacobiRotation<0, 2, 1> (A, V, Z, tol) || changed;
            changed = jacobiRotation<1, 2, 0> (A, V, Z, tol) || changed;
            // One sweep passed. Add accumulated changes (Z) to singular values (S)
            // Update diagonal elements of A for better accuracy as well.
            for(int i = 0; i < 3; ++i) {
                A[i][i] = S[i] += Z[i];
            }
            if (!changed)
                break;
        } while (maxOffDiagSymm(A) > absTol && numIter < maxIter);
    }
}

template <typename T>
void
jacobiEigenSolver (Matrix44<T>& A,
                   Vec4<T>& S,
                   Matrix44<T>& V,
                   const T tol)
{
    V.makeIdentity();

    for(int i = 0; i < 4; ++i) {
        S[i] = A[i][i];
    }

    const int maxIter = 20;  // In case we get really unlucky, prevents infinite loops
    const T absTol = tol * maxOffDiagSymm (A);  // Tolerance is in terms of the maximum
    if (absTol != 0)                        // _off-diagonal_ entry.
    {
        int numIter = 0;
        do
        {
            ++numIter;
            Vec4<T> Z(0, 0, 0, 0);
            bool changed = jacobiRotation<0, 1, 2, 3> (A, V, Z, tol);
            changed = jacobiRotation<0, 2, 1, 3> (A, V, Z, tol) || changed;
            changed = jacobiRotation<0, 3, 1, 2> (A, V, Z, tol) || changed;
            changed = jacobiRotation<1, 2, 0, 3> (A, V, Z, tol) || changed;
            changed = jacobiRotation<1, 3, 0, 2> (A, V, Z, tol) || changed;
            changed = jacobiRotation<2, 3, 0, 1> (A, V, Z, tol) || changed;
            for(int i = 0; i < 4; ++i) {
                A[i][i] = S[i] += Z[i];
            }
           if (!changed)
                break;
        } while (maxOffDiagSymm(A) > absTol && numIter < maxIter);
    }
}

template <typename TM, typename TV>
void
maxEigenVector (TM& A, TV& V)
{
    TV S;
    TM MV;
    jacobiEigenSolver(A, S, MV);

    int maxIdx(0);
    for(unsigned int i = 1; i < TV::dimensions(); ++i)
    {
        if(std::abs(S[i]) > std::abs(S[maxIdx]))
            maxIdx = i;
    }

    for(unsigned int i = 0; i < TV::dimensions(); ++i)
        V[i] = MV[i][maxIdx];
}

template <typename TM, typename TV>
void
minEigenVector (TM& A, TV& V)
{
    TV S;
    TM MV;
    jacobiEigenSolver(A, S, MV);

    int minIdx(0);
    for(unsigned int i = 1; i < TV::dimensions(); ++i)
    {
        if(std::abs(S[i]) < std::abs(S[minIdx]))
            minIdx = i;
    }

   for(unsigned int i = 0; i < TV::dimensions(); ++i)
        V[i] = MV[i][minIdx];
}

template IMATH_EXPORT void jacobiEigenSolver (Matrix33<float>& A,
                                              Vec3<float>& S,
                                              Matrix33<float>& V,
                                              const float tol);
template IMATH_EXPORT void jacobiEigenSolver (Matrix33<double>& A,
                                              Vec3<double>& S,
                                              Matrix33<double>& V,
                                              const double tol);
template IMATH_EXPORT void jacobiEigenSolver (Matrix44<float>& A,
                                              Vec4<float>& S,
                                              Matrix44<float>& V,
                                              const float tol);
template IMATH_EXPORT void jacobiEigenSolver (Matrix44<double>& A,
                                              Vec4<double>& S,
                                              Matrix44<double>& V,
                                              const double tol);

template IMATH_EXPORT void maxEigenVector (Matrix33<float>& A,
                                           Vec3<float>& S);
template IMATH_EXPORT void maxEigenVector (Matrix44<float>& A,
                                           Vec4<float>& S);
template IMATH_EXPORT void maxEigenVector (Matrix33<double>& A,
                                           Vec3<double>& S);
template IMATH_EXPORT void maxEigenVector (Matrix44<double>& A,
                                           Vec4<double>& S);

template IMATH_EXPORT void minEigenVector (Matrix33<float>& A,
                                           Vec3<float>& S);
template IMATH_EXPORT void minEigenVector (Matrix44<float>& A,
                                           Vec4<float>& S);
template IMATH_EXPORT void minEigenVector (Matrix33<double>& A,
                                           Vec3<double>& S);
template IMATH_EXPORT void minEigenVector (Matrix44<double>& A,
                                           Vec4<double>& S);

IMATH_INTERNAL_NAMESPACE_SOURCE_EXIT