mirror of
https://github.com/nasa/trick.git
synced 2025-01-26 06:09:18 +00:00
46 lines
1.1 KiB
C
46 lines
1.1 KiB
C
#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);
|
|
}
|
|
}
|