Implement function to generate a transformation matrix to rotate a vector to new a new orientation. Ref #582

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

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.

trick_source/trick_utils/math/src/RodriguesRotation.c

@@ -0,0 +1,45 @@
+#include <math.h>
+#include <trick/vector_macros.h>
+#include <trick/matrix_macros.h>
+
+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);
+    }
+}

trick_source/trick_utils/math/src/RodriquesRotation.c

@@ -1,26 +0,0 @@
-#include <math.h>
-
-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;
-
-}