Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <OgreRay.h>
00031 #include <OgrePlane.h>
00032 #include <OgreCamera.h>
00033 #include <OgreSceneNode.h>
00034 #include <OgreViewport.h>
00035
00036 #include "geometry.h"
00037
00038 namespace rviz
00039 {
00040
00044 bool getPointOnPlaneFromWindowXY( Ogre::Viewport* viewport,
00045 Ogre::Plane& plane,
00046 int window_x, int window_y,
00047 Ogre::Vector3& intersection_out )
00048 {
00049 int width = viewport->getActualWidth();
00050 int height = viewport->getActualHeight();
00051
00052 Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay( (float)window_x / (float)width,
00053 (float)window_y / (float)height );
00054 std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects( plane );
00055 if ( !intersection.first )
00056 {
00057 return false;
00058 }
00059 intersection_out = mouse_ray.getPoint( intersection.second );
00060
00061 return true;
00062 }
00063
00064 float mapAngleTo0_2Pi( float angle )
00065 {
00066 angle = fmod( angle, Ogre::Math::TWO_PI );
00067
00068 if( angle < 0.0f )
00069 {
00070 angle = Ogre::Math::TWO_PI + angle;
00071 }
00072 return angle;
00073 }
00074
00075 Ogre::Vector2 project3DPointToViewportXY(const Ogre::Viewport* view, const Ogre::Vector3& pos)
00076 {
00077 Ogre::Camera* cam = view->getCamera();
00078 Ogre::Vector3 pos2D = cam->getProjectionMatrix() * (cam->getViewMatrix() * pos);
00079
00080 Ogre::Real x = ((pos2D.x * 0.5) + 0.5);
00081 Ogre::Real y = 1 - ((pos2D.y * 0.5) + 0.5);
00082
00083 return Ogre::Vector2(x * view->getActualWidth(), y * view->getActualHeight());
00084 }
00085
00086 }