bool // false if zero magnitude
normalize() // Useful for deserialization of user data
{
T mag = m_real*m_real + m_comp.lengthSqr();
if (mag == T(0))
return false;
T fac = T(1) / sqrt(mag);
m_real *= fac;
m_comp *= fac;
return true;
}
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());
}
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()));
}
T
fgLengthSqr(const FgMatrixC<T,nrows,ncols> & m)
{return m.lengthSqr(); }
