double iwishpdf(gsl_matrix *X, gsl_matrix *Scale, gsl_matrix *inv, double dof)
{
double X_det, scale_det, denom, pdf, trace, numerator;
int m = X->size1;
int n = X->size2;
// Allocate matrix for inv(X)
gsl_matrix *for_mult = gsl_matrix_alloc(m, n);
// Get determinant of X and Scale matrix
X_det = matrix_determ(X);
scale_det = matrix_determ(Scale);
// Invert X
inv_matrix(X,inv);
// Multiple Scale * inv(X)
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Scale, inv, 1.0, for_mult);
// Get trace of above.
trace = matrix_trace(for_mult);
numerator = pow(scale_det, dof / 2.0) * pow(X_det, (-dof-m-1)/ 2.0) * exp(-0.5 * trace);
denom = pow(2,dof * m / 2) * mv_gamma(dof/2, m);
pdf = (numerator/denom);
return(pdf);
}
TEST_F(QuaternionTest, creating_quaternion_from_positive_diagonal_matrix_gives_real_part_as_half_of_the_squared_trace_and_zero_imaginary_part)
{
const auto matrix = create_positive_diagonal_matrix();
const Math::Quaternion<double> quat(matrix);
EXPECT_EQ(0.5 * std::sqrt(matrix_trace(matrix)), quat.w());
EXPECT_EQ(0, quat.x());
EXPECT_EQ(0, quat.y());
EXPECT_EQ(0, quat.x());
}
Math::Quaternion<double> create_quaternion_from_small_real_component(const Math::Matrix4d& matrix)
{
Math::Quaternion<double> result;
result.w() = 0.5 * std::sqrt(matrix_trace(matrix));
result.x() = 0.5 * std::sqrt(matrix(0,0) - matrix(1,1) - matrix(2,2) + matrix(3,3));
result.y() = 0.5 * std::sqrt(-matrix(0,0) + matrix(1,1) - matrix(2,2) + matrix(3,3));
result.z() = 0.5 * std::sqrt(-matrix(0,0) - matrix(1,1) + matrix(2,2) + matrix(3,3));
return result;
}
Math::Quaternion<double> create_quaternion_from_large_real_component(const Math::Matrix4d& matrix)
{
Math::Quaternion<double> result;
result.w() = 0.5 * std::sqrt(matrix_trace(matrix));
result.x() = 0.25 *(matrix(2,1) - matrix(1,2))/result.w();
result.y() = 0.25 *(matrix(0,2) - matrix(2,0))/result.w();
result.z() = 0.25 *(matrix(1,0) - matrix(0,1))/result.w();
return result;
}
