30 #ifndef RVIZ_VALIDATE_QUATERNIONS_H 31 #define RVIZ_VALIDATE_QUATERNIONS_H 33 #include <geometry_msgs/PoseStamped.h> 34 #include <OgreQuaternion.h> 38 #include <boost/array.hpp> 48 return w * w + x * x + y * y + z * z;
53 if ( 0.0
f == x && 0.0
f == y && 0.0
f == z && 0.0
f == w )
60 ROS_DEBUG_COND_NAMED( !is_normalized,
"quaternions",
"Quaternion [x: %.3f, y: %.3f, z: %.3f, w: %.3f] not normalized. " 61 "Magnitude: %.3f", x, y, z, w, std::sqrt(norm2) );
67 if ( 0.0
f == x && 0.0
f == y && 0.0
f == z && 0.0
f == w )
73 norm2 = std::sqrt( norm2 );
74 float invnorm = 1.0f / norm2;
85 return w * w + x * x + y * y + z * z;
90 if ( 0.0 == x && 0.0 == y && 0.0 == z && 0.0 == w )
97 ROS_DEBUG_COND_NAMED( !is_normalized,
"quaternions",
"Quaternion [x: %.3f, y: %.3f, z: %.3f, w: %.3f] not normalized. " 98 "Magnitude: %.3f", x, y, z, w, std::sqrt(norm2) );
104 if ( 0.0 == x && 0.0 == y && 0.0 == z && 0.0 == w )
110 norm2 = std::sqrt( norm2 );
111 double invnorm = 1.0 / norm2;
127 return validateQuaternions( quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z());
162 typedef std::vector<T> VecType;
163 typename VecType::const_iterator it = vec.begin();
164 typename VecType::const_iterator end = vec.end();
165 for ( ; it != end; ++it )
176 template<
typename T,
size_t N>
179 typedef boost::array<T, N> ArrType;
180 typename ArrType::const_iterator it = arr.begin();
181 typename ArrType::const_iterator end = arr.end();
182 for ( ; it != end; ++it )
195 #endif // RVIZ_VALIDATE_QUATERNIONS_H
#define ROS_DEBUG_COND_NAMED(cond, name,...)
float quaternionNorm2(float w, float x, float y, float z)
float normalizeQuaternion(float &w, float &x, float &y, float &z)
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
static double QUATERNION_NORMALIZATION_TOLERANCE