ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2025-01-18 02:40:08 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Created a function function to calculate transform using Rodrigues formula. Ref #567

Browse Source
This commit is contained in:
John M. Penn 2018-02-27 18:24:40 -06:00
parent 8bdcd28631
commit 4f0093ddd6
2 changed files with 37 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
include/trick/trick_math_proto.h
View File

@ -19,6 +19,17 @@ 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.

26
trick_source/trick_utils/math/src/RodriquesRotation.c Normal file
View File

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