34 T& rx, T& ry, T& rz) {
43 T norm =
sqrt(vx*vx + vy*vy + vz*vz);
55 geometry_msgs::Vector3 A,
56 geometry_msgs::Vector3 E,
57 geometry_msgs::Quaternion& orientation) {
64 float Ax = A.x, Ay = A.y, Az = A.z;
67 float Ex = E.x, Ey = E.y, Ez = E.z;
102 R[0][0] = Mx; R[0][1] = Hx; R[0][2] = -Ax;
103 R[1][0] = My; R[1][1] = Hy; R[1][2] = -Ay;
104 R[2][0] = Mz; R[2][1] = Hz; R[2][2] = -Az;
117 R[0][0] = Mx; R[0][1] = -Hx; R[0][2] = Ax;
118 R[1][0] = My; R[1][1] = -Hy; R[1][2] = Ay;
119 R[2][0] = Mz; R[2][1] = -Hz; R[2][2] = Az;
133 R[0][0] = Hx; R[0][1] = Mx; R[0][2] = Ax;
134 R[1][0] = Hy; R[1][1] = My; R[1][2] = Ay;
135 R[2][0] = Hz; R[2][1] = Mz; R[2][2] = Az;
150 geometry_msgs::Vector3 A,
151 geometry_msgs::Quaternion& orientation) {
157 geometry_msgs::Vector3 E;
158 if (fabs(A.x) > 0.1 || fabs(A.y) > 0.1) {
162 }
else if (fabs(A.z) > 0.1) {
Quaternion inverse() const
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
static bool computeOrientation(WorldFrame::WorldFrame frame, geometry_msgs::Vector3 acceleration, geometry_msgs::Vector3 magneticField, geometry_msgs::Quaternion &orientation)
void getRotation(Quaternion &q) const
static void crossProduct(T ax, T ay, T az, T bx, T by, T bz, T &rx, T &ry, T &rz)
void convert(const A &a, B &b)
static T normalizeVector(T &vx, T &vy, T &vz)