/**
* Copy the contents of the matrix to another addressable matrix. The matrices must have the same dimensions, else an
* assertion failure is generated.
*/
template <typename U> void copyTo(AddressableMatrix<U> & dst) const
{
long nr = this->numRows(), nc = this->numColumns();
alwaysAssertM(nr == dst.numRows() && nc == dst.numColumns(),
"AddressableMatrix: Copy destination has different dimensions");
for (long r = 0; r < nr; ++r)
for (long c = 0; c < nc; ++c)
dst.set(r, c, get(r, c));
}
void hessianAt(double const * p, AddressableMatrix<double> & result) const
{
checkHessianResultDims(result);
result.makeZero();
}
void hessianAt(float const * p, AddressableMatrix<float> & result) const
{
checkHessianResultDims(result);
result.makeZero();
}
/** Constructor. */
ConstIterator(AddressableMatrix const & m_, long r = 0, long c = 0)
: m(m_), nrows(m_.numRows()), ncols(m_.numColumns()), entry(IndexPair(r, c), 0)
{}
bool
svdCoreT(AddressableMatrix<R> & U, long rows, long cols, S * D, AddressableMatrix<T> & V)
{
static int const MAX_ITERATIONS = 30;
if (rows == 0 && cols == 0)
return true;
debugAssertM(rows >= cols, "SVD: Matrix must have more rows than columns");
long i, j, jj, k, l = 0, nm = 0;
int flag, its;
double c, f, h, s, x, y, z;
double anorm = 0.0, g = 0.0, scale = 0.0;
// Temp row vector
TheaArray<double> rv1((array_size_t)cols);
// Householder reduction to bidiagonal form
for (i = 0; i < cols; ++i)
{
// Left-hand reduction
l = i + 1;
rv1[i] = scale * g;
g = s = scale = 0.0;
if (i < rows)
{
for (k = i; k < rows; ++k)
{
scale += std::fabs((double)U.get(k, i));
}
if (scale)
{
for (k = i; k < rows; ++k)
{
double x = (double)U.get(k, i) / scale;
U.set(k, i, x);
s += (x * x);
}
f = (double)U.get(i, i);
g = -SVD_SIGN(std::sqrt(s), f);
h = f * g - s;
U.set(i, i, (R)(f - g));
if (i != cols - 1)
{
for (j = l; j < cols; j++)
{
for (s = 0.0, k = i; k < rows; ++k)
{
s += ((double)U.get(k, i) * (double)U.get(k, j));
}
f = s / h;
for (k = i; k < rows; ++k)
{
U.getMutable(k, j) += (R)(f * (double)U.get(k, i));
}
}
}
for (k = i; k < rows; ++k)
{
U.getMutable(k, i) *= (R)scale;
}
}
}
D[i] = (S)(scale * g);
// right-hand reduction
g = s = scale = 0.0;
if (i < rows && i != cols - 1)
{
for (k = l; k < cols; ++k)
{
scale += std::fabs((double)U.get(i, k));
}
if (scale)
{
for (k = l; k < cols; ++k)
{
double x = (double)U.get(i, k) / scale;
U.set(i, k, (R)x);
s += (x * x);
}
f = (double)U.get(i, l);
g = -SVD_SIGN(std::sqrt(s), f);
h = f * g - s;
U.set(i, l, (R)(f - g));
for (k = l; k < cols; ++k)
{
rv1[k] = (double)U.get(i, k) / h;
}
if (i != rows - 1)
{
for (j = l; j < rows; ++j)
{
for (s = 0.0, k = l; k < cols; ++k)
{
s += ((double)U.get(j, k) * (double)U.get(i, k));
}
for (k = l; k < cols; ++k)
{
U.getMutable(j, k) += (R)(s * rv1[k]);
}
}
}
for (k = l; k < cols; ++k)
{
U.getMutable(i, k) *= (R)scale;
}
}
}
anorm = std::max(anorm, std::fabs((double)D[i]) + std::fabs(rv1[i]));
}
// accumulate the right-hand transformation
for (i = cols - 1; i >= 0; --i)
{
if (i < cols - 1)
{
if (g != 0.0)
{
for (j = l; j < cols; j++)
{
V.set(j, i, (T)(((double)U.get(i, j) / (double)U.get(i, l)) / g));
}
// double division to avoid underflow
for (j = l; j < cols; ++j)
{
for (s = 0.0, k = l; k < cols; k++) {
s += ((double)U.get(i, k) * (double)V.get(k, j));
}
for (k = l; k < cols; ++k)
{
V.getMutable(k, j) += (T)(s * (double)V.get(k, i));
}
}
}
for (j = l; j < cols; ++j)
{
V.set(i, j, 0.0);
V.set(j, i, 0.0);
}
}
V.set(i, i, 1.0);
g = rv1[i];
l = i;
}
// accumulate the left-hand transformation
for (i = cols - 1; i >= 0; --i)
{
l = i + 1;
g = (double)D[i];
if (i < cols - 1)
{
for (j = l; j < cols; ++j)
{
U.set(i, j, 0.0);
}
}
if (g)
{
g = 1.0 / g;
if (i != cols - 1)
{
for (j = l; j < cols; ++j)
{
for (s = 0.0, k = l; k < rows; ++k)
{
s += ((double)U.get(k, i) * (double)U.get(k, j));
}
f = (s / (double)U.get(i, i)) * g;
for (k = i; k < rows; ++k)
{
U.getMutable(k, j) += (R)(f * (double)U.get(k, i));
}
}
}
for (j = i; j < rows; ++j)
{
U.getMutable(j, i) *= (R)g;
}
}
else
{
for (j = i; j < rows; ++j)
{
U.set(j, i, 0.0);
}
}
U.getMutable(i, i)++;
}
// diagonalize the bidiagonal form
for (k = cols - 1; k >= 0; --k)
{
// loop over singular values
for (its = 0; its < MAX_ITERATIONS; ++its)
{
// loop over allowed iterations
flag = 1;
for (l = k; l >= 0; --l)
{
// test for splitting
nm = l - 1;
if (std::fabs(rv1[l]) + anorm == anorm)
{
flag = 0;
break;
}
if (std::fabs((double)D[nm]) + anorm == anorm)
{
break;
}
}
if (flag)
{
c = 0.0;
s = 1.0;
for (i = l; i <= k; ++i)
{
f = s * rv1[i];
if (std::fabs(f) + anorm != anorm)
{
g = (double)D[i];
h = pythag(f, g);
D[i] = (S)h;
h = 1.0 / h;
c = g * h;
s = (- f * h);
for (j = 0; j < rows; ++j)
{
y = (double)U.get(j, nm);
z = (double)U.get(j, i);
U.set(j, nm, (R)(y * c + z * s));
U.set(j, i, (R)(z * c - y * s));
}
}
}
}
z = (double)D[k];
if (l == k)
{
// convergence
if (z < 0.0)
{
// make singular value nonnegative
D[k] = (S)(-z);
for (j = 0; j < cols; ++j)
{
V.set(j, k, -V.get(j, k));
}
}
break;
}
if (its >= MAX_ITERATIONS)
{
THEA_DEBUG << "SVD: Failed to converge";
return false;
}
// shift from bottom 2 x 2 minor
x = (double)D[l];
nm = k - 1;
y = (double)D[nm];
g = rv1[nm];
h = rv1[k];
f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
g = pythag(f, 1.0);
f = ((x - z) * (x + z) + h * ((y / (f + SVD_SIGN(g, f))) - h)) / x;
// next QR transformation
c = s = 1.0;
for (j = l; j <= nm; ++j)
{
i = j + 1;
g = rv1[i];
y = (double)D[i];
h = s * g;
g = c * g;
z = pythag(f, h);
rv1[j] = z;
c = f / z;
s = h / z;
f = x * c + g * s;
g = g * c - x * s;
h = y * s;
y = y * c;
for (jj = 0; jj < cols; ++jj)
{
x = (double)V.get(jj, j);
z = (double)V.get(jj, i);
V.set(jj, j, (T)(x * c + z * s));
V.set(jj, i, (T)(z * c - x * s));
}
z = pythag(f, h);
D[j] = (S)z;
if (z)
{
z = 1.0 / z;
c = f * z;
s = h * z;
}
f = (c * g) + (s * y);
x = (c * y) - (s * g);
for (jj = 0; jj < rows; jj++)
{
y = (double)U.get(jj, j);
z = (double)U.get(jj, i);
U.set(jj, j, (R)(y * c + z * s));
U.set(jj, i, (R)(z * c - y * s));
}
}
rv1[l] = 0.0;
rv1[k] = f;
D[k] = (S)x;
}
}
// So far V actually contains V^T, so we transpose V in-place
for (i = 0; i < rows; ++i)
for (j = i + 1; j < cols; ++j)
std::swap(V.getMutable(i, j), V.getMutable(j, i));
return true;
}
