orbit_camera.cpp
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 #include "orbit_camera.h"
00031 #include "shape.h"
00032 
00033 #include <OgreCamera.h>
00034 #include <OgreSceneManager.h>
00035 #include <OgreSceneNode.h>
00036 #include <OgreVector3.h>
00037 #include <OgreQuaternion.h>
00038 #include <OgreViewport.h>
00039 
00040 #include <stdint.h>
00041 #include <sstream>
00042 
00043 #define MIN_DISTANCE 0.01
00044 
00045 namespace rviz
00046 {
00047 
00048 static const float PITCH_LIMIT_LOW = 0.001;
00049 static const float PITCH_LIMIT_HIGH = Ogre::Math::PI - 0.001;
00050 static const float YAW_START = Ogre::Math::PI;// - 0.001;
00051 static const float PITCH_START = Ogre::Math::HALF_PI;
00052 
00053 OrbitCamera::OrbitCamera( Ogre::SceneManager* scene_manager )
00054 : CameraBase( scene_manager )
00055 , focal_point_( Ogre::Vector3::ZERO )
00056 , yaw_( YAW_START )
00057 , pitch_( PITCH_START )
00058 , distance_( 10.0f )
00059 {
00060   focal_point_object_ = new Shape( Shape::Sphere, scene_manager );
00061   focal_point_object_->setScale( Ogre::Vector3( 0.05f, 0.01f, 0.05f ) );
00062   focal_point_object_->setColor( 1.0f, 1.0f, 0.0f, 0.5f );
00063   focal_point_object_->getRootNode()->setVisible( false );
00064 
00065   update();
00066 }
00067 
00068 OrbitCamera::~OrbitCamera()
00069 {
00070   delete focal_point_object_;
00071 }
00072 
00073 void OrbitCamera::normalizePitch()
00074 {
00075   if ( pitch_ < PITCH_LIMIT_LOW )
00076   {
00077     pitch_ = PITCH_LIMIT_LOW;
00078   }
00079   else if ( pitch_ > PITCH_LIMIT_HIGH )
00080   {
00081     pitch_ = PITCH_LIMIT_HIGH;
00082   }
00083 }
00084 
00085 void OrbitCamera::normalizeYaw()
00086 {
00087   yaw_ = fmod( yaw_, Ogre::Math::TWO_PI );
00088 
00089   if ( yaw_ < 0.0f )
00090   {
00091     yaw_ = Ogre::Math::TWO_PI + yaw_;
00092   }
00093 }
00094 
00095 Ogre::Vector3 OrbitCamera::getGlobalFocalPoint()
00096 {
00097   Ogre::Vector3 global_focal_point = focal_point_;
00098 
00099   if ( relative_node_ )
00100   {
00101     global_focal_point = relative_node_->getOrientation() * focal_point_ + relative_node_->getPosition();
00102   }
00103 
00104   return global_focal_point;
00105 }
00106 
00107 void OrbitCamera::update()
00108 {
00109   Ogre::Vector3 global_focal_point = getGlobalFocalPoint();
00110 
00111   float x = distance_ * cos( yaw_ ) * sin( pitch_ ) + global_focal_point.x;
00112   float y = distance_ * cos( pitch_ ) + global_focal_point.y;
00113   float z = distance_ * sin( yaw_ ) * sin( pitch_ ) + global_focal_point.z;
00114 
00115   Ogre::Vector3 pos( x, y, z );
00116 
00117   if ( relative_node_ )
00118   {
00119     Ogre::Vector3 vec = pos - global_focal_point;
00120     pos = relative_node_->getOrientation() * vec + global_focal_point;
00121 
00122     camera_->setFixedYawAxis(true, relative_node_->getOrientation() * Ogre::Vector3::UNIT_Y);
00123   }
00124 
00125   camera_->setPosition( pos );
00126   camera_->lookAt( global_focal_point );
00127 
00128   focal_point_object_->setPosition( global_focal_point );
00129 }
00130 
00131 void OrbitCamera::yaw( float angle )
00132 {
00133   yaw_ += angle;
00134 
00135   normalizeYaw();
00136 
00137   update();
00138 }
00139 
00140 void OrbitCamera::pitch( float angle )
00141 {
00142   pitch_ += angle;
00143 
00144   normalizePitch();
00145 
00146   update();
00147 }
00148 
00149 void OrbitCamera::roll( float angle )
00150 {
00151 }
00152 
00153 Ogre::Vector3 OrbitCamera::getPosition()
00154 {
00155   return camera_->getPosition();
00156 }
00157 
00158 Ogre::Quaternion OrbitCamera::getOrientation()
00159 {
00160   return camera_->getOrientation();
00161 }
00162 
00163 void OrbitCamera::calculatePitchYawFromPosition( const Ogre::Vector3& position )
00164 {
00165   float x = position.x - focal_point_.x;
00166   float y = position.y - focal_point_.y;
00167   pitch_ = acos( y / distance_ );
00168 
00169   normalizePitch();
00170 
00171   float val = x / ( distance_ * sin( pitch_ ) );
00172 
00173   yaw_ = acos( val );
00174 
00175   Ogre::Vector3 direction = focal_point_ - position;
00176 
00177   if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 )
00178   {
00179     yaw_ = Ogre::Math::TWO_PI - yaw_;
00180   }
00181 }
00182 
00183 void OrbitCamera::setFrom( CameraBase* camera )
00184 {
00185   Ogre::Vector3 position = camera->getPosition();
00186   Ogre::Quaternion orientation = camera->getOrientation();
00187 
00188   Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_);
00189   focal_point_ = position + direction;
00190 
00191   calculatePitchYawFromPosition( position );
00192 
00193   update();
00194 }
00195 
00196 void OrbitCamera::setOrientation( float x, float y, float z, float w )
00197 {
00198   Ogre::Vector3 position = camera_->getPosition();
00199   Ogre::Quaternion orientation( w, x, y, z );
00200 
00201   Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_);
00202   focal_point_ = position + direction;
00203 
00204   calculatePitchYawFromPosition( position );
00205 
00206   update();
00207 }
00208 
00209 void OrbitCamera::zoom( float amount )
00210 {
00211   distance_ -= amount;
00212 
00213   if ( distance_ <= MIN_DISTANCE )
00214   {
00215     distance_ = MIN_DISTANCE;
00216   }
00217 
00218   update();
00219 }
00220 
00221 void OrbitCamera::setFocalPoint( const Ogre::Vector3& focal_point )
00222 {
00223   focal_point_ = focal_point;
00224 
00225   update();
00226 }
00227 
00228 void OrbitCamera::move( float x, float y, float z )
00229 {
00230   Ogre::Quaternion orientation = camera_->getOrientation();
00231 
00232   if ( relative_node_ )
00233   {
00234     orientation = relative_node_->getOrientation().Inverse() * orientation;
00235   }
00236 
00237   focal_point_ += orientation * Ogre::Vector3( x, y, z );
00238 
00239   update();
00240 }
00241 
00242 void OrbitCamera::setPosition( float x, float y, float z )
00243 {
00244   Ogre::Vector3 pos( x, y, z );
00245   distance_ = (pos - getGlobalFocalPoint()).length();
00246 
00247   calculatePitchYawFromPosition( Ogre::Vector3( x, y, z ) );
00248 
00249   update();
00250 }
00251 
00252 void OrbitCamera::lookAt( const Ogre::Vector3& point )
00253 {
00254   Ogre::Vector3 focal_point = point;
00255   Ogre::Vector3 camera_position = camera_->getPosition();
00256 
00257   if ( relative_node_ )
00258   {
00259     Ogre::Vector3 rel_pos = relative_node_->getPosition();
00260     Ogre::Quaternion rel_orient = relative_node_->getOrientation();
00261 
00262     focal_point = rel_orient.Inverse() * ( focal_point - rel_pos );
00263     camera_position = rel_orient.Inverse() * ( camera_position - rel_pos );
00264   }
00265 
00266   distance_ = focal_point.distance( camera_position );
00267   focal_point_ = focal_point;
00268 
00269   update();
00270 }
00271 
00272 void OrbitCamera::mouseLeftDrag( int diff_x, int diff_y, bool ctrl, bool alt, bool shift )
00273 {
00274   yaw( diff_x*0.005 );
00275   pitch( -diff_y*0.005 );
00276 }
00277 
00278 void OrbitCamera::mouseMiddleDrag( int diff_x, int diff_y, bool ctrl, bool alt, bool shift )
00279 {
00280   float fovY = camera_->getFOVy().valueRadians();
00281   float fovX = 2.0f * atan( tan( fovY / 2.0f ) * camera_->getAspectRatio() );
00282 
00283   int width = camera_->getViewport()->getActualWidth();
00284   int height = camera_->getViewport()->getActualHeight();
00285 
00286   move( -((float)diff_x / (float)width) * distance_ * tan( fovX / 2.0f ) * 2.0f, ((float)diff_y / (float)height) * distance_ * tan( fovY / 2.0f ) * 2.0f, 0.0f );
00287 
00288 }
00289 
00290 void OrbitCamera::mouseRightDrag( int diff_x, int diff_y, bool ctrl, bool alt, bool shift )
00291 {
00292   if (shift)
00293   {
00294     move(0.0f, 0.0f, diff_y * 0.1 * (distance_ / 10.0f));
00295   }
00296   else
00297   {
00298     zoom( -diff_y * 0.1 * (distance_ / 10.0f) );
00299   }
00300 }
00301 
00302 void OrbitCamera::scrollWheel( int diff, bool ctrl, bool alt, bool shift )
00303 {
00304   if (shift)
00305   {
00306     move(0.0f, 0.0f, -diff * 0.01 * (distance_ / 10.0f));
00307   }
00308   else
00309   {
00310     zoom( diff * 0.01 * (distance_ / 10.0f) );
00311   }
00312 }
00313 
00314 void OrbitCamera::mouseLeftDown( int x, int y )
00315 {
00316   focal_point_object_->getRootNode()->setVisible( true );
00317 }
00318 
00319 void OrbitCamera::mouseMiddleDown( int x, int y )
00320 {
00321   focal_point_object_->getRootNode()->setVisible( true );
00322 }
00323 
00324 void OrbitCamera::mouseRightDown( int x, int y )
00325 {
00326   focal_point_object_->getRootNode()->setVisible( true );
00327 }
00328 
00329 void OrbitCamera::mouseLeftUp( int x, int y )
00330 {
00331   focal_point_object_->getRootNode()->setVisible( false );
00332 }
00333 
00334 void OrbitCamera::mouseMiddleUp( int x, int y )
00335 {
00336   focal_point_object_->getRootNode()->setVisible( false );
00337 }
00338 
00339 void OrbitCamera::mouseRightUp( int x, int y )
00340 {
00341   focal_point_object_->getRootNode()->setVisible( false );
00342 }
00343 
00344 void OrbitCamera::fromString(const std::string& str)
00345 {
00346   std::istringstream iss(str);
00347 
00348   iss >> pitch_;
00349   iss.ignore();
00350   iss >> yaw_;
00351   iss.ignore();
00352   iss >> distance_;
00353   iss.ignore();
00354   iss >> focal_point_.x;
00355   iss.ignore();
00356   iss >> focal_point_.y;
00357   iss.ignore();
00358   iss >> focal_point_.z;
00359 
00360   update();
00361 }
00362 
00363 std::string OrbitCamera::toString()
00364 {
00365   std::ostringstream oss;
00366   oss << pitch_ << " " << yaw_ << " " << distance_ << " " << focal_point_.x << " " << focal_point_.y << " " << focal_point_.z;
00367 
00368   return oss.str();
00369 }
00370 
00371 } // namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:31