From 51269217ef086a38c1244c3f7154c1efef191449 Mon Sep 17 00:00:00 2001 From: Helen Ginn Date: Fri, 27 Apr 2018 22:24:52 +0200 Subject: decompose rotation matrix into angle and axis --- libcrystfel/src/taketwo.c | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) (limited to 'libcrystfel') diff --git a/libcrystfel/src/taketwo.c b/libcrystfel/src/taketwo.c index 082e7886..a24a30c0 100644 --- a/libcrystfel/src/taketwo.c +++ b/libcrystfel/src/taketwo.c @@ -406,6 +406,32 @@ static void closest_rot_mat(struct rvec vec1, struct rvec vec2, rotation_around_axis(axis, bestAngle, twizzle); } +static double matrix_angle(gsl_matrix *m) +{ + double a = gsl_matrix_get(m, 0, 0); + double b = gsl_matrix_get(m, 1, 1); + double c = gsl_matrix_get(m, 2, 2); + + double cos_t = (a + b + c - 1) / 2; + double theta = acos(cos_t); + + return theta; +} + +static struct rvec matrix_axis(gsl_matrix *a) +{ + double ang = matrix_angle(a); + double cos_t = cos(ang); + double p = gsl_matrix_get(a, 0, 0); + double q = gsl_matrix_get(a, 1, 1); + double r = gsl_matrix_get(a, 2, 2); + double x = sqrt((p - cos_t) / (1 - cos_t)); + double y = sqrt((q - cos_t) / (1 - cos_t)); + double z = sqrt((r - cos_t) / (1 - cos_t)); + + struct rvec v = new_rvec(x, y, z); + return v; +} static double matrix_trace(gsl_matrix *a) { -- cgit v1.2.3