FgMatrixV<T>
fgDiagonal(const FgMatrixV<T> & vec)
{
FGASSERT((vec.numRows() == 1) || (vec.numCols() == 1));
uint dim = vec.numRows() * vec.numCols();
FgMatrixV<T> ret(dim,dim,T(0));
for (uint ii=0; ii<dim; ++ii)
ret.elem(ii,ii) = vec[ii];
return ret;
}
FgMatrixV<T>
fgModulateCols(
const FgMatrixV<T> & matrix,
const FgMatrixV<T> & modVector)
{
FGASSERT(matrix.numCols() == modVector.numElems());
FgMatrixD ret = matrix;
for (uint rr=0; rr<matrix.numRows(); ++rr)
for (uint cc=0; cc<matrix.numCols(); ++cc)
ret.elem(rr,cc) *= modVector[cc];
return ret;
}
void
fgRealSymmEigs(
const FgMatrixV<T> &rsm,
FgMatrixV<T> &val, // RETURNED: Col vector of eigenvalues, smallest to largest
FgMatrixV<T> &vec) // RETURNED: Col vectors are respective eigenvectors
{
// JAMA enters an infinite loop with NaNs:
for (size_t ii=0; ii<rsm.m_data.size(); ++ii)
FGASSERT(boost::math::isfinite(rsm.m_data[ii]));
uint dim = rsm.numRows();
FGASSERT(rsm.numCols() == dim);
// We use a const cast since we know 'solver' will not modify the elements, even though
// the Array2D object holds a non-const pointer to our data.
JAMA::Eigenvalue<T>
solver(TNT::Array2D<T>(rsm.numRows(),rsm.numCols(),const_cast<T*>(rsm.dataPtr())));
TNT::Array2D<T> vecs;
TNT::Array1D<T> vals;
solver.getV(vecs);
solver.getRealEigenvalues(vals);
val.resize(dim,1);
vec.resize(dim,dim);
int idim = static_cast<int>(dim);
for (int row=0; row<idim; row++) {
val[row] = vals[row];
for (uint col=0; col<dim; col++)
vec.elem(row,col) = vecs[row][col];
}
}
// Accumulate in sub-matrix:
void
accSubMat(size_t row,size_t col,const FgMatrixV & m)
{
FGASSERT((m.nrows+row <= nrows) && (m.ncols+col <= ncols));
for (uint rr=0; rr<m.nrows; ++rr)
for (uint cc=0; cc<m.ncols; ++cc)
elem(row+rr,col+cc) += m.elem(rr,cc);
}
FgMatrixV<T>
fgConcatVert(
const FgMatrixV<T> & upper,
const FgMatrixV<T> & lower)
{
if (upper.empty())
return lower;
if (lower.empty())
return upper;
FGASSERT(upper.numCols() == lower.numCols());
FgMatrixV<T> ret(upper.numRows()+lower.numRows(),upper.numCols());
for (uint ii=0; ii<upper.numElems(); ++ii)
ret[ii] = upper[ii];
uint off = upper.numElems();
for (uint ii=0; ii<lower.numElems(); ++ii)
ret[off+ii] = lower[ii];
return ret;
}
FgMatrixV
operator*(const FgMatrixV & m) const
{
FgMatrixV<T> newMat(nrows,m.ncols);
FGASSERT(ncols == m.nrows);
for (uint ii=0; ii<nrows; ii++) {
for (uint jj=0; jj<m.ncols; jj++) {
newMat.elem(ii,jj) = 0.0;
for (uint kk=0; kk<ncols; kk++)
newMat.elem(ii,jj) += this->elem(ii,kk) * m.elem(kk,jj);
}
}
return newMat;
}
explicit
FgMatrixC(const FgMatrixV<T>& mm) {
FGASSERT((nrows == mm.numRows()) && (ncols == mm.numCols()));
for (uint ii=0; ii<nrows*ncols; ++ii)
m[ii] = mm[ii];
}
inline FgMatrixC<T,1,2>
fgBounds(const FgMatrixV<T> & mat)
{
return fgBounds(mat.dataVec());
}
inline T
fgMaxElem(const FgMatrixV<T> & mat)
{return fgMax(mat.dataVec()); }
FgMatrixV<T>
fgConcatHoriz(
const FgMatrixV<T> & left,
const FgMatrixV<T> & right)
{
if (left.empty())
return right;
if (right.empty())
return left;
FGASSERT(left.numRows() == right.numRows());
uint numRows = left.numRows(),
numCols = left.numCols() + right.numCols();
FgMatrixV<T> retval(numRows,numCols);
for (uint rr=0; rr<numRows; rr++)
{
uint col=0;
for (uint cc=0; cc<left.numCols(); ++cc)
retval.elem(rr,col++) = left.elem(rr,cc);
for (uint cc=0; cc<right.numCols(); ++cc)
retval.elem(rr,col++) = right.elem(rr,cc);
}
return retval;
}
FgMatrixV<T>
fgNormalize(const FgMatrixV<T> & m)
{return m * (1/std::sqrt(m.mag())); }
FgMatrixV<T>
fgConcatVert(
const FgMatrixV<T> & upper,
const FgMatrixV<T> & middle,
const FgMatrixV<T> & lower)
{
FGASSERT(upper.numCols() == middle.numCols());
FGASSERT(middle.numCols() == lower.numCols());
FgMatrixV<T> retval(upper.numRows()+middle.numRows()+lower.numRows(),upper.numCols());
uint row=0;
for (uint rr=0; rr<upper.numRows(); rr++)
for (uint col=0; col<upper.numCols(); col++)
retval.elem(row++,col) = upper.elem(rr,col);
for (uint rr=0; rr<middle.numRows(); rr++)
for (uint col=0; col<middle.numCols(); col++)
retval.elem(row++,col) = middle.elem(rr,col);
for (uint rr=0; rr<lower.numRows(); rr++)
for (uint col=0; col<lower.numCols(); col++)
retval.elem(row++,col) = lower.elem(rr,col);
return retval;
}
