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 "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;
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 }