///////////////////////////////////////////////////////////////////////////
//
// 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