keyFrameInterpolator.cpp
1 /****************************************************************************
2 
3  Copyright (C) 2002-2008 Gilles Debunne. All rights reserved.
4 
5  This file is part of the QGLViewer library version 2.3.10.
6 
7  http://www.libqglviewer.com - contact@libqglviewer.com
8 
9  This file may be used under the terms of the GNU General Public License
10  versions 2.0 or 3.0 as published by the Free Software Foundation and
11  appearing in the LICENSE file included in the packaging of this file.
12  In addition, as a special exception, Gilles Debunne gives you certain
13  additional rights, described in the file GPL_EXCEPTION in this package.
14 
15  libQGLViewer uses dual licensing. Commercial/proprietary software must
16  purchase a libQGLViewer Commercial License.
17 
18  This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
19  WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
20 
21 *****************************************************************************/
22 
23 #include "domUtils.h"
24 #include "qglviewer.h" // for QGLViewer::drawAxis and Camera::drawCamera
25 
26 using namespace qglviewer;
27 using namespace std;
28 
29 #if QT_VERSION < 0x040000
30 // Patch for QPtrList / QList syntax difference
31 # define peekNext current
32 #endif
33 
41  : frame_(NULL), period_(40), interpolationTime_(0.0), interpolationSpeed_(1.0), interpolationStarted_(false),
42  closedPath_(false), loopInterpolation_(false), pathIsValid_(false), valuesAreValid_(true), currentFrameValid_(false)
43  // #CONNECTION# Values cut pasted initFromDOMElement()
44 {
45  setFrame(frame);
46 #if QT_VERSION < 0x040000
47  keyFrame_.setAutoDelete(true);
48 #endif
49  for (int i=0; i<4; ++i)
50 #if QT_VERSION >= 0x040000
51  currentFrame_[i] = new QMutableListIterator<KeyFrame*>(keyFrame_);
52 #else
53  currentFrame_[i] = new QPtrListIterator<KeyFrame>(keyFrame_);
54 #endif
55  connect(&timer_, SIGNAL(timeout()), SLOT(update()));
56 }
57 
60 {
61  deletePath();
62  for (int i=0; i<4; ++i)
63  delete currentFrame_[i];
64 }
65 
68 {
69  if (this->frame())
70  disconnect(this, SIGNAL( interpolated() ), this->frame(), SIGNAL( interpolated() ));
71 
72  frame_ = frame;
73 
74  if (this->frame())
75  connect(this, SIGNAL( interpolated() ), this->frame(), SIGNAL( interpolated() ));
76 }
77 
84 void KeyFrameInterpolator::update()
85 {
87 
88  interpolationTime_ += interpolationSpeed() * interpolationPeriod() / 1000.0;
89 
90  if (interpolationTime() > keyFrame_.last()->time())
91  {
92  if (loopInterpolation())
93  setInterpolationTime(keyFrame_.first()->time() + interpolationTime_ - keyFrame_.last()->time());
94  else
95  {
96  // Make sure last KeyFrame is reached and displayed
97  interpolateAtTime(keyFrame_.last()->time());
99  }
100  Q_EMIT endReached();
101  }
102  else
103  if (interpolationTime() < keyFrame_.first()->time())
104  {
105  if (loopInterpolation())
106  setInterpolationTime(keyFrame_.last()->time() - keyFrame_.first()->time() + interpolationTime_);
107  else
108  {
109  // Make sure first KeyFrame is reached and displayed
110  interpolateAtTime(keyFrame_.first()->time());
112  }
113  Q_EMIT endReached();
114  }
115 }
116 
117 
139 {
140  if (period >= 0)
141  setInterpolationPeriod(period);
142 
143  if (!keyFrame_.isEmpty())
144  {
145  if ((interpolationSpeed() > 0.0) && (interpolationTime() >= keyFrame_.last()->time()))
146  setInterpolationTime(keyFrame_.first()->time());
147  if ((interpolationSpeed() < 0.0) && (interpolationTime() <= keyFrame_.first()->time()))
148  setInterpolationTime(keyFrame_.last()->time());
149  timer_.start(interpolationPeriod());
150  interpolationStarted_ = true;
151  update();
152  }
153 }
154 
155 
158 {
159  timer_.stop();
160  interpolationStarted_ = false;
161 }
162 
163 
169 {
172 }
173 
186 void KeyFrameInterpolator::addKeyFrame(const Frame* const frame, float time)
187 {
188  if (!frame)
189  return;
190 
191  if (keyFrame_.isEmpty())
192  interpolationTime_ = time;
193 
194  if ( (!keyFrame_.isEmpty()) && (keyFrame_.last()->time() > time) )
195  qWarning("Error in KeyFrameInterpolator::addKeyFrame: time is not monotone");
196  else
197  keyFrame_.append(new KeyFrame(frame, time));
198  connect(frame, SIGNAL(modified()), SLOT(invalidateValues()));
199  valuesAreValid_ = false;
200  pathIsValid_ = false;
201  currentFrameValid_ = false;
203 }
204 
212 void KeyFrameInterpolator::addKeyFrame(const Frame& frame, float time)
213 {
214  if (keyFrame_.isEmpty())
215  interpolationTime_ = time;
216 
217  if ( (!keyFrame_.isEmpty()) && (keyFrame_.last()->time() > time) )
218  qWarning("Error in KeyFrameInterpolator::addKeyFrame: time is not monotone");
219  else
220  keyFrame_.append(new KeyFrame(frame, time));
221 
222  valuesAreValid_ = false;
223  pathIsValid_ = false;
224  currentFrameValid_ = false;
226 }
227 
228 
233 void KeyFrameInterpolator::addKeyFrame(const Frame* const frame)
234 {
235  float time;
236  if (keyFrame_.isEmpty())
237  time = 0.0;
238  else
239  time = lastTime() + 1.0;
240 
241  addKeyFrame(frame, time);
242 }
243 
249 {
250  float time;
251  if (keyFrame_.isEmpty())
252  time = 0.0;
253  else
254  time = keyFrame_.last()->time() + 1.0;
255 
256  addKeyFrame(frame, time);
257 }
258 
261 {
263 #if QT_VERSION >= 0x040000
264  qDeleteAll(keyFrame_);
265 #endif
266  keyFrame_.clear();
267  pathIsValid_ = false;
268  valuesAreValid_ = false;
269  currentFrameValid_ = false;
270 }
271 
272 static void drawCamera(float scale)
273 {
274  glDisable(GL_LIGHTING);
275 
276  const float halfHeight = scale * 0.07;
277  const float halfWidth = halfHeight * 1.3;
278  const float dist = halfHeight / tan(M_PI/8.0);
279 
280  const float arrowHeight = 1.5f * halfHeight;
281  const float baseHeight = 1.2f * halfHeight;
282  const float arrowHalfWidth = 0.5f * halfWidth;
283  const float baseHalfWidth = 0.3f * halfWidth;
284 
285  // Frustum outline
286  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
287  glBegin(GL_LINE_STRIP);
288  glVertex3f(-halfWidth, halfHeight,-dist);
289  glVertex3f(-halfWidth,-halfHeight,-dist);
290  glVertex3f( 0.0f, 0.0f, 0.0f);
291  glVertex3f( halfWidth,-halfHeight,-dist);
292  glVertex3f(-halfWidth,-halfHeight,-dist);
293  glEnd();
294  glBegin(GL_LINE_STRIP);
295  glVertex3f( halfWidth,-halfHeight,-dist);
296  glVertex3f( halfWidth, halfHeight,-dist);
297  glVertex3f( 0.0f, 0.0f, 0.0f);
298  glVertex3f(-halfWidth, halfHeight,-dist);
299  glVertex3f( halfWidth, halfHeight,-dist);
300  glEnd();
301 
302  // Up arrow
303  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
304  // Base
305  glBegin(GL_QUADS);
306  glVertex3f(-baseHalfWidth, halfHeight,-dist);
307  glVertex3f( baseHalfWidth, halfHeight,-dist);
308  glVertex3f( baseHalfWidth, baseHeight,-dist);
309  glVertex3f(-baseHalfWidth, baseHeight,-dist);
310  glEnd();
311 
312  // Arrow
313  glBegin(GL_TRIANGLES);
314  glVertex3f( 0.0f, arrowHeight,-dist);
315  glVertex3f(-arrowHalfWidth, baseHeight, -dist);
316  glVertex3f( arrowHalfWidth, baseHeight, -dist);
317  glEnd();
318 }
319 
351 void KeyFrameInterpolator::drawPath(int mask, int nbFrames, float scale)
352 {
353  const int nbSteps = 30;
354  if (!pathIsValid_)
355  {
356  path_.clear();
357 #if QT_VERSION < 0x040000
358  path_.reserve(nbSteps*keyFrame_.count());
359 #endif
360 
361  if (keyFrame_.isEmpty())
362  return;
363 
364  if (!valuesAreValid_)
365  updateModifiedFrameValues();
366 
367  if (keyFrame_.first() == keyFrame_.last())
368  path_.push_back(Frame(keyFrame_.first()->position(), keyFrame_.first()->orientation()));
369  else
370  {
371  static Frame fr;
372  KeyFrame* kf_[4];
373  kf_[0] = keyFrame_.first();
374  kf_[1] = kf_[0];
375 #if QT_VERSION >= 0x040000
376  int index = 1;
377  kf_[2] = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;
378  index++;
379  kf_[3] = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;
380 #else
381  kf_[2] = keyFrame_.next();
382  kf_[3] = keyFrame_.next();
383 #endif
384 
385  while (kf_[2])
386  {
387  Vec diff = kf_[2]->position() - kf_[1]->position();
388  Vec v1 = 3.0 * diff - 2.0 * kf_[1]->tgP() - kf_[2]->tgP();
389  Vec v2 = -2.0 * diff + kf_[1]->tgP() + kf_[2]->tgP();
390 
391  // cout << kf_[0]->time() << " , " << kf_[1]->time() << " , " << kf_[2]->time() << " , " << kf_[3]->time() << endl;
392  for (int step=0; step<nbSteps; ++step)
393  {
394  float alpha = step / static_cast<float>(nbSteps);
395  fr.setPosition(kf_[1]->position() + alpha * (kf_[1]->tgP() + alpha * (v1+alpha*v2)));
396  fr.setOrientation(Quaternion::squad(kf_[1]->orientation(), kf_[1]->tgQ(), kf_[2]->tgQ(), kf_[2]->orientation(), alpha));
397  path_.push_back(fr);
398  }
399 
400  // Shift
401  kf_[0] = kf_[1];
402  kf_[1] = kf_[2];
403  kf_[2] = kf_[3];
404 #if QT_VERSION >= 0x040000
405  index++;
406  kf_[3] = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;
407 #else
408  kf_[3] = keyFrame_.next();
409 #endif
410  }
411  // Add last KeyFrame
412  path_.push_back(Frame(kf_[1]->position(), kf_[1]->orientation()));
413  }
414  pathIsValid_ = true;
415  }
416 
417  if (mask)
418  {
419  glDisable(GL_LIGHTING);
420  glLineWidth(2);
421 
422  if (mask & 1)
423  {
424  glBegin(GL_LINE_STRIP);
425 #if QT_VERSION >= 0x040000
426  Q_FOREACH (Frame fr, path_)
427  glVertex3fv(fr.position());
428 #else
429 # if QT_VERSION < 0x030000
430  for (int i=0; i < path_.size(); ++i)
431  glVertex3fv((path_.at(i)).position());
432 # else
433  for (QValueVector<Frame>::const_iterator pnt=path_.begin(), end=path_.end(); pnt!=end; ++pnt)
434  glVertex3fv((*pnt).position());
435 # endif
436 #endif
437  glEnd();
438  }
439  if (mask & 6)
440  {
441  int count = 0;
442  if (nbFrames > nbSteps)
443  nbFrames = nbSteps;
444  float goal = 0.0f;
445 #if QT_VERSION >= 0x040000
446  Q_FOREACH (Frame fr, path_)
447 #else
448 # if QT_VERSION < 0x030000
449  for (int i=0; i < path_.size(); ++i)
450 # else
451  for (QValueVector<Frame>::const_iterator pnt=path_.begin(), end=path_.end(); pnt!=end; ++pnt)
452 # endif
453 #endif
454  if ((count++) >= goal)
455  {
456  goal += nbSteps / static_cast<float>(nbFrames);
457  glPushMatrix();
458 #if QT_VERSION >= 0x040000
459  glMultMatrixd(fr.matrix());
460 #else
461 # if QT_VERSION < 0x030000
462  glMultMatrixd((path_.at(i)).matrix());
463 # else
464  glMultMatrixd((*pnt).matrix());
465 # endif
466 #endif
467  if (mask & 2) drawCamera(scale);
468  if (mask & 4) QGLViewer::drawAxis(scale/10.0);
469  glPopMatrix();
470  }
471  }
472  }
473 }
474 
475 void KeyFrameInterpolator::updateModifiedFrameValues()
476 {
477  Quaternion prevQ = keyFrame_.first()->orientation();
478  KeyFrame* kf;
479 #if QT_VERSION >= 0x040000
480  for (int i=0; i<keyFrame_.size(); ++i)
481  {
482  kf = keyFrame_.at(i);
483 #else
484  for (kf = keyFrame_.first(); kf; kf=keyFrame_.next())
485  {
486 #endif
487  if (kf->frame())
488  kf->updateValuesFromPointer();
489  kf->flipOrientationIfNeeded(prevQ);
490  prevQ = kf->orientation();
491  }
492 
493  KeyFrame* prev = keyFrame_.first();
494  kf = keyFrame_.first();
495 #if QT_VERSION >= 0x040000
496  int index = 1;
497 #endif
498  while (kf)
499  {
500 #if QT_VERSION >= 0x040000
501  KeyFrame* next = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;
502  index++;
503 #else
504  KeyFrame* next = keyFrame_.next();
505 #endif
506  if (next)
507  kf->computeTangent(prev, next);
508  else
509  kf->computeTangent(prev, kf);
510  prev = kf;
511  kf = next;
512  }
513  valuesAreValid_ = true;
514 }
515 
523 {
524  const KeyFrame* const kf = keyFrame_.at(index);
525  return Frame(kf->position(), kf->orientation());
526 }
527 
532 {
533  return keyFrame_.at(index)->time();
534 }
535 
541 {
542  return lastTime() - firstTime();
543 }
544 
549 {
550  if (keyFrame_.isEmpty())
551  return 0.0;
552  else
553  return keyFrame_.first()->time();
554 }
555 
560 {
561  if (keyFrame_.isEmpty())
562  return 0.0;
563  else
564  return keyFrame_.last()->time();
565 }
566 
567 void KeyFrameInterpolator::updateCurrentKeyFrameForTime(float time)
568 {
569  // Assertion: times are sorted in monotone order.
570  // Assertion: keyFrame_ is not empty
571 
572  // TODO: Special case for loops when closed path is implemented !!
573  if (!currentFrameValid_)
574  // Recompute everything from scrach
575 #if QT_VERSION >= 0x040000
576  currentFrame_[1]->toFront();
577 #else
578  currentFrame_[1]->toFirst();
579 #endif
580 
581  while (currentFrame_[1]->peekNext()->time() > time)
582  {
583  currentFrameValid_ = false;
584 #if QT_VERSION >= 0x040000
585  if (!currentFrame_[1]->hasPrevious())
586 #else
587  if (currentFrame_[1]->atFirst())
588 #endif
589  break;
590 #if QT_VERSION >= 0x040000
591  currentFrame_[1]->previous();
592 #else
593  --(*currentFrame_[1]);
594 #endif
595  }
596 
597  if (!currentFrameValid_)
598  *currentFrame_[2] = *currentFrame_[1];
599 
600  while (currentFrame_[2]->peekNext()->time() < time)
601  {
602  currentFrameValid_ = false;
603 #if QT_VERSION >= 0x040000
604  if (!currentFrame_[2]->hasNext())
605 #else
606  if (currentFrame_[2]->atLast())
607 #endif
608  break;
609 #if QT_VERSION >= 0x040000
610  currentFrame_[2]->next();
611 #else
612  ++(*currentFrame_[2]);
613 #endif
614  }
615 
616  if (!currentFrameValid_)
617  {
618  *currentFrame_[1] = *currentFrame_[2];
619 #if QT_VERSION >= 0x040000
620  if ((currentFrame_[1]->hasPrevious()) && (time < currentFrame_[2]->peekNext()->time()))
621  currentFrame_[1]->previous();
622 #else
623  if ((!currentFrame_[1]->atFirst()) && (time < currentFrame_[2]->current()->time()))
624  --(*currentFrame_[1]);
625 #endif
626 
627  *currentFrame_[0] = *currentFrame_[1];
628 #if QT_VERSION >= 0x040000
629  if (currentFrame_[0]->hasPrevious())
630  currentFrame_[0]->previous();
631 #else
632  if (!currentFrame_[0]->atFirst())
633  --(*currentFrame_[0]);
634 #endif
635 
636  *currentFrame_[3] = *currentFrame_[2];
637 #if QT_VERSION >= 0x040000
638  if (currentFrame_[3]->hasNext())
639  currentFrame_[3]->next();
640 #else
641  if (!currentFrame_[3]->atLast())
642  ++(*currentFrame_[3]);
643 #endif
644 
645  currentFrameValid_ = true;
646  splineCacheIsValid_ = false;
647  }
648 
649  // cout << "Time = " << time << " : " << currentFrame_[0]->peekNext()->time() << " , " <<
650  // currentFrame_[1]->peekNext()->time() << " , " << currentFrame_[2]->peekNext()->time() << " , " << currentFrame_[3]->peekNext()->time() << endl;
651 }
652 
653 void KeyFrameInterpolator::updateSplineCache()
654 {
655  Vec delta = currentFrame_[2]->peekNext()->position() - currentFrame_[1]->peekNext()->position();
656  v1 = 3.0 * delta - 2.0 * currentFrame_[1]->peekNext()->tgP() - currentFrame_[2]->peekNext()->tgP();
657  v2 = -2.0 * delta + currentFrame_[1]->peekNext()->tgP() + currentFrame_[2]->peekNext()->tgP();
658  splineCacheIsValid_ = true;
659 }
660 
669 {
670  setInterpolationTime(time);
671 
672  if ((keyFrame_.isEmpty()) || (!frame()))
673  return;
674 
675  if (!valuesAreValid_)
676  updateModifiedFrameValues();
677 
678  updateCurrentKeyFrameForTime(time);
679 
680  if (!splineCacheIsValid_)
681  updateSplineCache();
682 
683  float alpha;
684  float dt = currentFrame_[2]->peekNext()->time() - currentFrame_[1]->peekNext()->time();
685  if (dt == 0.0)
686  alpha = 0.0;
687  else
688  alpha = (time - currentFrame_[1]->peekNext()->time()) / dt;
689 
690  // Linear interpolation - debug
691  // Vec pos = alpha*(currentFrame_[2]->peekNext()->position()) + (1.0-alpha)*(currentFrame_[1]->peekNext()->position());
692  Vec pos = currentFrame_[1]->peekNext()->position() + alpha * (currentFrame_[1]->peekNext()->tgP() + alpha * (v1+alpha*v2));
693  Quaternion q = Quaternion::squad(currentFrame_[1]->peekNext()->orientation(), currentFrame_[1]->peekNext()->tgQ(),
694  currentFrame_[2]->peekNext()->tgQ(), currentFrame_[2]->peekNext()->orientation(), alpha);
696 
697  Q_EMIT interpolated();
698 }
699 
715 QDomElement KeyFrameInterpolator::domElement(const QString& name, QDomDocument& document) const
716 {
717  QDomElement de = document.createElement(name);
718  int count = 0;
719 #if QT_VERSION >= 0x040000
720  Q_FOREACH (KeyFrame* kf, keyFrame_)
721 #else
722  for (KeyFrame* kf = keyFrame_.first(); kf; kf = keyFrame_.next() )
723 #endif
724  {
725  Frame fr(kf->position(), kf->orientation());
726  QDomElement kfNode = fr.domElement("KeyFrame", document);
727  kfNode.setAttribute("index", QString::number(count));
728  kfNode.setAttribute("time", QString::number(kf->time()));
729  de.appendChild(kfNode);
730  ++count;
731  }
732  de.setAttribute("nbKF", QString::number(keyFrame_.count()));
733  de.setAttribute("time", QString::number(interpolationTime()));
734  de.setAttribute("speed", QString::number(interpolationSpeed()));
735  de.setAttribute("period", QString::number(interpolationPeriod()));
736  de.setAttribute("closedPath", (closedPath()?"true":"false"));
737  de.setAttribute("loop", (loopInterpolation()?"true":"false"));
738  return de;
739 }
740 
749 void KeyFrameInterpolator::initFromDOMElement(const QDomElement& element)
750 {
751 #if QT_VERSION >= 0x040000
752  qDeleteAll(keyFrame_);
753 #endif
754  keyFrame_.clear();
755  QDomElement child=element.firstChild().toElement();
756  while (!child.isNull())
757  {
758  if (child.tagName() == "KeyFrame")
759  {
760  Frame fr;
761  fr.initFromDOMElement(child);
762  float time = DomUtils::floatFromDom(child, "time", 0.0);
763  addKeyFrame(fr, time);
764  }
765 
766  child = child.nextSibling().toElement();
767  }
768 
769  // #CONNECTION# Values cut pasted from constructor
770  setInterpolationTime(DomUtils::floatFromDom(element, "time", 0.0));
771  setInterpolationSpeed(DomUtils::floatFromDom(element, "speed", 1.0));
772  setInterpolationPeriod(DomUtils::intFromDom(element, "period", 40));
773  setClosedPath(DomUtils::boolFromDom(element, "closedPath", false));
774  setLoopInterpolation(DomUtils::boolFromDom(element, "loop", false));
775 
776  // setFrame(NULL);
777  pathIsValid_ = false;
778  valuesAreValid_ = false;
779  currentFrameValid_ = false;
780 
782 }
783 
784 #ifndef DOXYGEN
785 
787 KeyFrameInterpolator::KeyFrame::KeyFrame(const Frame& fr, float t)
788  : time_(t), frame_(NULL)
789 {
790  p_ = fr.position();
791  q_ = fr.orientation();
792 }
793 
794 KeyFrameInterpolator::KeyFrame::KeyFrame(const Frame* fr, float t)
795  : time_(t), frame_(fr)
796 {
797  updateValuesFromPointer();
798 }
799 
800 void KeyFrameInterpolator::KeyFrame::updateValuesFromPointer()
801 {
802  p_ = frame()->position();
803  q_ = frame()->orientation();
804 }
805 
806 void KeyFrameInterpolator::KeyFrame::computeTangent(const KeyFrame* const prev, const KeyFrame* const next)
807 {
808  tgP_ = 0.5 * (next->position() - prev->position());
809  tgQ_ = Quaternion::squadTangent(prev->orientation(), q_, next->orientation());
810 }
811 
812 void KeyFrameInterpolator::KeyFrame::flipOrientationIfNeeded(const Quaternion& prev)
813 {
814  if (Quaternion::dot(prev, q_) < 0.0)
815  q_.negate();
816 }
817 
818 #endif //DOXYGEN