FgMatrixC<T,dim,1>
fgClamp(FgMatrixC<T,dim,1> val,FgMatrixC<T,dim,2> bounds)
{
for (uint ii=0; ii<dim; ++ii)
val[ii] = fgClamp(val[ii],bounds.elem(ii,0),bounds.elem(ii,1));
return val;
}
FgMatrixC<T,sz,sz>
fgDiagonal(FgMatrixC<T,sz,1> vec)
{
FgMatrixC<T,sz,sz> ret;
for (uint ii=0; ii<sz; ++ii)
ret.elem(ii,ii) = vec[ii];
return ret;
}
FgAffineC<T,dim>
asAffine() const
{
FgMatrixC<T,dim,dim> scales;
for (uint ii=0; ii<dim; ++ii)
scales.elm(ii,ii) = m_scales[ii];
return FgAffineC<T,dim>(scales,m_trans);
}
FgMatrixC<T,3,3>
fgPermuteAxes(uint axisToBecomeX)
{
FgMatrixC<T,3,3> ret;
for (uint ii=0; ii<3; ++ii)
ret.elem(ii,(ii+axisToBecomeX)%3) = T(1);
return ret;
}
FgMatrixC<T,dims+1,dims+1>
fgAsHomogMat(const FgMatrixC<T,dims,1> & translation)
{
FgMatrixC<T,dims+1,dims+1> ret;
ret.setIdentity();
for (uint rr=0; rr<dims; rr++)
ret.elem(rr,dims) = translation[rr];
return ret;
}
FgMatrixC<T,dim,ncols>
operator*(const FgMatrixC<T,dim,ncols> & vec) const
{
FgMatrixC<T,dim,ncols> ret;
for (uint row=0; row<dim; ++row)
for (uint col=0; col<ncols; ++col)
ret.elm(col,row) = m_scales[row] * (vec.elm(col,row) + m_trans[row]);
return ret;
}
FgMatrixC<T,ncols,nrows>
transpose() const
{
FgMatrixC<T,ncols,nrows> tMat;
for (uint ii=0; ii<nrows; ii++)
for (uint jj=0; jj<ncols; jj++)
tMat.elem(jj,ii) = elem(ii,jj);
return tMat;
}
FgMatrixC<T,nrows,1>
fgMaxColwise(const FgMatrixC<T,nrows,ncols> & mat)
{
FG_STATIC_ASSERT(ncols > 1);
FgMatrixC<T,nrows,1> ret(mat.colVec(0));
for (uint row=0; row<nrows; ++row)
for (uint col=1; col<ncols; ++col)
fgSetIfGreater(ret[row],mat.elm(col,row));
return ret;
}
FgMatrixC<T,dims+1,dims+1>
fgAsHomogMat(const FgMatrixC<T,dims,dims> & linear)
{
FgMatrixC<T,dims+1,dims+1> ret;
for (uint rr=0; rr<dims; ++rr)
for (uint cc=0; cc<dims; ++cc)
ret.elem(rr,cc) = linear.elem(rr,cc);
ret.elem(dims,dims) = T(1);
return ret;
}
FgMatrixC<T,2,2>
fgMatRotate(T radians)
{
FgMatrixC<T,2,2> mat;
FgTypeAttributeFloatingS<T>();
T ct = T(cos(radians));
T st = T(sin(radians));
mat.elem(0,0)=ct; mat.elem(0,1)=-st;
mat.elem(1,0)=st; mat.elem(1,1)=ct;
return mat;
}
FgMatrixC<T,1,2>
fgBounds(const FgMatrixC<T,nrows,ncols> & mat)
{
FGASSERT(mat.numElems() > 0);
FgMatrixC<T,1,2> ret(mat[0]);
for (uint ii=0; ii<mat.numElems(); ++ii)
{
fgSetIfLess (ret[0],mat[ii]);
fgSetIfGreater (ret[1],mat[ii]);
}
return ret;
}
FgMatrixC<T,dim,2>
fgCubeBounds(const vector<FgMatrixC<T,dim,1> > & verts,T padRatio=1)
{
FgMatrixC<T,dim,2> ret;
FgMatrixC<T,dim,2> bounds = fgBounds(verts);
FgMatrixC<T,dim,1> lo = bounds.colVec(0),
hi = bounds.colVec(1),
centre = (lo + hi) * T(0.5);
T hsize = fgMaxElem(hi - lo) * 0.5f * padRatio;
ret = fgConcatHoriz(centre-FgMatrixC<T,dim,1>(hsize),centre+FgMatrixC<T,dim,1>(hsize));
return ret;
}
FgMatrixC<T,dim,2>
fgBoundsIntersection(
const FgMatrixC<T,dim,2> & b1,
const FgMatrixC<T,dim,2> & b2)
{
FgMatrixC<T,dim,2> ret(b1);
for (uint dd=0; dd<dim; ++dd)
{
fgSetIfGreater(ret.elm(0,dd),b2.elm(0,dd));
fgSetIfLess(ret.elm(1,dd),b2.elm(1,dd));
}
return ret;
}
FgMatrixC<T,2,2> fgMatInverse(
const FgMatrixC<T,2,2>& m)
{
FgTypeAttributeFloatingS<T>();
FgMatrixC<T,2,2> retval;
T fac = (m.elem(0,0) * m.elem(1,1) - m.elem(0,1) * m.elem(1,0));
FGASSERT(fac != T(0));
fac = T(1) / fac;
retval.elem(0,0) = m.elem(1,1) * fac;
retval.elem(0,1) = - m.elem(0,1) * fac;
retval.elem(1,0) = - m.elem(1,0) * fac;
retval.elem(1,1) = m.elem(0,0) * fac;
return retval;
}
double
lnNormalCholesky(
const FgMatrixC<double,dim,1> & pos,
const FgMatrixC<double,dim,1> & mean,
const FgMatrixC<double,dim,dim> & chol)
{
double det = 1.0;
for (uint ii=0; ii<dim; ii++)
det *= chol.elem(dim,dim);
FgMatrixC<double,dim,1> mhlbs = chol * (pos-mean);
return (0.5 * std::log(det) - // Cholesky has all diagonals > 0
0.5 * double(dim) * fgLn_2pi() -
0.5 * mhlbs.lengthSqr());
}
FgMatrixC<T,nrows1+nrows2,ncols>
fgConcatVert(
const FgMatrixC<T,nrows1,ncols> & upper,
const FgMatrixC<T,nrows2,ncols> & lower)
{
FgMatrixC<T,nrows1+nrows2,ncols> ret;
for (uint rr=0; rr<nrows1; ++rr)
for (uint cc=0; cc<ncols; ++cc)
ret.elem(rr,cc) = upper.elem(rr,cc);
for (uint rr=0; rr<nrows2; ++rr)
for (uint cc=0; cc<ncols; ++cc)
ret.elem(nrows1+rr,cc) = lower.elem(rr,cc);
return ret;
}
FgMatrixC<T,dims+1,dims+1>
fgAsHomogMat(
const FgMatrixC<T,dims,dims> & linTrans,
const FgMatrixC<T,dims,1> & translation)
{
FgMatrixC<T,dims+1,dims+1> ret;
for (uint rr=0; rr<dims; rr++)
for (uint cc=0; cc<dims; cc++)
ret.elem(rr,cc) = linTrans.elem(rr,cc);
for (uint rr=0; rr<dims; rr++)
ret.elem(rr,dims) = translation[rr];
ret.elem(dims,dims) = 1;
return ret;
}
FgMatrixC<T,dim,2>
fgBounds(const std::vector<FgMatrixC<T,dim,1> > & data)
{
FGASSERT(data.size() > 0);
FgMatrixC<T,dim,2> ret;
ret.setSubMatrix(data[0],0,0);
ret.setSubMatrix(data[0],0,1);
for (size_t ii=1; ii<data.size(); ++ii) {
for (uint dd=0; dd<dim; ++dd) {
fgSetIfLess (ret.elm(0,dd),data[ii][dd]);
fgSetIfGreater (ret.elm(1,dd),data[ii][dd]);
}
}
return ret;
}
double
normalCholesky(
const FgMatrixC<double,dim,1> & pos,
const FgMatrixC<double,dim,1> & mean,
const FgMatrixC<double,dim,dim> & chol) // Right-hand cholesky term of concentration
{
double det = 1.0;
for (uint ii=0; ii<dim; ii++)
det *= chol.elem(ii,ii); // Cholesky is upper or lower triangular.
FgMatrixC<double,dim,1> mhlbs = chol * (pos-mean);
return (
std::pow(2.0 * fgPi(),double(dim) * -0.5) *
std::sqrt(det) *
fgExp(-0.5 * mhlbs.lengthSqr()));
}
FgMatrixC<T,dim,2>
fgBounds(
const FgMatrixC<T,dim,1> & v0,
const FgMatrixC<T,dim,1> & v1,
const FgMatrixC<T,dim,1> & v2)
{
FgMatrixC<T,dim,2> ret;
for (uint dd=0; dd<dim; ++dd) {
ret.elm(0,dd) = ret.elm(1,dd) = v0[dd];
fgSetIfLess(ret.elm(0,dd),v1[dd]);
fgSetIfLess(ret.elm(0,dd),v2[dd]);
fgSetIfGreater(ret.elm(1,dd),v1[dd]);
fgSetIfGreater(ret.elm(1,dd),v2[dd]);
}
return ret;
}
FgMatrixC<T,nrows,ncols1+ncols2>
fgConcatHoriz(
const FgMatrixC<T,nrows,ncols1> & lhs,
const FgMatrixC<T,nrows,ncols2> & rhs)
{
FgMatrixC<T,nrows,ncols1+ncols2> ret;
for (uint row=0; row<nrows; ++row)
{
uint col=0;
for (uint cc=0; cc<ncols1; ++cc)
ret.elem(row,col++) = lhs.elem(row,cc);
for (uint cc=0; cc<ncols2; ++cc)
ret.elem(row,col++) = rhs.elem(row,cc);
}
return ret;
}
bool
fgBoundsIntersect(
const FgMatrixC<T,dim,2> & bnds1,
const FgMatrixC<T,dim,2> & bnds2,
FgMatrixC<T,dim,2> & retval)
{
FgMatrixC<T,dim,2> tmp;
for (uint dd=0; dd<dim; ++dd)
{
tmp.elm(0,dd) = std::max(bnds1.elm(0,dd),bnds2.elm(0,dd));
tmp.elm(1,dd) = std::min(bnds1.elm(1,dd),bnds2.elm(1,dd));
if (tmp.elm(0,dd) > tmp.elm(1,dd))
return false;
}
retval = tmp;
return true;
}
void
fgRound_(
const FgMatrixC<Flt,nrows,ncols> & lhs,
FgMatrixC<Int,nrows,ncols> & rhs)
{
for (uint ii=0; ii<rhs.numElems(); ++ii)
fgRound_(lhs[ii],rhs[ii]);
}
FgMatrixC<int,nrows,ncols>
fgRound(FgMatrixC<double,nrows,ncols> m)
{
FgMatrixC<int,nrows,ncols> ret;
for (uint ii=0; ii<m.numElems(); ++ii)
ret[ii] = fgRound(m[ii]);
return ret;
}
FgMatrixC<T,nrows,ncols>
fgFloor(const FgMatrixC<T,nrows,ncols> & mat)
{
FgMatrixC<T,nrows,ncols> ret;
for (uint ii=0; ii<mat.numElems(); ++ii)
ret[ii] = std::floor(mat[ii]);
return ret;
}
FgMatrixC<uint,nrows,ncols>
fgRoundU(FgMatrixC<float,nrows,ncols> m)
{
FgMatrixC<uint,nrows,ncols> ret;
for (uint ii=0; ii<m.numElems(); ++ii)
ret[ii] = fgRoundU(m[ii]);
return ret;
}
void
setSubMat(const FgMatrixC<T,srows,scols> & sub,uint row,uint col)
{
FGASSERT((srows+row <= nrows) && (scols+col <= ncols));
for (uint rr=0; rr<srows; rr++)
for (uint cc=0; cc<scols; cc++)
elem(rr+row,cc+col) = sub.elem(rr,cc);
}
void
accSubMat(size_t row,size_t col,FgMatrixC<T,mrows,mcols> m)
{
FGASSERT((mrows+row <= nrows) && (mcols+col <= ncols));
for (uint rr=0; rr<mrows; ++rr)
for (uint cc=0; cc<mcols; ++cc)
elem(row+rr,col+cc) += m.elem(rr,cc);
}
T
fgTrace(const FgMatrixC<T,dim,dim> & m)
{
T ret(0);
for (uint ii=0; ii<m.numElems(); ii+=dim+1)
ret += m[ii];
return ret;
}
T
fgMinElem(const FgMatrixC<T,nrows,ncols> & mat)
{
T ret(mat[0]);
size_t sz = mat.size();
for (size_t ii=1; ii<sz; ++ii)
fgSetIfLess(ret,mat[ii]);
return ret;
}
