orbit_camera.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include "orbit_camera.h"
31 #include "shape.h"
32 
33 #include <OgreCamera.h>
34 #include <OgreSceneManager.h>
35 #include <OgreSceneNode.h>
36 #include <OgreVector3.h>
37 #include <OgreQuaternion.h>
38 #include <OgreViewport.h>
39 
40 #include <stdint.h>
41 #include <sstream>
42 
43 #define MIN_DISTANCE 0.01
44 
45 namespace rviz
46 {
47 
48 static const float PITCH_LIMIT_LOW = 0.001;
49 static const float PITCH_LIMIT_HIGH = Ogre::Math::PI - 0.001;
50 static const float YAW_START = Ogre::Math::PI;// - 0.001;
51 static const float PITCH_START = Ogre::Math::HALF_PI;
52 
53 OrbitCamera::OrbitCamera( Ogre::SceneManager* scene_manager )
54 : CameraBase( scene_manager )
55 , focal_point_( Ogre::Vector3::ZERO )
56 , yaw_( YAW_START )
57 , pitch_( PITCH_START )
58 , distance_( 10.0f )
59 {
60  focal_point_object_ = new Shape( Shape::Sphere, scene_manager );
61  focal_point_object_->setScale( Ogre::Vector3( 0.05f, 0.01f, 0.05f ) );
62  focal_point_object_->setColor( 1.0f, 1.0f, 0.0f, 0.5f );
63  focal_point_object_->getRootNode()->setVisible( false );
64 
65  update();
66 }
67 
69 {
70  delete focal_point_object_;
71 }
72 
74 {
75  if ( pitch_ < PITCH_LIMIT_LOW )
76  {
78  }
79  else if ( pitch_ > PITCH_LIMIT_HIGH )
80  {
82  }
83 }
84 
86 {
87  yaw_ = fmod( yaw_, Ogre::Math::TWO_PI );
88 
89  if ( yaw_ < 0.0f )
90  {
91  yaw_ = Ogre::Math::TWO_PI + yaw_;
92  }
93 }
94 
96 {
97  Ogre::Vector3 global_focal_point = focal_point_;
98 
99  if ( relative_node_ )
100  {
101  global_focal_point = relative_node_->getOrientation() * focal_point_ + relative_node_->getPosition();
102  }
103 
104  return global_focal_point;
105 }
106 
108 {
109  Ogre::Vector3 global_focal_point = getGlobalFocalPoint();
110 
111  float x = distance_ * cos( yaw_ ) * sin( pitch_ ) + global_focal_point.x;
112  float y = distance_ * cos( pitch_ ) + global_focal_point.y;
113  float z = distance_ * sin( yaw_ ) * sin( pitch_ ) + global_focal_point.z;
114 
115  Ogre::Vector3 pos( x, y, z );
116 
117  if ( relative_node_ )
118  {
119  Ogre::Vector3 vec = pos - global_focal_point;
120  pos = relative_node_->getOrientation() * vec + global_focal_point;
121 
122  camera_->setFixedYawAxis(true, relative_node_->getOrientation() * Ogre::Vector3::UNIT_Y);
123  }
124 
125  camera_->setPosition( pos );
126  camera_->lookAt( global_focal_point );
127 
128  focal_point_object_->setPosition( global_focal_point );
129 }
130 
131 void OrbitCamera::yaw( float angle )
132 {
133  yaw_ += angle;
134 
135  normalizeYaw();
136 
137  update();
138 }
139 
140 void OrbitCamera::pitch( float angle )
141 {
142  pitch_ += angle;
143 
144  normalizePitch();
145 
146  update();
147 }
148 
149 void OrbitCamera::roll( float angle )
150 {
151 }
152 
154 {
155  return camera_->getPosition();
156 }
157 
158 Ogre::Quaternion OrbitCamera::getOrientation()
159 {
160  return camera_->getOrientation();
161 }
162 
163 void OrbitCamera::calculatePitchYawFromPosition( const Ogre::Vector3& position )
164 {
165  float x = position.x - focal_point_.x;
166  float y = position.y - focal_point_.y;
167  pitch_ = acos( y / distance_ );
168 
169  normalizePitch();
170 
171  float val = x / ( distance_ * sin( pitch_ ) );
172 
173  yaw_ = acos( val );
174 
175  Ogre::Vector3 direction = focal_point_ - position;
176 
177  if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 )
178  {
179  yaw_ = Ogre::Math::TWO_PI - yaw_;
180  }
181 }
182 
184 {
185  Ogre::Vector3 position = camera->getPosition();
186  Ogre::Quaternion orientation = camera->getOrientation();
187 
188  Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_);
189  focal_point_ = position + direction;
190 
191  calculatePitchYawFromPosition( position );
192 
193  update();
194 }
195 
196 void OrbitCamera::setOrientation( float x, float y, float z, float w )
197 {
198  Ogre::Vector3 position = camera_->getPosition();
199  Ogre::Quaternion orientation( w, x, y, z );
200 
201  Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_);
202  focal_point_ = position + direction;
203 
204  calculatePitchYawFromPosition( position );
205 
206  update();
207 }
208 
209 void OrbitCamera::zoom( float amount )
210 {
211  distance_ -= amount;
212 
213  if ( distance_ <= MIN_DISTANCE )
214  {
216  }
217 
218  update();
219 }
220 
221 void OrbitCamera::setFocalPoint( const Ogre::Vector3& focal_point )
222 {
223  focal_point_ = focal_point;
224 
225  update();
226 }
227 
228 void OrbitCamera::move( float x, float y, float z )
229 {
230  Ogre::Quaternion orientation = camera_->getOrientation();
231 
232  if ( relative_node_ )
233  {
234  orientation = relative_node_->getOrientation().Inverse() * orientation;
235  }
236 
237  focal_point_ += orientation * Ogre::Vector3( x, y, z );
238 
239  update();
240 }
241 
242 void OrbitCamera::setPosition( float x, float y, float z )
243 {
244  Ogre::Vector3 pos( x, y, z );
245  distance_ = (pos - getGlobalFocalPoint()).length();
246 
247  calculatePitchYawFromPosition( Ogre::Vector3( x, y, z ) );
248 
249  update();
250 }
251 
252 void OrbitCamera::lookAt( const Ogre::Vector3& point )
253 {
254  Ogre::Vector3 focal_point = point;
255  Ogre::Vector3 camera_position = camera_->getPosition();
256 
257  if ( relative_node_ )
258  {
259  Ogre::Vector3 rel_pos = relative_node_->getPosition();
260  Ogre::Quaternion rel_orient = relative_node_->getOrientation();
261 
262  focal_point = rel_orient.Inverse() * ( focal_point - rel_pos );
263  camera_position = rel_orient.Inverse() * ( camera_position - rel_pos );
264  }
265 
266  distance_ = focal_point.distance( camera_position );
267  focal_point_ = focal_point;
268 
269  update();
270 }
271 
272 void OrbitCamera::mouseLeftDrag( int diff_x, int diff_y, bool ctrl, bool alt, bool shift )
273 {
274  yaw( diff_x*0.005 );
275  pitch( -diff_y*0.005 );
276 }
277 
278 void OrbitCamera::mouseMiddleDrag( int diff_x, int diff_y, bool ctrl, bool alt, bool shift )
279 {
280  float fovY = camera_->getFOVy().valueRadians();
281  float fovX = 2.0f * atan( tan( fovY / 2.0f ) * camera_->getAspectRatio() );
282 
283  int width = camera_->getViewport()->getActualWidth();
284  int height = camera_->getViewport()->getActualHeight();
285 
286  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 );
287 
288 }
289 
290 void OrbitCamera::mouseRightDrag( int diff_x, int diff_y, bool ctrl, bool alt, bool shift )
291 {
292  if (shift)
293  {
294  move(0.0f, 0.0f, diff_y * 0.1 * (distance_ / 10.0f));
295  }
296  else
297  {
298  zoom( -diff_y * 0.1 * (distance_ / 10.0f) );
299  }
300 }
301 
302 void OrbitCamera::scrollWheel( int diff, bool ctrl, bool alt, bool shift )
303 {
304  if (shift)
305  {
306  move(0.0f, 0.0f, -diff * 0.01 * (distance_ / 10.0f));
307  }
308  else
309  {
310  zoom( diff * 0.01 * (distance_ / 10.0f) );
311  }
312 }
313 
314 void OrbitCamera::mouseLeftDown( int x, int y )
315 {
316  focal_point_object_->getRootNode()->setVisible( true );
317 }
318 
319 void OrbitCamera::mouseMiddleDown( int x, int y )
320 {
321  focal_point_object_->getRootNode()->setVisible( true );
322 }
323 
324 void OrbitCamera::mouseRightDown( int x, int y )
325 {
326  focal_point_object_->getRootNode()->setVisible( true );
327 }
328 
329 void OrbitCamera::mouseLeftUp( int x, int y )
330 {
331  focal_point_object_->getRootNode()->setVisible( false );
332 }
333 
334 void OrbitCamera::mouseMiddleUp( int x, int y )
335 {
336  focal_point_object_->getRootNode()->setVisible( false );
337 }
338 
339 void OrbitCamera::mouseRightUp( int x, int y )
340 {
341  focal_point_object_->getRootNode()->setVisible( false );
342 }
343 
344 void OrbitCamera::fromString(const std::string& str)
345 {
346  std::istringstream iss(str);
347 
348  iss >> pitch_;
349  iss.ignore();
350  iss >> yaw_;
351  iss.ignore();
352  iss >> distance_;
353  iss.ignore();
354  iss >> focal_point_.x;
355  iss.ignore();
356  iss >> focal_point_.y;
357  iss.ignore();
358  iss >> focal_point_.z;
359 
360  update();
361 }
362 
364 {
365  std::ostringstream oss;
366  oss << pitch_ << " " << yaw_ << " " << distance_ << " " << focal_point_.x << " " << focal_point_.y << " " << focal_point_.z;
367 
368  return oss.str();
369 }
370 
371 } // namespace rviz
virtual Ogre::Vector3 getPosition()=0
Get the position of this camera.
virtual void fromString(const std::string &str)
Loads the camera&#39;s configure from the supplied string (generated through toString()) ...
virtual void mouseLeftDown(int x, int y)
void setFocalPoint(const Ogre::Vector3 &focal_point)
Set the focal point of the camera. Keeps the pitch/yaw/distance the same.
float distance_
The camera&#39;s distance from the focal point.
Definition: orbit_camera.h:138
Ogre::Vector3 focal_point_
The camera&#39;s focal point.
Definition: orbit_camera.h:135
f
static const float YAW_START
virtual void yaw(float angle)
Yaw the camera.
Generic interface for a camera.
Definition: camera_base.h:53
virtual void roll(float angle)
Roll the camera.
virtual void pitch(float angle)
Pitch the camera.
void calculatePitchYawFromPosition(const Ogre::Vector3 &position)
Calculates pitch and yaw values given a new position and the current focal point. ...
void normalizeYaw()
Normalizes the camera&#39;s yaw in the range [0, 2*pi)
virtual Ogre::Vector3 getPosition()
Get the position of this camera.
virtual void setPosition(float x, float y, float z)
Set the position of the camera.
TFSIMD_FORCE_INLINE const tfScalar & y() const
OrbitCamera(Ogre::SceneManager *scene_manager)
virtual std::string toString()
Returns a string representation of the camera&#39;s configuration.
virtual void mouseMiddleDrag(int diff_x, int diff_y, bool ctrl, bool alt, bool shift)
Handle a middle mouse button drag.
virtual void setColor(float r, float g, float b, float a)
Set the color of the object. Values are in the range [0, 1].
Definition: shape.cpp:142
Ogre::Camera * camera_
Ogre camera associated with this camera object.
Definition: camera_base.h:219
virtual void mouseRightDrag(int diff_x, int diff_y, bool ctrl, bool alt, bool shift)
Handle a right mouse button drag.
Ogre::SceneNode * relative_node_
Definition: camera_base.h:222
virtual void scrollWheel(int diff, bool ctrl, bool alt, bool shift)
Handle a scrollwheel change.
void zoom(float amount)
Move in/out from the focal point, ie. adjust distance_ by amount.
virtual void update()
Calculates the camera&#39;s position and orientation from the yaw, pitch, distance and focal point...
virtual void lookAt(const Ogre::Vector3 &point)
Point the camera at the specified point.
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
Definition: shape.cpp:152
virtual ~OrbitCamera()
virtual void mouseMiddleDown(int x, int y)
TFSIMD_FORCE_INLINE Vector3()
virtual Ogre::Quaternion getOrientation()
Get the orientation of this camera.
Shape * focal_point_object_
Definition: orbit_camera.h:140
virtual void mouseRightDown(int x, int y)
TFSIMD_FORCE_INLINE const tfScalar & x() const
float pitch_
The camera&#39;s pitch (rotation around the x-axis), in radians.
Definition: orbit_camera.h:137
void normalizePitch()
Normalizes the camera&#39;s pitch, preventing it from reaching vertical (or turning upside down) ...
TFSIMD_FORCE_INLINE const tfScalar & z() const
virtual void mouseRightUp(int x, int y)
virtual void move(float x, float y, float z)
Move the camera relative to its orientation.
virtual Ogre::Quaternion getOrientation()=0
Get the orientation of this camera.
static const float PITCH_LIMIT_HIGH
virtual void setOrientation(float x, float y, float z, float w)
Set the orientation of the camera from a quaternion.
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
virtual void mouseMiddleUp(int x, int y)
Ogre::SceneNode * getRootNode()
Get the root scene node (pivot node) for this object.
Definition: shape.h:97
static const float PITCH_LIMIT_LOW
float yaw_
The camera&#39;s yaw (rotation around the y-axis), in radians.
Definition: orbit_camera.h:136
virtual void mouseLeftDrag(int diff_x, int diff_y, bool ctrl, bool alt, bool shift)
Handle a left mouse button drag.
Ogre::Vector3 getGlobalFocalPoint()
#define MIN_DISTANCE
virtual void mouseLeftUp(int x, int y)
static const float PITCH_START
virtual void setScale(const Ogre::Vector3 &scale)
Set the scale of the object. Always relative to the identity orientation of the object.
Definition: shape.cpp:162
virtual void setFrom(CameraBase *camera)
Set the position/orientation of this camera from another camera.


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51