convert.h
Go to the documentation of this 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_RENDER_CONVERT_H
00031 #define RVIZ_RENDER_CONVERT_H
00032 
00033 #include <OGRE/OgreVector3.h>
00034 #include <OGRE/OgreQuaternion.h>
00035 #include <OGRE/OgreMatrix3.h>
00036 #include <OGRE/OgreMatrix4.h>
00037 
00038 #include <rve_msgs/Matrix4.h>
00039 #include <rve_msgs/Vector3.h>
00040 #include <rve_msgs/Quaternion.h>
00041 
00046 namespace rve_render_server
00047 {
00048 
00049 extern Ogre::Matrix3 g_ogre_to_robot_matrix;
00050 extern Ogre::Matrix3 g_robot_to_ogre_matrix;
00051 extern Ogre::Matrix3 g_robot_to_ogre_normal_matrix;
00052 
00053 extern Ogre::Quaternion g_ogre_to_robot_quat;
00054 extern Ogre::Quaternion g_robot_to_ogre_quat;
00055 
00056 inline Ogre::Vector3 convert(const rve_msgs::Vector3& v)
00057 {
00058   return Ogre::Vector3(v.x, v.y, v.z);
00059 }
00060 
00061 inline Ogre::Quaternion convert(const rve_msgs::Quaternion& q)
00062 {
00063   return Ogre::Quaternion(q.w, q.x, q.y, q.z);
00064 }
00065 
00066 inline Ogre::Matrix4 convert(const rve_msgs::Matrix4& m)
00067 {
00068   return Ogre::Matrix4(m.m[0],m.m[1],m.m[2],m.m[3],
00069                        m.m[4],m.m[5],m.m[6],m.m[7],
00070                        m.m[8],m.m[9],m.m[10],m.m[11],
00071                        m.m[12],m.m[13],m.m[14],m.m[15]);
00072 }
00073 
00078 inline Ogre::Vector3 fromRobot( const rve_msgs::Vector3& point )
00079 {
00080   return g_robot_to_ogre_matrix * convert(point);
00081 }
00082 
00083 inline Ogre::Vector3 fromRobot( const Ogre::Vector3& point )
00084 {
00085   return g_robot_to_ogre_matrix * point;
00086 }
00087 
00088 inline Ogre::Vector3 normalFromRobot(const Ogre::Vector3& normal)
00089 {
00090   return g_robot_to_ogre_normal_matrix * normal;
00091 }
00092 
00097 inline Ogre::Vector3 scaleFromRobot( const Ogre::Vector3& scale )
00098 {
00099   return Ogre::Vector3(fabsf(-scale.y), fabsf(scale.z), fabsf(-scale.x));
00100 }
00101 
00102 inline Ogre::Vector3 scaleFromRobot( const rve_msgs::Vector3& scale )
00103 {
00104   return scaleFromRobot(convert(scale));
00105 }
00106 
00111 inline Ogre::Quaternion fromRobot( const rve_msgs::Quaternion& quat )
00112 {
00113   return g_robot_to_ogre_quat * convert(quat);
00114 }
00115 
00116 inline Ogre::Quaternion fromRobot(const Ogre::Quaternion& quat)
00117 {
00118   return g_robot_to_ogre_quat * quat;
00119 }
00120 
00121 inline Ogre::Quaternion fromRobotRelative(const rve_msgs::Quaternion& quat)
00122 {
00123   return Ogre::Quaternion(quat.w, -quat.y, quat.z, -quat.x);
00124 }
00125 
00126 inline Ogre::Quaternion fromRobotRelative(const Ogre::Quaternion& quat)
00127 {
00128   return Ogre::Quaternion(quat.w, -quat.y, quat.z, -quat.x);
00129 }
00130 
00131 } // namespace rve_render_server
00132 #endif


rve_render_server
Author(s): Josh Faust
autogenerated on Wed Dec 11 2013 14:31:15