$search
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 <OGRE/OgreCamera.h> 00034 #include <OGRE/OgreSceneManager.h> 00035 #include <OGRE/OgreSceneNode.h> 00036 #include <OGRE/OgreVector3.h> 00037 #include <OGRE/OgreQuaternion.h> 00038 #include <OGRE/OgreViewport.h> 00039 00040 #include <stdint.h> 00041 #include <sstream> 00042 00043 #define MIN_DISTANCE 0.01 00044 00045 namespace ogre_tools 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 ogre_tools