$search
00001 /* 00002 * Copyright (c) 2009, 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 #include "fps_view_controller.h" 00031 #include "rviz/viewport_mouse_event.h" 00032 #include "rviz/visualization_manager.h" 00033 00034 #include <OGRE/OgreCamera.h> 00035 #include <OGRE/OgreSceneManager.h> 00036 #include <OGRE/OgreSceneNode.h> 00037 #include <OGRE/OgreVector3.h> 00038 #include <OGRE/OgreQuaternion.h> 00039 #include <OGRE/OgreViewport.h> 00040 00041 #include <ogre_tools/shape.h> 00042 00043 #include <stdint.h> 00044 #include <sstream> 00045 00046 namespace rviz 00047 { 00048 00049 static const float PITCH_LIMIT_LOW = -Ogre::Math::HALF_PI + 0.001; 00050 static const float PITCH_LIMIT_HIGH = Ogre::Math::HALF_PI - 0.001; 00051 00052 FPSViewController::FPSViewController(VisualizationManager* manager, const std::string& name, Ogre::SceneNode* target_scene_node) 00053 : ViewController(manager, name, target_scene_node) 00054 , yaw_(0.0f) 00055 , pitch_(0.0f) 00056 { 00057 } 00058 00059 FPSViewController::~FPSViewController() 00060 { 00061 } 00062 00063 void FPSViewController::handleMouseEvent(ViewportMouseEvent& event) 00064 { 00065 bool moved = false; 00066 if ( event.event.Dragging() ) 00067 { 00068 int32_t diff_x = event.event.GetX() - event.last_x; 00069 int32_t diff_y = event.event.GetY() - event.last_y; 00070 00071 if ( event.event.LeftIsDown() && !event.event.ShiftDown() ) 00072 { 00073 yaw( -diff_x*0.005 ); 00074 pitch( -diff_y*0.005 ); 00075 } 00076 else if ( event.event.MiddleIsDown() || 00077 ( event.event.ShiftDown() && event.event.LeftIsDown() )) 00078 { 00079 move( diff_x*0.01, -diff_y*0.01, 0.0f ); 00080 } 00081 else if ( event.event.RightIsDown() ) 00082 { 00083 move( 0.0f, 0.0f, diff_y*0.1 ); 00084 } 00085 00086 moved = true; 00087 } 00088 00089 if ( event.event.GetWheelRotation() != 0 ) 00090 { 00091 int diff = event.event.GetWheelRotation(); 00092 move( 0.0f, 0.0f, -diff * 0.01 ); 00093 00094 moved = true; 00095 } 00096 00097 if (moved) 00098 { 00099 manager_->queueRender(); 00100 } 00101 } 00102 00103 void FPSViewController::setOrientation(const Ogre::Quaternion& orientation) 00104 { 00105 Ogre::Quaternion quat = orientation; 00106 yaw_ = quat.getYaw( false ).valueRadians(); 00107 pitch_ = quat.getPitch( false ).valueRadians(); 00108 00109 Ogre::Vector3 direction = quat * Ogre::Vector3::NEGATIVE_UNIT_Z; 00110 if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 ) 00111 { 00112 if ( pitch_ > Ogre::Math::HALF_PI ) 00113 { 00114 pitch_ = -Ogre::Math::HALF_PI + (pitch_ - Ogre::Math::HALF_PI); 00115 } 00116 else if ( pitch_ < -Ogre::Math::HALF_PI ) 00117 { 00118 pitch_ = Ogre::Math::HALF_PI - (-pitch_ - Ogre::Math::HALF_PI); 00119 } 00120 00121 yaw_ = -yaw_; 00122 00123 if ( direction.dotProduct( Ogre::Vector3::UNIT_X ) < 0 ) 00124 { 00125 yaw_ -= Ogre::Math::PI; 00126 } 00127 else 00128 { 00129 yaw_ += Ogre::Math::PI; 00130 } 00131 } 00132 00133 normalizePitch(); 00134 normalizeYaw(); 00135 } 00136 00137 void FPSViewController::onActivate() 00138 { 00139 if (camera_->getProjectionType() == Ogre::PT_ORTHOGRAPHIC) 00140 { 00141 camera_->setProjectionType(Ogre::PT_PERSPECTIVE); 00142 } 00143 else 00144 { 00145 setOrientation(camera_->getOrientation()); 00146 } 00147 } 00148 00149 void FPSViewController::onDeactivate() 00150 { 00151 } 00152 00153 void FPSViewController::onUpdate(float dt, float ros_dt) 00154 { 00155 updateCamera(); 00156 } 00157 00158 void FPSViewController::lookAt( const Ogre::Vector3& point ) 00159 { 00160 camera_->lookAt( point ); 00161 setOrientation( camera_->getOrientation() ); 00162 } 00163 00164 void FPSViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation) 00165 { 00166 lookAt(target_scene_node_->getPosition()); 00167 } 00168 00169 void FPSViewController::normalizePitch() 00170 { 00171 if ( pitch_ < PITCH_LIMIT_LOW ) 00172 { 00173 pitch_ = PITCH_LIMIT_LOW; 00174 } 00175 else if ( pitch_ > PITCH_LIMIT_HIGH ) 00176 { 00177 pitch_ = PITCH_LIMIT_HIGH; 00178 } 00179 } 00180 00181 void FPSViewController::normalizeYaw() 00182 { 00183 yaw_ = fmod( yaw_, Ogre::Math::TWO_PI ); 00184 00185 if ( yaw_ < 0.0f ) 00186 { 00187 yaw_ = Ogre::Math::TWO_PI + yaw_; 00188 } 00189 } 00190 00191 void FPSViewController::updateCamera() 00192 { 00193 Ogre::Matrix3 pitch, yaw; 00194 00195 yaw.FromAxisAngle( Ogre::Vector3::UNIT_Y, Ogre::Radian( yaw_ ) ); 00196 pitch.FromAxisAngle( Ogre::Vector3::UNIT_X, Ogre::Radian( pitch_ ) ); 00197 00198 camera_->setOrientation( yaw * pitch ); 00199 } 00200 00201 void FPSViewController::yaw( float angle ) 00202 { 00203 yaw_ += angle; 00204 00205 normalizeYaw(); 00206 } 00207 00208 void FPSViewController::pitch( float angle ) 00209 { 00210 pitch_ += angle; 00211 00212 normalizePitch(); 00213 } 00214 00215 void FPSViewController::move( float x, float y, float z ) 00216 { 00217 Ogre::Vector3 translate( x, y, z ); 00218 camera_->setPosition( camera_->getPosition() + camera_->getOrientation() * translate ); 00219 } 00220 00221 void FPSViewController::fromString(const std::string& str) 00222 { 00223 std::istringstream iss(str); 00224 00225 iss >> pitch_; 00226 iss.ignore(); 00227 iss >> yaw_; 00228 iss.ignore(); 00229 00230 Ogre::Vector3 vec; 00231 iss >> vec.x; 00232 iss.ignore(); 00233 iss >> vec.y; 00234 iss.ignore(); 00235 iss >> vec.z; 00236 iss.ignore(); 00237 camera_->setPosition(vec); 00238 00239 resetTargetSceneNodePosition(); 00240 } 00241 00242 std::string FPSViewController::toString() 00243 { 00244 std::ostringstream oss; 00245 oss << pitch_ << " " << yaw_ << " " << camera_->getPosition().x << " " << camera_->getPosition().y << " " << camera_->getPosition().z; 00246 00247 return oss.str(); 00248 } 00249 00250 00251 }