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,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,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,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;
}
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;
}
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;
}
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);
}
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;
}
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,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;
}
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,3,3>
fgMatRotateAxis( // RHR rotation around an arbitrary axis
T radians,
const FgMatrixC<T,3,1> &axis) // vector describing that axis
{
FgMatrixC<T,3,3> mat;
FgTypeAttributeFloatingS<T>();
T len = axis.length();
FGASSERT(len > T(0));
FgMatrixC<T,3,1> ax = axis * (T(1) / len); // Ensure the rotation axis is normalized
T ct = (T)cos(radians);
T st = (T)sin(radians);
T vt = T(1)-ct;
mat.elem(0,0) = ax[0]*ax[0]*vt + ct;
mat.elem(0,1) = ax[0]*ax[1]*vt - ax[2]*st;
mat.elem(0,2) = ax[0]*ax[2]*vt + ax[1]*st;
mat.elem(1,0) = ax[0]*ax[1]*vt + ax[2]*st;
mat.elem(1,1) = ax[1]*ax[1]*vt + ct;
mat.elem(1,2) = ax[1]*ax[2]*vt - ax[0]*st;
mat.elem(2,0) = ax[0]*ax[2]*vt - ax[1]*st;
mat.elem(2,1) = ax[1]*ax[2]*vt + ax[0]*st;
mat.elem(2,2) = ax[2]*ax[2]*vt + ct;
return mat;
}
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,3,3>
fgMatRotateZ(T radians) // RHR rotation around Z axis
{
FgMatrixC<T,3,3> 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(0,2)=0.0;
mat.elem(1,0)=st; mat.elem(1,1)=ct; mat.elem(1,2)=0.0;
mat.elem(2,0)=0.0; mat.elem(2,1)=0.0; mat.elem(2,2)=1.0;
return mat;
}
FgMatrixC<T,nrows,ncols1+ncols2+ncols3>
fgConcatHoriz(
const FgMatrixC<T,nrows,ncols1> & m1,
const FgMatrixC<T,nrows,ncols2> & m2,
const FgMatrixC<T,nrows,ncols3> & m3)
{
FgMatrixC<T,nrows,ncols1+ncols2+ncols3> ret;
for (uint row=0; row<nrows; ++row)
{
uint col=0;
for (uint cc=0; cc<ncols1; ++cc)
ret.elem(row,col++) = m1.elem(row,cc);
for (uint cc=0; cc<ncols2; ++cc)
ret.elem(row,col++) = m2.elem(row,cc);
for (uint cc=0; cc<ncols3; ++cc)
ret.elem(row,col++) = m3.elem(row,cc);
}
return ret;
}
FgMatrixC<T,3,3> fgMatInverse(
const FgMatrixC<T,3,3>& m)
{
FgTypeAttributeFloatingS<T>();
FgMatrixC<T,3,3> r;
T fac = (m.elem(0,0)*m.elem(1,1)*m.elem(2,2) - m.elem(0,0)*m.elem(1,2)*m.elem(2,1) +
m.elem(1,0)*m.elem(0,2)*m.elem(2,1) - m.elem(1,0)*m.elem(0,1)*m.elem(2,2) +
m.elem(2,0)*m.elem(0,1)*m.elem(1,2) - m.elem(2,0)*m.elem(1,1)*m.elem(0,2));
FGASSERT(fac != T(0));
r.elem(0,0) = m.elem(1,1) * m.elem(2,2) - m.elem(1,2) * m.elem(2,1);
r.elem(0,1) = m.elem(0,2) * m.elem(2,1) - m.elem(0,1) * m.elem(2,2);
r.elem(0,2) = m.elem(0,1) * m.elem(1,2) - m.elem(0,2) * m.elem(1,1);
r.elem(1,0) = m.elem(1,2) * m.elem(2,0) - m.elem(1,0) * m.elem(2,2);
r.elem(1,1) = m.elem(0,0) * m.elem(2,2) - m.elem(0,2) * m.elem(2,0);
r.elem(1,2) = m.elem(0,2) * m.elem(1,0) - m.elem(0,0) * m.elem(1,2);
r.elem(2,0) = m.elem(1,0) * m.elem(2,1) - m.elem(1,1) * m.elem(2,0);
r.elem(2,1) = m.elem(0,1) * m.elem(2,0) - m.elem(0,0) * m.elem(2,1);
r.elem(2,2) = m.elem(0,0) * m.elem(1,1) - m.elem(1,0) * m.elem(0,1);
r *= T(1) / fac;
return r;
}
