From c7d95da9a80a4109f82738a009e5582c0ec081d9 Mon Sep 17 00:00:00 2001 From: "John M. Penn" Date: Mon, 19 Mar 2018 18:10:04 -0500 Subject: [PATCH] Implement function to generate a transformation matrix to rotate a vector to new a new orientation. Ref #582 --- include/trick/RodriguesRotation.h | 32 +++++++++++++ include/trick/trick_math_proto.h | 12 +---- .../trick_utils/math/src/RodriguesRotation.c | 45 +++++++++++++++++++ .../trick_utils/math/src/RodriquesRotation.c | 26 ----------- 4 files changed, 78 insertions(+), 37 deletions(-) create mode 100644 include/trick/RodriguesRotation.h create mode 100644 trick_source/trick_utils/math/src/RodriguesRotation.c delete mode 100644 trick_source/trick_utils/math/src/RodriquesRotation.c diff --git a/include/trick/RodriguesRotation.h b/include/trick/RodriguesRotation.h new file mode 100644 index 00000000..164d7ff7 --- /dev/null +++ b/include/trick/RodriguesRotation.h @@ -0,0 +1,32 @@ +#ifndef RODRIGUES_ROTATION_H +#define RODRIGUES_ROTATION_H + +#ifdef __cplusplus +extern "C" { +#endif + +/** +* @ingroup TRICK_MATH +* @brief Generate a transformation matrix for rotation about a given line by a given +* angle, using Rodrigues’ formula. +* +* @param C_out - Transformation matrix for final to initial state. +* @param k - Vector in the direction of the rotation Axis. +* @param theta - Angle of rotation in radians. +*/ +void RotAboutLineByAngle(double C_out[3][3], double k[3], double theta); + +/** +* @ingroup TRICK_MATH +* @brief Generate a transformation matrix to rotate a vector to new a new orientation. +* +* @param v - Original vector. +* @param w - Vector after rotation. +* @param R_out - Rotation matrix, such that w = Rv. +*/ +void RotVectorToNewOrientation(double R_out[3][3], double v[3], double w[3]); + +#ifdef __cplusplus +} +#endif +#endif diff --git a/include/trick/trick_math_proto.h b/include/trick/trick_math_proto.h index 1a7db021..9953bbb8 100644 --- a/include/trick/trick_math_proto.h +++ b/include/trick/trick_math_proto.h @@ -9,6 +9,7 @@ #include "trick/wave_form.h" #include "trick/reference_frame.h" #include "trick/rand_generator.h" +#include "trick/RodriguesRotation.h" #ifdef __cplusplus extern "C" { @@ -19,17 +20,6 @@ extern "C" { $TRICK_HOME/trick_source/trick_utils/math/include/trick_math_proto.h */ -/** -* @ingroup TRICK_MATH -* @brief Generate a transformation matrix for rotation about a given -* line by a given angle, using Rodrigues’ formula. -* -* @param C_out - Transformation matrix for final to initial state. -* @param k - Vector in the direction of the rotation Axis. -* @param theta - Angle of rotation in radians. -*/ -void RotAboutLineByAngle(double C_out[3][3], double k[3], double theta); - /** * @ingroup TRICK_MATH * Invert matrix using LU Decomposition method. diff --git a/trick_source/trick_utils/math/src/RodriguesRotation.c b/trick_source/trick_utils/math/src/RodriguesRotation.c new file mode 100644 index 00000000..1ee0a705 --- /dev/null +++ b/trick_source/trick_utils/math/src/RodriguesRotation.c @@ -0,0 +1,45 @@ +#include +#include +#include + +void RotAboutLineByAngle(double R_out[3][3], double k[3], double theta) { + + double c = cos(theta); + double a = 1.0 - c; + double b = sin(theta); + double n[3]; + + V_NORM(n, k); + + /* Calculate the rotation matrix using the Rodrigues formula. */ + R_out[0][0] = c + 0 + n[0]*n[0]*a; + R_out[0][1] = 0 - n[2]*b + n[0]*n[1]*a; + R_out[0][2] = 0 + n[1]*b + n[0]*n[2]*a; + + R_out[1][0] = 0 + n[2]*b + n[0]*n[1]*a; + R_out[1][1] = c + 0 + n[1]*n[1]*a; + R_out[1][2] = 0 - n[0]*b + n[1]*n[2]*a; + + R_out[2][0] = 0 - n[1]*b + n[0]*n[2]*a; + R_out[2][1] = 0 + n[0]*b + n[1]*n[2]*a; + R_out[2][2] = c + 0 + n[2]*n[2]*a; + +} + +#define SMALL_NUMBER 1e-10 + +void RotVectorToNewOrientation(double R_out[3][3], double v[3], double w[3]) { + + double a = V_DOT(v,v); + double b = V_DOT(v,w); + double theta = acos(b/a); + + if ( fabs(theta) < SMALL_NUMBER ) { + M_IDENT(R_out); + } else { + double k[3],n[3]; + V_CROSS(k, v, w); + V_NORM(n, k); + RotAboutLineByAngle(R_out, n, theta); + } +} diff --git a/trick_source/trick_utils/math/src/RodriquesRotation.c b/trick_source/trick_utils/math/src/RodriquesRotation.c deleted file mode 100644 index fbc90540..00000000 --- a/trick_source/trick_utils/math/src/RodriquesRotation.c +++ /dev/null @@ -1,26 +0,0 @@ -#include - -void RotAboutLineByAngle(double C_out[3][3], double k[3], double theta) { - - double c = cos(theta); - double a = 1.0 - c; - double b = sin(theta); - - /* Normalize the rotation axis vector */ - double length = sqrt(k[0]*k[0] + k[1]*k[1] + k[2]*k[2]); - double n[3] = {k[0]/length, k[1]/length, k[2]/length}; - - /* Calculate the rotation matrix using the Rodrigues formula. */ - C_out[0][0] = c + 0 + n[0]*n[0]*a; - C_out[0][1] = 0 - n[2]*b + n[0]*n[1]*a; - C_out[0][2] = 0 + n[1]*b + n[0]*n[2]*a; - - C_out[1][0] = 0 + n[2]*b + n[0]*n[1]*a; - C_out[1][1] = c + 0 + n[1]*n[1]*a; - C_out[1][2] = 0 - n[0]*b + n[1]*n[2]*a; - - C_out[2][0] = 0 - n[1]*b + n[0]*n[2]*a; - C_out[2][1] = 0 + n[0]*b + n[1]*n[2]*a; - C_out[2][2] = c + 0 + n[2]*n[2]*a; - -}