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 static const float PITCH_LIMIT_LOW = 0.001;
48 static const float PITCH_LIMIT_HIGH = Ogre::Math::PI - 0.001;
49 static const float YAW_START = Ogre::Math::PI; // - 0.001;
50 static const float PITCH_START = Ogre::Math::HALF_PI;
51 
52 OrbitCamera::OrbitCamera(Ogre::SceneManager* scene_manager)
53  : CameraBase(scene_manager)
54  , focal_point_(Ogre::Vector3::ZERO)
55  , yaw_(YAW_START)
56  , pitch_(PITCH_START)
57  , distance_(10.0f)
58 {
59  focal_point_object_ = new Shape(Shape::Sphere, scene_manager);
60  focal_point_object_->setScale(Ogre::Vector3(0.05f, 0.01f, 0.05f));
61  focal_point_object_->setColor(1.0f, 1.0f, 0.0f, 0.5f);
62  focal_point_object_->getRootNode()->setVisible(false);
63 
64  update();
65 }
66 
68 {
69  delete focal_point_object_;
70 }
71 
73 {
74  if (pitch_ < PITCH_LIMIT_LOW)
75  {
77  }
78  else if (pitch_ > PITCH_LIMIT_HIGH)
79  {
81  }
82 }
83 
85 {
86  yaw_ = fmod(yaw_, Ogre::Math::TWO_PI);
87 
88  if (yaw_ < 0.0f)
89  {
90  yaw_ = Ogre::Math::TWO_PI + yaw_;
91  }
92 }
93 
95 {
96  Ogre::Vector3 global_focal_point = focal_point_;
97 
98  if (relative_node_)
99  {
100  global_focal_point = relative_node_->getOrientation() * focal_point_ + relative_node_->getPosition();
101  }
102 
103  return global_focal_point;
104 }
105 
107 {
108  Ogre::Vector3 global_focal_point = getGlobalFocalPoint();
109 
110  float x = distance_ * cos(yaw_) * sin(pitch_) + global_focal_point.x;
111  float y = distance_ * cos(pitch_) + global_focal_point.y;
112  float z = distance_ * sin(yaw_) * sin(pitch_) + global_focal_point.z;
113 
114  Ogre::Vector3 pos(x, y, z);
115 
116  if (relative_node_)
117  {
118  Ogre::Vector3 vec = pos - global_focal_point;
119  pos = relative_node_->getOrientation() * vec + global_focal_point;
120 
121  camera_->setFixedYawAxis(true, relative_node_->getOrientation() * Ogre::Vector3::UNIT_Y);
122  }
123 
124  camera_->setPosition(pos);
125  camera_->lookAt(global_focal_point);
126 
127  focal_point_object_->setPosition(global_focal_point);
128 }
129 
130 void OrbitCamera::yaw(float angle)
131 {
132  yaw_ += angle;
133 
134  normalizeYaw();
135 
136  update();
137 }
138 
139 void OrbitCamera::pitch(float angle)
140 {
141  pitch_ += angle;
142 
143  normalizePitch();
144 
145  update();
146 }
147 
148 void OrbitCamera::roll(float /*angle*/)
149 {
150 }
151 
153 {
154  return camera_->getPosition();
155 }
156 
157 Ogre::Quaternion OrbitCamera::getOrientation()
158 {
159  return camera_->getOrientation();
160 }
161 
162 void OrbitCamera::calculatePitchYawFromPosition(const Ogre::Vector3& position)
163 {
164  float x = position.x - focal_point_.x;
165  float y = position.y - focal_point_.y;
166  pitch_ = acos(y / distance_);
167 
168  normalizePitch();
169 
170  float val = x / (distance_ * sin(pitch_));
171 
172  yaw_ = acos(val);
173 
174  Ogre::Vector3 direction = focal_point_ - position;
175 
176  if (direction.dotProduct(Ogre::Vector3::NEGATIVE_UNIT_Z) < 0)
177  {
178  yaw_ = Ogre::Math::TWO_PI - yaw_;
179  }
180 }
181 
183 {
184  Ogre::Vector3 position = camera->getPosition();
185  Ogre::Quaternion orientation = camera->getOrientation();
186 
187  Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_);
188  focal_point_ = position + direction;
189 
191 
192  update();
193 }
194 
195 void OrbitCamera::setOrientation(float x, float y, float z, float w)
196 {
197  Ogre::Vector3 position = camera_->getPosition();
198  Ogre::Quaternion orientation(w, x, y, z);
199 
200  Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_);
201  focal_point_ = position + direction;
202 
204 
205  update();
206 }
207 
208 void OrbitCamera::zoom(float amount)
209 {
210  distance_ -= amount;
211 
212  if (distance_ <= MIN_DISTANCE)
213  {
215  }
216 
217  update();
218 }
219 
220 void OrbitCamera::setFocalPoint(const Ogre::Vector3& focal_point)
221 {
222  focal_point_ = focal_point;
223 
224  update();
225 }
226 
227 void OrbitCamera::move(float x, float y, float z)
228 {
229  Ogre::Quaternion orientation = camera_->getOrientation();
230 
231  if (relative_node_)
232  {
233  orientation = relative_node_->getOrientation().Inverse() * orientation;
234  }
235 
236  focal_point_ += orientation * Ogre::Vector3(x, y, z);
237 
238  update();
239 }
240 
241 void OrbitCamera::setPosition(float x, float y, float z)
242 {
243  Ogre::Vector3 pos(x, y, z);
244  distance_ = (pos - getGlobalFocalPoint()).length();
245 
246  calculatePitchYawFromPosition(Ogre::Vector3(x, y, z));
247 
248  update();
249 }
250 
251 void OrbitCamera::lookAt(const Ogre::Vector3& point)
252 {
253  Ogre::Vector3 focal_point = point;
254  Ogre::Vector3 camera_position = camera_->getPosition();
255 
256  if (relative_node_)
257  {
258  Ogre::Vector3 rel_pos = relative_node_->getPosition();
259  Ogre::Quaternion rel_orient = relative_node_->getOrientation();
260 
261  focal_point = rel_orient.Inverse() * (focal_point - rel_pos);
262  camera_position = rel_orient.Inverse() * (camera_position - rel_pos);
263  }
264 
265  distance_ = focal_point.distance(camera_position);
266  focal_point_ = focal_point;
267 
268  update();
269 }
270 
271 void OrbitCamera::mouseLeftDrag(int diff_x, int diff_y, bool /*ctrl*/, bool /*alt*/, bool /*shift*/)
272 {
273  yaw(diff_x * 0.005);
274  pitch(-diff_y * 0.005);
275 }
276 
277 void OrbitCamera::mouseMiddleDrag(int diff_x, int diff_y, bool /*ctrl*/, bool /*alt*/, bool /*shift*/)
278 {
279  float fovY = camera_->getFOVy().valueRadians();
280  float fovX = 2.0f * atan(tan(fovY / 2.0f) * camera_->getAspectRatio());
281 
282  int width = camera_->getViewport()->getActualWidth();
283  int height = camera_->getViewport()->getActualHeight();
284 
285  move(-((float)diff_x / (float)width) * distance_ * tan(fovX / 2.0f) * 2.0f,
286  ((float)diff_y / (float)height) * distance_ * tan(fovY / 2.0f) * 2.0f, 0.0f);
287 }
288 
289 void OrbitCamera::mouseRightDrag(int /*diff_x*/, int diff_y, bool /*ctrl*/, bool /*alt*/, bool shift)
290 {
291  if (shift)
292  {
293  move(0.0f, 0.0f, diff_y * 0.1 * (distance_ / 10.0f));
294  }
295  else
296  {
297  zoom(-diff_y * 0.1 * (distance_ / 10.0f));
298  }
299 }
300 
301 void OrbitCamera::scrollWheel(int diff, bool /*ctrl*/, bool /*alt*/, bool shift)
302 {
303  if (shift)
304  {
305  move(0.0f, 0.0f, -diff * 0.01 * (distance_ / 10.0f));
306  }
307  else
308  {
309  zoom(diff * 0.01 * (distance_ / 10.0f));
310  }
311 }
312 
313 void OrbitCamera::mouseLeftDown(int /*x*/, int /*y*/)
314 {
315  focal_point_object_->getRootNode()->setVisible(true);
316 }
317 
318 void OrbitCamera::mouseMiddleDown(int /*x*/, int /*y*/)
319 {
320  focal_point_object_->getRootNode()->setVisible(true);
321 }
322 
323 void OrbitCamera::mouseRightDown(int /*x*/, int /*y*/)
324 {
325  focal_point_object_->getRootNode()->setVisible(true);
326 }
327 
328 void OrbitCamera::mouseLeftUp(int /*x*/, int /*y*/)
329 {
330  focal_point_object_->getRootNode()->setVisible(false);
331 }
332 
333 void OrbitCamera::mouseMiddleUp(int /*x*/, int /*y*/)
334 {
335  focal_point_object_->getRootNode()->setVisible(false);
336 }
337 
338 void OrbitCamera::mouseRightUp(int /*x*/, int /*y*/)
339 {
340  focal_point_object_->getRootNode()->setVisible(false);
341 }
342 
343 void OrbitCamera::fromString(const std::string& str)
344 {
345  std::istringstream iss(str);
346 
347  iss >> pitch_;
348  iss.ignore();
349  iss >> yaw_;
350  iss.ignore();
351  iss >> distance_;
352  iss.ignore();
353  iss >> focal_point_.x;
354  iss.ignore();
355  iss >> focal_point_.y;
356  iss.ignore();
357  iss >> focal_point_.z;
358 
359  update();
360 }
361 
363 {
364  std::ostringstream oss;
365  oss << pitch_ << " " << yaw_ << " " << distance_ << " " << focal_point_.x << " " << focal_point_.y
366  << " " << focal_point_.z;
367 
368  return oss.str();
369 }
370 
371 } // namespace rviz
void mouseRightUp(int x, int y) override
void mouseLeftDrag(int diff_x, int diff_y, bool ctrl, bool alt, bool shift) override
Handle a left mouse button drag.
void setPosition(float x, float y, float z) override
Set the position of the camera.
std::string toString() override
Returns a string representation of the camera&#39;s configuration.
virtual Ogre::Vector3 getPosition()=0
Get the position of this camera.
void mouseLeftDown(int x, int y) override
void setFocalPoint(const Ogre::Vector3 &focal_point)
Set the focal point of the camera. Keeps the pitch/yaw/distance the same.
void yaw(float angle) override
Yaw the camera.
void mouseRightDown(int x, int y) override
float distance_
The camera&#39;s distance from the focal point.
Definition: orbit_camera.h:150
Ogre::Vector3 focal_point_
The camera&#39;s focal point.
Definition: orbit_camera.h:147
f
static const float YAW_START
void pitch(float angle) override
Pitch the camera.
Generic interface for a camera.
Definition: camera_base.h:53
void setScale(const Ogre::Vector3 &scale) override
Set the scale of the object. Always relative to the identity orientation of the object.
Definition: shape.cpp:160
void move(float x, float y, float z) override
Move the camera relative to its orientation.
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)
void fromString(const std::string &str) override
Loads the camera&#39;s configure from the supplied string (generated through toString()) ...
void mouseRightDrag(int diff_x, int diff_y, bool ctrl, bool alt, bool shift) override
Handle a right mouse button drag.
void scrollWheel(int diff, bool ctrl, bool alt, bool shift) override
Handle a scrollwheel change.
OrbitCamera(Ogre::SceneManager *scene_manager)
void setColor(float r, float g, float b, float a) override
Set the color of the object. Values are in the range [0, 1].
Definition: shape.cpp:140
void setOrientation(float x, float y, float z, float w) override
Set the orientation of the camera from a quaternion.
void mouseMiddleUp(int x, int y) override
void mouseMiddleDrag(int diff_x, int diff_y, bool ctrl, bool alt, bool shift) override
Handle a middle mouse button drag.
Ogre::Camera * camera_
Ogre camera associated with this camera object.
Definition: camera_base.h:236
Ogre::SceneNode * relative_node_
Definition: camera_base.h:239
void zoom(float amount)
Move in/out from the focal point, ie. adjust distance_ by amount.
void mouseMiddleDown(int x, int y) override
Ogre::Vector3 getPosition() override
Get the position of this camera.
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
void setFrom(CameraBase *camera) override
Set the position/orientation of this camera from another camera.
void lookAt(const Ogre::Vector3 &point) override
Point the camera at the specified point.
Shape * focal_point_object_
Definition: orbit_camera.h:152
float pitch_
The camera&#39;s pitch (rotation around the x-axis), in radians.
Definition: orbit_camera.h:149
void normalizePitch()
Normalizes the camera&#39;s pitch, preventing it from reaching vertical (or turning upside down) ...
void update() override
Calculates the camera&#39;s position and orientation from the yaw, pitch, distance and focal point...
void setPosition(const Ogre::Vector3 &position) override
Set the position of this object.
Definition: shape.cpp:150
void roll(float angle) override
Roll the camera.
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
virtual Ogre::Quaternion getOrientation()=0
Get the orientation of this camera.
~OrbitCamera() override
static const float PITCH_LIMIT_HIGH
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
Ogre::SceneNode * getRootNode()
Get the root scene node (pivot node) for this object.
Definition: shape.h:100
static const float PITCH_LIMIT_LOW
void mouseLeftUp(int x, int y) override
float yaw_
The camera&#39;s yaw (rotation around the y-axis), in radians.
Definition: orbit_camera.h:148
Ogre::Quaternion getOrientation() override
Get the orientation of this camera.
Ogre::Vector3 getGlobalFocalPoint()
#define MIN_DISTANCE
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
static const float PITCH_START


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Aug 15 2022 02:05:17