source: pacpussensors/trunk/Vislab/lib3dv-1.2.0/lib3dv/eigen/demos/opengl/quaternion_demo.cpp

Last change on this file was 136, checked in by ldecherf, 8 years ago

Doc
File size: 18.7 KB
Line 
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
5//
6// This Source Code Form is subject to the terms of the Mozilla
7// Public License v. 2.0. If a copy of the MPL was not distributed
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10#include "quaternion_demo.h"
11#include "icosphere.h"
12
13#include <Eigen/Geometry>
14#include <Eigen/QR>
15#include <Eigen/LU>
16
17#include <iostream>
18#include <QEvent>
19#include <QMouseEvent>
20#include <QInputDialog>
21#include <QGridLayout>
22#include <QButtonGroup>
23#include <QRadioButton>
24#include <QDockWidget>
25#include <QPushButton>
26#include <QGroupBox>
27
28using namespace Eigen;
29
30class FancySpheres
31{
32 public:
33 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34
35 FancySpheres()
36 {
37 const int levels = 4;
38 const float scale = 0.33;
39 float radius = 100;
40 std::vector<int> parents;
41
42 // leval 0
43 mCenters.push_back(Vector3f::Zero());
44 parents.push_back(-1);
45 mRadii.push_back(radius);
46
47 // generate level 1 using icosphere vertices
48 radius *= 0.45;
49 {
50 float dist = mRadii[0]*0.9;
51 for (int i=0; i<12; ++i)
52 {
53 mCenters.push_back(mIcoSphere.vertices()[i] * dist);
54 mRadii.push_back(radius);
55 parents.push_back(0);
56 }
57 }
58
59 static const float angles [10] = {
60 0, 0,
61 M_PI, 0.*M_PI,
62 M_PI, 0.5*M_PI,
63 M_PI, 1.*M_PI,
64 M_PI, 1.5*M_PI
65 };
66
67 // generate other levels
68 int start = 1;
69 for (int l=1; l<levels; l++)
70 {
71 radius *= scale;
72 int end = mCenters.size();
73 for (int i=start; i<end; ++i)
74 {
75 Vector3f c = mCenters[i];
76 Vector3f ax0 = (c - mCenters[parents[i]]).normalized();
77 Vector3f ax1 = ax0.unitOrthogonal();
78 Quaternionf q;
79 q.setFromTwoVectors(Vector3f::UnitZ(), ax0);
80 Affine3f t = Translation3f(c) * q * Scaling(mRadii[i]+radius);
81 for (int j=0; j<5; ++j)
82 {
83 Vector3f newC = c + ( (AngleAxisf(angles[j*2+1], ax0)
84 * AngleAxisf(angles[j*2+0] * (l==1 ? 0.35 : 0.5), ax1)) * ax0)
85 * (mRadii[i] + radius*0.8);
86 mCenters.push_back(newC);
87 mRadii.push_back(radius);
88 parents.push_back(i);
89 }
90 }
91 start = end;
92 }
93 }
94
95 void draw()
96 {
97 int end = mCenters.size();
98 glEnable(GL_NORMALIZE);
99 for (int i=0; i<end; ++i)
100 {
101 Affine3f t = Translation3f(mCenters[i]) * Scaling(mRadii[i]);
102 gpu.pushMatrix(GL_MODELVIEW);
103 gpu.multMatrix(t.matrix(),GL_MODELVIEW);
104 mIcoSphere.draw(2);
105 gpu.popMatrix(GL_MODELVIEW);
106 }
107 glDisable(GL_NORMALIZE);
108 }
109 protected:
110 std::vector<Vector3f> mCenters;
111 std::vector<float> mRadii;
112 IcoSphere mIcoSphere;
113};
114
115
116// generic linear interpolation method
117template<typename T> T lerp(float t, const T& a, const T& b)
118{
119 return a*(1-t) + b*t;
120}
121
122// quaternion slerp
123template<> Quaternionf lerp(float t, const Quaternionf& a, const Quaternionf& b)
124{ return a.slerp(t,b); }
125
126// linear interpolation of a frame using the type OrientationType
127// to perform the interpolation of the orientations
128template<typename OrientationType>
129inline static Frame lerpFrame(float alpha, const Frame& a, const Frame& b)
130{
131 return Frame(lerp(alpha,a.position,b.position),
132 Quaternionf(lerp(alpha,OrientationType(a.orientation),OrientationType(b.orientation))));
133}
134
135template<typename _Scalar> class EulerAngles
136{
137public:
138 enum { Dim = 3 };
139 typedef _Scalar Scalar;
140 typedef Matrix<Scalar,3,3> Matrix3;
141 typedef Matrix<Scalar,3,1> Vector3;
142 typedef Quaternion<Scalar> QuaternionType;
143
144protected:
145
146 Vector3 m_angles;
147
148public:
149
150 EulerAngles() {}
151 inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {}
152 inline EulerAngles(const QuaternionType& q) { *this = q; }
153
154 const Vector3& coeffs() const { return m_angles; }
155 Vector3& coeffs() { return m_angles; }
156
157 EulerAngles& operator=(const QuaternionType& q)
158 {
159 Matrix3 m = q.toRotationMatrix();
160 return *this = m;
161 }
162
163 EulerAngles& operator=(const Matrix3& m)
164 {
165 // mat = cy*cz -cy*sz sy
166 // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
167 // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
168 m_angles.coeffRef(1) = std::asin(m.coeff(0,2));
169 m_angles.coeffRef(0) = std::atan2(-m.coeff(1,2),m.coeff(2,2));
170 m_angles.coeffRef(2) = std::atan2(-m.coeff(0,1),m.coeff(0,0));
171 return *this;
172 }
173
174 Matrix3 toRotationMatrix(void) const
175 {
176 Vector3 c = m_angles.array().cos();
177 Vector3 s = m_angles.array().sin();
178 Matrix3 res;
179 res << c.y()*c.z(), -c.y()*s.z(), s.y(),
180 c.z()*s.x()*s.y()+c.x()*s.z(), c.x()*c.z()-s.x()*s.y()*s.z(), -c.y()*s.x(),
181 -c.x()*c.z()*s.y()+s.x()*s.z(), c.z()*s.x()+c.x()*s.y()*s.z(), c.x()*c.y();
182 return res;
183 }
184
185 operator QuaternionType() { return QuaternionType(toRotationMatrix()); }
186};
187
188// Euler angles slerp
189template<> EulerAngles<float> lerp(float t, const EulerAngles<float>& a, const EulerAngles<float>& b)
190{
191 EulerAngles<float> res;
192 res.coeffs() = lerp(t, a.coeffs(), b.coeffs());
193 return res;
194}
195
196
197RenderingWidget::RenderingWidget()
198{
199 mAnimate = false;
200 mCurrentTrackingMode = TM_NO_TRACK;
201 mNavMode = NavTurnAround;
202 mLerpMode = LerpQuaternion;
203 mRotationMode = RotationStable;
204 mTrackball.setCamera(&mCamera);
205
206 // required to capture key press events
207 setFocusPolicy(Qt::ClickFocus);
208}
209
210void RenderingWidget::grabFrame(void)
211{
212 // ask user for a time
213 bool ok = false;
214 double t = 0;
215 if (!m_timeline.empty())
216 t = (--m_timeline.end())->first + 1.;
217 t = QInputDialog::getDouble(this, "Eigen's RenderingWidget", "time value: ",
218 t, 0, 1e3, 1, &ok);
219 if (ok)
220 {
221 Frame aux;
222 aux.orientation = mCamera.viewMatrix().linear();
223 aux.position = mCamera.viewMatrix().translation();
224 m_timeline[t] = aux;
225 }
226}
227
228void RenderingWidget::drawScene()
229{
230 static FancySpheres sFancySpheres;
231 float length = 50;
232 gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitX(), Color(1,0,0,1));
233 gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitY(), Color(0,1,0,1));
234 gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitZ(), Color(0,0,1,1));
235
236 // draw the fractal object
237 float sqrt3 = internal::sqrt(3.);
238 glLightfv(GL_LIGHT0, GL_AMBIENT, Vector4f(0.5,0.5,0.5,1).data());
239 glLightfv(GL_LIGHT0, GL_DIFFUSE, Vector4f(0.5,1,0.5,1).data());
240 glLightfv(GL_LIGHT0, GL_SPECULAR, Vector4f(1,1,1,1).data());
241 glLightfv(GL_LIGHT0, GL_POSITION, Vector4f(-sqrt3,-sqrt3,sqrt3,0).data());
242
243 glLightfv(GL_LIGHT1, GL_AMBIENT, Vector4f(0,0,0,1).data());
244 glLightfv(GL_LIGHT1, GL_DIFFUSE, Vector4f(1,0.5,0.5,1).data());
245 glLightfv(GL_LIGHT1, GL_SPECULAR, Vector4f(1,1,1,1).data());
246 glLightfv(GL_LIGHT1, GL_POSITION, Vector4f(-sqrt3,sqrt3,-sqrt3,0).data());
247
248 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, Vector4f(0.7, 0.7, 0.7, 1).data());
249 glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, Vector4f(0.8, 0.75, 0.6, 1).data());
250 glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, Vector4f(1, 1, 1, 1).data());
251 glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 64);
252
253 glEnable(GL_LIGHTING);
254 glEnable(GL_LIGHT0);
255 glEnable(GL_LIGHT1);
256
257 sFancySpheres.draw();
258 glVertexPointer(3, GL_FLOAT, 0, mVertices[0].data());
259 glNormalPointer(GL_FLOAT, 0, mNormals[0].data());
260 glEnableClientState(GL_VERTEX_ARRAY);
261 glEnableClientState(GL_NORMAL_ARRAY);
262 glDrawArrays(GL_TRIANGLES, 0, mVertices.size());
263 glDisableClientState(GL_VERTEX_ARRAY);
264 glDisableClientState(GL_NORMAL_ARRAY);
265
266 glDisable(GL_LIGHTING);
267}
268
269void RenderingWidget::animate()
270{
271 m_alpha += double(m_timer.interval()) * 1e-3;
272
273 TimeLine::const_iterator hi = m_timeline.upper_bound(m_alpha);
274 TimeLine::const_iterator lo = hi;
275 --lo;
276
277 Frame currentFrame;
278
279 if(hi==m_timeline.end())
280 {
281 // end
282 currentFrame = lo->second;
283 stopAnimation();
284 }
285 else if(hi==m_timeline.begin())
286 {
287 // start
288 currentFrame = hi->second;
289 }
290 else
291 {
292 float s = (m_alpha - lo->first)/(hi->first - lo->first);
293 if (mLerpMode==LerpEulerAngles)
294 currentFrame = ::lerpFrame<EulerAngles<float> >(s, lo->second, hi->second);
295 else if (mLerpMode==LerpQuaternion)
296 currentFrame = ::lerpFrame<Eigen::Quaternionf>(s, lo->second, hi->second);
297 else
298 {
299 std::cerr << "Invalid rotation interpolation mode (abort)\n";
300 exit(2);
301 }
302 currentFrame.orientation.coeffs().normalize();
303 }
304
305 currentFrame.orientation = currentFrame.orientation.inverse();
306 currentFrame.position = - (currentFrame.orientation * currentFrame.position);
307 mCamera.setFrame(currentFrame);
308
309 updateGL();
310}
311
312void RenderingWidget::keyPressEvent(QKeyEvent * e)
313{
314 switch(e->key())
315 {
316 case Qt::Key_Up:
317 mCamera.zoom(2);
318 break;
319 case Qt::Key_Down:
320 mCamera.zoom(-2);
321 break;
322 // add a frame
323 case Qt::Key_G:
324 grabFrame();
325 break;
326 // clear the time line
327 case Qt::Key_C:
328 m_timeline.clear();
329 break;
330 // move the camera to initial pos
331 case Qt::Key_R:
332 resetCamera();
333 break;
334 // start/stop the animation
335 case Qt::Key_A:
336 if (mAnimate)
337 {
338 stopAnimation();
339 }
340 else
341 {
342 m_alpha = 0;
343 connect(&m_timer, SIGNAL(timeout()), this, SLOT(animate()));
344 m_timer.start(1000/30);
345 mAnimate = true;
346 }
347 break;
348 default:
349 break;
350 }
351
352 updateGL();
353}
354
355void RenderingWidget::stopAnimation()
356{
357 disconnect(&m_timer, SIGNAL(timeout()), this, SLOT(animate()));
358 m_timer.stop();
359 mAnimate = false;
360 m_alpha = 0;
361}
362
363void RenderingWidget::mousePressEvent(QMouseEvent* e)
364{
365 mMouseCoords = Vector2i(e->pos().x(), e->pos().y());
366 bool fly = (mNavMode==NavFly) || (e->modifiers()&Qt::ControlModifier);
367 switch(e->button())
368 {
369 case Qt::LeftButton:
370 if(fly)
371 {
372 mCurrentTrackingMode = TM_LOCAL_ROTATE;
373 mTrackball.start(Trackball::Local);
374 }
375 else
376 {
377 mCurrentTrackingMode = TM_ROTATE_AROUND;
378 mTrackball.start(Trackball::Around);
379 }
380 mTrackball.track(mMouseCoords);
381 break;
382 case Qt::MidButton:
383 if(fly)
384 mCurrentTrackingMode = TM_FLY_Z;
385 else
386 mCurrentTrackingMode = TM_ZOOM;
387 break;
388 case Qt::RightButton:
389 mCurrentTrackingMode = TM_FLY_PAN;
390 break;
391 default:
392 break;
393 }
394}
395void RenderingWidget::mouseReleaseEvent(QMouseEvent*)
396{
397 mCurrentTrackingMode = TM_NO_TRACK;
398 updateGL();
399}
400
401void RenderingWidget::mouseMoveEvent(QMouseEvent* e)
402{
403 // tracking
404 if(mCurrentTrackingMode != TM_NO_TRACK)
405 {
406 float dx = float(e->x() - mMouseCoords.x()) / float(mCamera.vpWidth());
407 float dy = - float(e->y() - mMouseCoords.y()) / float(mCamera.vpHeight());
408
409 // speedup the transformations
410 if(e->modifiers() & Qt::ShiftModifier)
411 {
412 dx *= 10.;
413 dy *= 10.;
414 }
415
416 switch(mCurrentTrackingMode)
417 {
418 case TM_ROTATE_AROUND:
419 case TM_LOCAL_ROTATE:
420 if (mRotationMode==RotationStable)
421 {
422 // use the stable trackball implementation mapping
423 // the 2D coordinates to 3D points on a sphere.
424 mTrackball.track(Vector2i(e->pos().x(), e->pos().y()));
425 }
426 else
427 {
428 // standard approach mapping the x and y displacements as rotations
429 // around the camera's X and Y axes.
430 Quaternionf q = AngleAxisf( dx*M_PI, Vector3f::UnitY())
431 * AngleAxisf(-dy*M_PI, Vector3f::UnitX());
432 if (mCurrentTrackingMode==TM_LOCAL_ROTATE)
433 mCamera.localRotate(q);
434 else
435 mCamera.rotateAroundTarget(q);
436 }
437 break;
438 case TM_ZOOM :
439 mCamera.zoom(dy*100);
440 break;
441 case TM_FLY_Z :
442 mCamera.localTranslate(Vector3f(0, 0, -dy*200));
443 break;
444 case TM_FLY_PAN :
445 mCamera.localTranslate(Vector3f(dx*200, dy*200, 0));
446 break;
447 default:
448 break;
449 }
450
451 updateGL();
452 }
453
454 mMouseCoords = Vector2i(e->pos().x(), e->pos().y());
455}
456
457void RenderingWidget::paintGL()
458{
459 glEnable(GL_DEPTH_TEST);
460 glDisable(GL_CULL_FACE);
461 glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
462 glDisable(GL_COLOR_MATERIAL);
463 glDisable(GL_BLEND);
464 glDisable(GL_ALPHA_TEST);
465 glDisable(GL_TEXTURE_1D);
466 glDisable(GL_TEXTURE_2D);
467 glDisable(GL_TEXTURE_3D);
468
469 // Clear buffers
470 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
471
472 mCamera.activateGL();
473
474 drawScene();
475}
476
477void RenderingWidget::initializeGL()
478{
479 glClearColor(1., 1., 1., 0.);
480 glLightModeli(GL_LIGHT_MODEL_LOCAL_VIEWER, 1);
481 glDepthMask(GL_TRUE);
482 glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
483
484 mCamera.setPosition(Vector3f(-200, -200, -200));
485 mCamera.setTarget(Vector3f(0, 0, 0));
486 mInitFrame.orientation = mCamera.orientation().inverse();
487 mInitFrame.position = mCamera.viewMatrix().translation();
488}
489
490void RenderingWidget::resizeGL(int width, int height)
491{
492 mCamera.setViewport(width,height);
493}
494
495void RenderingWidget::setNavMode(int m)
496{
497 mNavMode = NavMode(m);
498}
499
500void RenderingWidget::setLerpMode(int m)
501{
502 mLerpMode = LerpMode(m);
503}
504
505void RenderingWidget::setRotationMode(int m)
506{
507 mRotationMode = RotationMode(m);
508}
509
510void RenderingWidget::resetCamera()
511{
512 if (mAnimate)
513 stopAnimation();
514 m_timeline.clear();
515 Frame aux0 = mCamera.frame();
516 aux0.orientation = aux0.orientation.inverse();
517 aux0.position = mCamera.viewMatrix().translation();
518 m_timeline[0] = aux0;
519
520 Vector3f currentTarget = mCamera.target();
521 mCamera.setTarget(Vector3f::Zero());
522
523 // compute the rotation duration to move the camera to the target
524 Frame aux1 = mCamera.frame();
525 aux1.orientation = aux1.orientation.inverse();
526 aux1.position = mCamera.viewMatrix().translation();
527 float duration = aux0.orientation.angularDistance(aux1.orientation) * 0.9;
528 if (duration<0.1) duration = 0.1;
529
530 // put the camera at that time step:
531 aux1 = aux0.lerp(duration/2,mInitFrame);
532 // and make it look at the target again
533 aux1.orientation = aux1.orientation.inverse();
534 aux1.position = - (aux1.orientation * aux1.position);
535 mCamera.setFrame(aux1);
536 mCamera.setTarget(Vector3f::Zero());
537
538 // add this camera keyframe
539 aux1.orientation = aux1.orientation.inverse();
540 aux1.position = mCamera.viewMatrix().translation();
541 m_timeline[duration] = aux1;
542
543 m_timeline[2] = mInitFrame;
544 m_alpha = 0;
545 animate();
546 connect(&m_timer, SIGNAL(timeout()), this, SLOT(animate()));
547 m_timer.start(1000/30);
548 mAnimate = true;
549}
550
551QWidget* RenderingWidget::createNavigationControlWidget()
552{
553 QWidget* panel = new QWidget();
554 QVBoxLayout* layout = new QVBoxLayout();
555
556 {
557 QPushButton* but = new QPushButton("reset");
558 but->setToolTip("move the camera to initial position (with animation)");
559 layout->addWidget(but);
560 connect(but, SIGNAL(clicked()), this, SLOT(resetCamera()));
561 }
562 {
563 // navigation mode
564 QGroupBox* box = new QGroupBox("navigation mode");
565 QVBoxLayout* boxLayout = new QVBoxLayout;
566 QButtonGroup* group = new QButtonGroup(panel);
567 QRadioButton* but;
568 but = new QRadioButton("turn around");
569 but->setToolTip("look around an object");
570 group->addButton(but, NavTurnAround);
571 boxLayout->addWidget(but);
572 but = new QRadioButton("fly");
573 but->setToolTip("free navigation like a spaceship\n(this mode can also be enabled pressing the \"shift\" key)");
574 group->addButton(but, NavFly);
575 boxLayout->addWidget(but);
576 group->button(mNavMode)->setChecked(true);
577 connect(group, SIGNAL(buttonClicked(int)), this, SLOT(setNavMode(int)));
578 box->setLayout(boxLayout);
579 layout->addWidget(box);
580 }
581 {
582 // track ball, rotation mode
583 QGroupBox* box = new QGroupBox("rotation mode");
584 QVBoxLayout* boxLayout = new QVBoxLayout;
585 QButtonGroup* group = new QButtonGroup(panel);
586 QRadioButton* but;
587 but = new QRadioButton("stable trackball");
588 group->addButton(but, RotationStable);
589 boxLayout->addWidget(but);
590 but->setToolTip("use the stable trackball implementation mapping\nthe 2D coordinates to 3D points on a sphere");
591 but = new QRadioButton("standard rotation");
592 group->addButton(but, RotationStandard);
593 boxLayout->addWidget(but);
594 but->setToolTip("standard approach mapping the x and y displacements\nas rotations around the camera's X and Y axes");
595 group->button(mRotationMode)->setChecked(true);
596 connect(group, SIGNAL(buttonClicked(int)), this, SLOT(setRotationMode(int)));
597 box->setLayout(boxLayout);
598 layout->addWidget(box);
599 }
600 {
601 // interpolation mode
602 QGroupBox* box = new QGroupBox("spherical interpolation");
603 QVBoxLayout* boxLayout = new QVBoxLayout;
604 QButtonGroup* group = new QButtonGroup(panel);
605 QRadioButton* but;
606 but = new QRadioButton("quaternion slerp");
607 group->addButton(but, LerpQuaternion);
608 boxLayout->addWidget(but);
609 but->setToolTip("use quaternion spherical interpolation\nto interpolate orientations");
610 but = new QRadioButton("euler angles");
611 group->addButton(but, LerpEulerAngles);
612 boxLayout->addWidget(but);
613 but->setToolTip("use Euler angles to interpolate orientations");
614 group->button(mNavMode)->setChecked(true);
615 connect(group, SIGNAL(buttonClicked(int)), this, SLOT(setLerpMode(int)));
616 box->setLayout(boxLayout);
617 layout->addWidget(box);
618 }
619 layout->addItem(new QSpacerItem(0,0,QSizePolicy::Minimum,QSizePolicy::Expanding));
620 panel->setLayout(layout);
621 return panel;
622}
623
624QuaternionDemo::QuaternionDemo()
625{
626 mRenderingWidget = new RenderingWidget();
627 setCentralWidget(mRenderingWidget);
628
629 QDockWidget* panel = new QDockWidget("navigation", this);
630 panel->setAllowedAreas((QFlags<Qt::DockWidgetArea>)(Qt::RightDockWidgetArea | Qt::LeftDockWidgetArea));
631 addDockWidget(Qt::RightDockWidgetArea, panel);
632 panel->setWidget(mRenderingWidget->createNavigationControlWidget());
633}
634
635int main(int argc, char *argv[])
636{
637 std::cout << "Navigation:\n";
638 std::cout << " left button: rotate around the target\n";
639 std::cout << " middle button: zoom\n";
640 std::cout << " left button + ctrl quake rotate (rotate around camera position)\n";
641 std::cout << " middle button + ctrl walk (progress along camera's z direction)\n";
642 std::cout << " left button: pan (translate in the XY camera's plane)\n\n";
643 std::cout << "R : move the camera to initial position\n";
644 std::cout << "A : start/stop animation\n";
645 std::cout << "C : clear the animation\n";
646 std::cout << "G : add a key frame\n";
647
648 QApplication app(argc, argv);
649 QuaternionDemo demo;
650 demo.resize(600,500);
651 demo.show();
652 return app.exec();
653}
654
655#include "quaternion_demo.moc"
656
Note: See TracBrowser for help on using the repository browser.