52 lines
1.2 KiB
C++
52 lines
1.2 KiB
C++
/**
|
|
* @file inertialUtils.cpp
|
|
*
|
|
* @date Nov 28, 2011
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <gtsam_unstable/dynamics/inertialUtils.h>
|
|
|
|
namespace gtsam {
|
|
namespace dynamics {
|
|
|
|
/* ************************************************************************* */
|
|
Matrix RRTMbn(const Vector& euler) {
|
|
assert(euler.size() == 3);
|
|
const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1));
|
|
const double t2 = tan(euler(2-1)), c2 = cos(euler(2-1));
|
|
Matrix Ebn(3,3);
|
|
Ebn << 1.0, s1 * t2, c1 * t2,
|
|
0.0, c1, -s1,
|
|
0.0, s1 / c2, c1 / c2;
|
|
return Ebn;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Matrix RRTMbn(const Rot3& att) {
|
|
return RRTMbn(att.rpy());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Matrix RRTMnb(const Vector& euler) {
|
|
assert(euler.size() == 3);
|
|
Matrix Enb(3,3);
|
|
const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1));
|
|
const double s2 = sin(euler(2-1)), c2 = cos(euler(2-1));
|
|
Enb << 1.0, 0.0, -s2,
|
|
0.0, c1, s1*c2,
|
|
0.0, -s1, c1*c2;
|
|
return Enb;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Matrix RRTMnb(const Rot3& att) {
|
|
return RRTMnb(att.rpy());
|
|
}
|
|
|
|
} // \namespace dynamics
|
|
} // \namespace gtsam
|
|
|
|
|
|
|