rviz: common.h Source File

00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #ifndef RVIZ_COMMON_H
00031 #define RVIZ_COMMON_H
00032 
00033 #include <OGRE/OgreVector3.h>
00034 #include <OGRE/OgreQuaternion.h>
00035 #include <OGRE/OgreMatrix3.h>
00036 
00037 #include <boost/function.hpp>
00038 
00043 namespace rviz
00044 {
00045 
00046 extern Ogre::Matrix3 g_ogre_to_robot_matrix;
00047 extern Ogre::Matrix3 g_robot_to_ogre_matrix;
00048 
00049 extern Ogre::Quaternion g_ogre_to_robot_quat;
00050 extern Ogre::Quaternion g_robot_to_ogre_quat;
00051 
00055 void initializeCommon();
00056 
00061 inline void robotToOgre( Ogre::Vector3& point )
00062 {
00063   point = g_robot_to_ogre_matrix * point;
00064 }
00065 
00070 inline void scaleRobotToOgre( Ogre::Vector3& scale )
00071 {
00072   scale = g_robot_to_ogre_matrix * scale;
00073 
00074   scale.x = fabsf( scale.x );
00075   scale.y = fabsf( scale.y );
00076   scale.z = fabsf( scale.z );
00077 }
00078 
00083 inline void robotToOgre( Ogre::Quaternion& quat )
00084 {
00085   quat = g_robot_to_ogre_quat * quat;
00086 }
00087 
00092 inline void robotToOgre( Ogre::Matrix3& mat )
00093 {
00094   mat = g_robot_to_ogre_matrix * mat;
00095 }
00096 
00097 
00102 inline void ogreToRobot( Ogre::Vector3& point )
00103 {
00104   point = g_ogre_to_robot_matrix * point;
00105 }
00106 
00111 inline void scaleOgreToRobot( Ogre::Vector3& scale )
00112 {
00113   scale = g_ogre_to_robot_matrix * scale;
00114 
00115   scale.x = fabsf( scale.x );
00116   scale.y = fabsf( scale.y );
00117   scale.z = fabsf( scale.z );
00118 }
00119 
00124 inline void ogreToRobot( Ogre::Quaternion& quat )
00125 {
00126   quat = g_ogre_to_robot_quat * quat;
00127 }
00128 
00133 inline void ogreToRobot( Ogre::Matrix3& mat )
00134 {
00135   mat = g_ogre_to_robot_matrix * mat;
00136 }
00137 
00145 inline Ogre::Matrix3 ogreMatrixFromRobotEulers( float yaw, float pitch, float roll )
00146 {
00147   Ogre::Matrix3 mat;
00148   mat.FromEulerAnglesZYX( Ogre::Radian( yaw ), Ogre::Radian( pitch ), Ogre::Radian( roll ) );
00149   robotToOgre( mat );
00150 
00151   return mat;
00152 }
00153 
00154 }
00155 #endif