keyFrameInterpolator.cpp

Go to the documentation of this file.

/****************************************************************************

 Copyright (C) 2002-2013 Gilles Debunne. All rights reserved.

 This file is part of the QGLViewer library version 2.4.0.

 http://www.libqglviewer.com - contact@libqglviewer.com

 This file may be used under the terms of the GNU General Public License 
 versions 2.0 or 3.0 as published by the Free Software Foundation and
 appearing in the LICENSE file included in the packaging of this file.
 In addition, as a special exception, Gilles Debunne gives you certain 
 additional rights, described in the file GPL_EXCEPTION in this package.

 libQGLViewer uses dual licensing. Commercial/proprietary software must
 purchase a libQGLViewer Commercial License.

 This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.

*****************************************************************************/

#include "domUtils.h"
#include "qglviewer.h" // for QGLViewer::drawAxis and Camera::drawCamera

using namespace qglviewer;
using namespace std;

#if QT_VERSION < 0x040000
// Patch for QPtrList / QList syntax difference
# define peekNext current
#endif

KeyFrameInterpolator::KeyFrameInterpolator(Frame* frame)
  : frame_(NULL), period_(40), interpolationTime_(0.0), interpolationSpeed_(1.0), interpolationStarted_(false),
    closedPath_(false), loopInterpolation_(false), pathIsValid_(false), valuesAreValid_(true), currentFrameValid_(false)
  // #CONNECTION# Values cut pasted initFromDOMElement()
{
  setFrame(frame);
#if QT_VERSION < 0x040000
  keyFrame_.setAutoDelete(true);
#endif
  for (int i=0; i<4; ++i)
#if QT_VERSION >= 0x040000
    currentFrame_[i] = new QMutableListIterator<KeyFrame*>(keyFrame_);
#else
    currentFrame_[i] = new QPtrListIterator<KeyFrame>(keyFrame_);
#endif
  connect(&timer_, SIGNAL(timeout()), SLOT(update()));
}

KeyFrameInterpolator::~KeyFrameInterpolator()
{
  deletePath();
  for (int i=0; i<4; ++i)
    delete currentFrame_[i];
}

void KeyFrameInterpolator::setFrame(Frame* const frame)
{
  if (this->frame())
    disconnect(this, SIGNAL( interpolated() ), this->frame(), SIGNAL( interpolated() ));

  frame_ = frame;

  if (this->frame())
    connect(this, SIGNAL( interpolated() ), this->frame(), SIGNAL( interpolated() ));
}

void KeyFrameInterpolator::update()
{
  interpolateAtTime(interpolationTime());

  interpolationTime_ += interpolationSpeed() * interpolationPeriod() / 1000.0;

  if (interpolationTime() > keyFrame_.last()->time())
    {
      if (loopInterpolation())
        setInterpolationTime(keyFrame_.first()->time() + interpolationTime_ - keyFrame_.last()->time());
      else
        {
          // Make sure last KeyFrame is reached and displayed
          interpolateAtTime(keyFrame_.last()->time());
          stopInterpolation();
        }
      Q_EMIT endReached();
    }
  else
    if (interpolationTime() < keyFrame_.first()->time())
      {
        if (loopInterpolation())
          setInterpolationTime(keyFrame_.last()->time() - keyFrame_.first()->time() + interpolationTime_);
        else
          {
            // Make sure first KeyFrame is reached and displayed
            interpolateAtTime(keyFrame_.first()->time());
            stopInterpolation();
          }
        Q_EMIT endReached();
      }
}


void KeyFrameInterpolator::startInterpolation(int period)
{
  if (period >= 0)
    setInterpolationPeriod(period);

  if (!keyFrame_.isEmpty())
    {
      if ((interpolationSpeed() > 0.0) && (interpolationTime() >= keyFrame_.last()->time()))
        setInterpolationTime(keyFrame_.first()->time());
      if ((interpolationSpeed() < 0.0) && (interpolationTime() <= keyFrame_.first()->time()))
        setInterpolationTime(keyFrame_.last()->time());
      timer_.start(interpolationPeriod());
      interpolationStarted_ = true;
      update();
    }
}


void KeyFrameInterpolator::stopInterpolation()
{
  timer_.stop();
  interpolationStarted_ = false;
}


void KeyFrameInterpolator::resetInterpolation()
{
  stopInterpolation();
  setInterpolationTime(firstTime());
}

void KeyFrameInterpolator::addKeyFrame(const Frame* const frame, float time)
{
  if (!frame)
    return;

  if (keyFrame_.isEmpty())
    interpolationTime_ = time;

  if ( (!keyFrame_.isEmpty()) && (keyFrame_.last()->time() > time) )
    qWarning("Error in KeyFrameInterpolator::addKeyFrame: time is not monotone");
  else
    keyFrame_.append(new KeyFrame(frame, time));
  connect(frame, SIGNAL(modified()), SLOT(invalidateValues()));
  valuesAreValid_ = false;
  pathIsValid_ = false;
  currentFrameValid_ = false;
  resetInterpolation();
}

void KeyFrameInterpolator::addKeyFrame(const Frame& frame, float time)
{
  if (keyFrame_.isEmpty())
    interpolationTime_ = time;

  if ( (!keyFrame_.isEmpty()) && (keyFrame_.last()->time() > time) )
    qWarning("Error in KeyFrameInterpolator::addKeyFrame: time is not monotone");
  else
    keyFrame_.append(new KeyFrame(frame, time));

  valuesAreValid_ = false;
  pathIsValid_ = false;
  currentFrameValid_ = false;
  resetInterpolation();
}


void KeyFrameInterpolator::addKeyFrame(const Frame* const frame)
{
  float time;
  if (keyFrame_.isEmpty())
    time = 0.0;
  else
    time = lastTime() + 1.0;

  addKeyFrame(frame, time);
}

void KeyFrameInterpolator::addKeyFrame(const Frame& frame)
{
  float time;
  if (keyFrame_.isEmpty())
    time = 0.0;
  else
    time = keyFrame_.last()->time() + 1.0;

  addKeyFrame(frame, time);
}

void KeyFrameInterpolator::deletePath()
{
  stopInterpolation();
#if QT_VERSION >= 0x040000
  qDeleteAll(keyFrame_);
#endif
  keyFrame_.clear();
  pathIsValid_ = false;
  valuesAreValid_ = false;
  currentFrameValid_ = false;
}

static void drawCamera(float scale)
{
  glDisable(GL_LIGHTING);

  const float halfHeight = scale * 0.07;
  const float halfWidth  = halfHeight * 1.3;
  const float dist = halfHeight / tan(M_PI/8.0);

  const float arrowHeight    = 1.5f * halfHeight;
  const float baseHeight     = 1.2f * halfHeight;
  const float arrowHalfWidth = 0.5f * halfWidth;
  const float baseHalfWidth  = 0.3f * halfWidth;

  // Frustum outline
  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
  glBegin(GL_LINE_STRIP);
  glVertex3f(-halfWidth, halfHeight,-dist);
  glVertex3f(-halfWidth,-halfHeight,-dist);
  glVertex3f( 0.0f, 0.0f, 0.0f);
  glVertex3f( halfWidth,-halfHeight,-dist);
  glVertex3f(-halfWidth,-halfHeight,-dist);
  glEnd();
  glBegin(GL_LINE_STRIP);
  glVertex3f( halfWidth,-halfHeight,-dist);
  glVertex3f( halfWidth, halfHeight,-dist);
  glVertex3f( 0.0f, 0.0f, 0.0f);
  glVertex3f(-halfWidth, halfHeight,-dist);
  glVertex3f( halfWidth, halfHeight,-dist);
  glEnd();

  // Up arrow
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
  // Base
  glBegin(GL_QUADS);
  glVertex3f(-baseHalfWidth, halfHeight,-dist);
  glVertex3f( baseHalfWidth, halfHeight,-dist);
  glVertex3f( baseHalfWidth, baseHeight,-dist);
  glVertex3f(-baseHalfWidth, baseHeight,-dist);
  glEnd();

  // Arrow
  glBegin(GL_TRIANGLES);
  glVertex3f( 0.0f,           arrowHeight,-dist);
  glVertex3f(-arrowHalfWidth, baseHeight, -dist);
  glVertex3f( arrowHalfWidth, baseHeight, -dist);
  glEnd();
}

void KeyFrameInterpolator::drawPath(int mask, int nbFrames, float scale)
{
  const int nbSteps = 30;
  if (!pathIsValid_)
    {
      path_.clear();
#if QT_VERSION < 0x040000
      path_.reserve(nbSteps*keyFrame_.count());
#endif

      if (keyFrame_.isEmpty())
        return;

      if (!valuesAreValid_)
        updateModifiedFrameValues();

      if (keyFrame_.first() == keyFrame_.last())
        path_.push_back(Frame(keyFrame_.first()->position(), keyFrame_.first()->orientation()));
      else
        {
          static Frame fr;
          KeyFrame* kf_[4];
          kf_[0] = keyFrame_.first();
          kf_[1] = kf_[0];
#if QT_VERSION >= 0x040000
          int index = 1;
          kf_[2] = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;
          index++;
          kf_[3] = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;
#else
          kf_[2] = keyFrame_.next();
          kf_[3] = keyFrame_.next();
#endif

          while (kf_[2])
            {
              Vec diff = kf_[2]->position() - kf_[1]->position();
              Vec v1 = 3.0 * diff - 2.0 * kf_[1]->tgP() - kf_[2]->tgP();
              Vec v2 = -2.0 * diff + kf_[1]->tgP() + kf_[2]->tgP();

              // cout << kf_[0]->time() << " , " << kf_[1]->time() << " , " << kf_[2]->time() << " , " << kf_[3]->time() << endl;
              for (int step=0; step<nbSteps; ++step)
                {
                  float alpha = step / static_cast<float>(nbSteps);
                  fr.setPosition(kf_[1]->position() + alpha * (kf_[1]->tgP() + alpha * (v1+alpha*v2)));
                  fr.setOrientation(Quaternion::squad(kf_[1]->orientation(), kf_[1]->tgQ(), kf_[2]->tgQ(), kf_[2]->orientation(), alpha));
                  path_.push_back(fr);
                }

              // Shift
              kf_[0] = kf_[1];
              kf_[1] = kf_[2];
              kf_[2] = kf_[3];
#if QT_VERSION >= 0x040000
              index++;
              kf_[3] = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;
#else
              kf_[3] = keyFrame_.next();
#endif
            }
          // Add last KeyFrame
          path_.push_back(Frame(kf_[1]->position(), kf_[1]->orientation()));
        }
      pathIsValid_ = true;
    }

  if (mask)
    {
      glDisable(GL_LIGHTING);
      glLineWidth(2);

      if (mask & 1)
        {
          glBegin(GL_LINE_STRIP);
#if QT_VERSION >= 0x040000
          Q_FOREACH (Frame fr, path_)
            glVertex3fv(fr.position());
#else
# if QT_VERSION < 0x030000
          for (int i=0; i < path_.size(); ++i)
            glVertex3fv((path_.at(i)).position());
# else
          for (QValueVector<Frame>::const_iterator pnt=path_.begin(), end=path_.end(); pnt!=end; ++pnt)
            glVertex3fv((*pnt).position());
# endif
#endif
          glEnd();
        }
      if (mask & 6)
        {
          int count = 0;
          if (nbFrames > nbSteps)
            nbFrames = nbSteps;
          float goal = 0.0f;
#if QT_VERSION >= 0x040000
          Q_FOREACH (Frame fr, path_)
#else
# if QT_VERSION < 0x030000
          for (int i=0; i < path_.size(); ++i)
# else
          for (QValueVector<Frame>::const_iterator pnt=path_.begin(), end=path_.end(); pnt!=end; ++pnt)
# endif
#endif
              if ((count++) >= goal)
                {
                  goal += nbSteps / static_cast<float>(nbFrames);
                  glPushMatrix();
#if QT_VERSION >= 0x040000
                  glMultMatrixd(fr.matrix());
#else
# if QT_VERSION < 0x030000
                  glMultMatrixd((path_.at(i)).matrix());
# else
                  glMultMatrixd((*pnt).matrix());
# endif
#endif
                  if (mask & 2) drawCamera(scale);
                  if (mask & 4) QGLViewer::drawAxis(scale/10.0);
                  glPopMatrix();
                }
        }
    }
}

void KeyFrameInterpolator::updateModifiedFrameValues()
{
  Quaternion prevQ = keyFrame_.first()->orientation();
  KeyFrame* kf;
#if QT_VERSION >= 0x040000
  for (int i=0; i<keyFrame_.size(); ++i)
    {
      kf = keyFrame_.at(i);
#else
  for (kf = keyFrame_.first(); kf; kf=keyFrame_.next())
    {
#endif
      if (kf->frame())
        kf->updateValuesFromPointer();
      kf->flipOrientationIfNeeded(prevQ);
      prevQ = kf->orientation();
    }

  KeyFrame* prev = keyFrame_.first();
  kf = keyFrame_.first();
#if QT_VERSION >= 0x040000
  int index = 1;
#endif
  while (kf)
    {
#if QT_VERSION >= 0x040000
      KeyFrame* next = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;
      index++;
#else
      KeyFrame* next = keyFrame_.next();
#endif
      if (next)
        kf->computeTangent(prev, next);
      else
        kf->computeTangent(prev, kf);
      prev = kf;
      kf = next;
    }
  valuesAreValid_ = true;
}

Frame KeyFrameInterpolator::keyFrame(int index) const
{
  const KeyFrame* const kf = keyFrame_.at(index);
  return Frame(kf->position(), kf->orientation());
}

float KeyFrameInterpolator::keyFrameTime(int index) const
{
  return keyFrame_.at(index)->time();
}

float KeyFrameInterpolator::duration() const
{
  return lastTime() - firstTime();
}

float KeyFrameInterpolator::firstTime() const
{
  if (keyFrame_.isEmpty())
    return 0.0;
  else
    return keyFrame_.first()->time();
}

float KeyFrameInterpolator::lastTime() const
{
  if (keyFrame_.isEmpty())
    return 0.0;
  else
    return keyFrame_.last()->time();
}

void KeyFrameInterpolator::updateCurrentKeyFrameForTime(float time)
{
  // Assertion: times are sorted in monotone order.
  // Assertion: keyFrame_ is not empty

  // TODO: Special case for loops when closed path is implemented !!
  if (!currentFrameValid_)
    // Recompute everything from scrach
#if QT_VERSION >= 0x040000
    currentFrame_[1]->toFront();
#else
    currentFrame_[1]->toFirst();
#endif

  while (currentFrame_[1]->peekNext()->time() > time)
    {
      currentFrameValid_ = false;
#if QT_VERSION >= 0x040000
      if (!currentFrame_[1]->hasPrevious())
#else
      if (currentFrame_[1]->atFirst())
#endif
        break;
#if QT_VERSION >= 0x040000
      currentFrame_[1]->previous();
#else
      --(*currentFrame_[1]);
#endif
    }

  if (!currentFrameValid_)
    *currentFrame_[2] = *currentFrame_[1];

  while (currentFrame_[2]->peekNext()->time() < time)
    {
      currentFrameValid_ = false;
#if QT_VERSION >= 0x040000
      if (!currentFrame_[2]->hasNext())
#else
      if (currentFrame_[2]->atLast())
#endif
        break;
#if QT_VERSION >= 0x040000
      currentFrame_[2]->next();
#else
      ++(*currentFrame_[2]);
#endif
    }

  if (!currentFrameValid_)
    {
      *currentFrame_[1] = *currentFrame_[2];
#if QT_VERSION >= 0x040000
      if ((currentFrame_[1]->hasPrevious()) && (time < currentFrame_[2]->peekNext()->time()))
        currentFrame_[1]->previous();
#else
      if ((!currentFrame_[1]->atFirst()) && (time < currentFrame_[2]->current()->time()))
        --(*currentFrame_[1]);
#endif

      *currentFrame_[0] = *currentFrame_[1];
#if QT_VERSION >= 0x040000
      if (currentFrame_[0]->hasPrevious())
        currentFrame_[0]->previous();
#else
      if (!currentFrame_[0]->atFirst())
        --(*currentFrame_[0]);
#endif

      *currentFrame_[3] = *currentFrame_[2];
#if QT_VERSION >= 0x040000
      if (currentFrame_[3]->hasNext())
        currentFrame_[3]->next();
#else
      if (!currentFrame_[3]->atLast())
        ++(*currentFrame_[3]);
#endif

      currentFrameValid_ = true;
      splineCacheIsValid_ = false;
    }

  // cout << "Time = " << time << " : " << currentFrame_[0]->peekNext()->time() << " , " <<
  // currentFrame_[1]->peekNext()->time() << " , " << currentFrame_[2]->peekNext()->time() << " , " <<  currentFrame_[3]->peekNext()->time() << endl;
}

void KeyFrameInterpolator::updateSplineCache()
{
  Vec delta = currentFrame_[2]->peekNext()->position() - currentFrame_[1]->peekNext()->position();
  v1 = 3.0 * delta - 2.0 * currentFrame_[1]->peekNext()->tgP() - currentFrame_[2]->peekNext()->tgP();
  v2 = -2.0 * delta + currentFrame_[1]->peekNext()->tgP() + currentFrame_[2]->peekNext()->tgP();
  splineCacheIsValid_ = true;
}

void KeyFrameInterpolator::interpolateAtTime(float time)
{
  setInterpolationTime(time);

  if ((keyFrame_.isEmpty()) || (!frame()))
    return;

  if (!valuesAreValid_)
    updateModifiedFrameValues();

  updateCurrentKeyFrameForTime(time);

  if (!splineCacheIsValid_)
    updateSplineCache();

  float alpha;
  float dt = currentFrame_[2]->peekNext()->time() - currentFrame_[1]->peekNext()->time();
  if (dt == 0.0)
    alpha = 0.0;
  else
    alpha = (time - currentFrame_[1]->peekNext()->time()) / dt;

  // Linear interpolation - debug
  // Vec pos = alpha*(currentFrame_[2]->peekNext()->position()) + (1.0-alpha)*(currentFrame_[1]->peekNext()->position());
  Vec pos = currentFrame_[1]->peekNext()->position() + alpha * (currentFrame_[1]->peekNext()->tgP() + alpha * (v1+alpha*v2));
  Quaternion q = Quaternion::squad(currentFrame_[1]->peekNext()->orientation(), currentFrame_[1]->peekNext()->tgQ(),
                                   currentFrame_[2]->peekNext()->tgQ(), currentFrame_[2]->peekNext()->orientation(), alpha);
  frame()->setPositionAndOrientationWithConstraint(pos, q);

  Q_EMIT interpolated();
}

QDomElement KeyFrameInterpolator::domElement(const QString& name, QDomDocument& document) const
{
  QDomElement de = document.createElement(name);
  int count = 0;
#if QT_VERSION >= 0x040000
  Q_FOREACH (KeyFrame* kf, keyFrame_)
#else
  for (KeyFrame* kf = keyFrame_.first(); kf; kf = keyFrame_.next() )
#endif
    {
      Frame fr(kf->position(), kf->orientation());
      QDomElement kfNode = fr.domElement("KeyFrame", document);
      kfNode.setAttribute("index", QString::number(count));
      kfNode.setAttribute("time",  QString::number(kf->time()));
      de.appendChild(kfNode);
      ++count;
    }
  de.setAttribute("nbKF", QString::number(keyFrame_.count()));
  de.setAttribute("time", QString::number(interpolationTime()));
  de.setAttribute("speed", QString::number(interpolationSpeed()));
  de.setAttribute("period", QString::number(interpolationPeriod()));
  de.setAttribute("closedPath", (closedPath()?"true":"false"));
  de.setAttribute("loop", (loopInterpolation()?"true":"false"));
  return de;
}

void KeyFrameInterpolator::initFromDOMElement(const QDomElement& element)
{
#if QT_VERSION >= 0x040000
  qDeleteAll(keyFrame_);
#endif
  keyFrame_.clear();
  QDomElement child=element.firstChild().toElement();
  while (!child.isNull())
    {
      if (child.tagName() == "KeyFrame")
        {
          Frame fr;
          fr.initFromDOMElement(child);
          float time = DomUtils::floatFromDom(child, "time", 0.0);
          addKeyFrame(fr, time);
        }

      child = child.nextSibling().toElement();
    }

  // #CONNECTION# Values cut pasted from constructor
  setInterpolationTime(DomUtils::floatFromDom(element, "time", 0.0));
  setInterpolationSpeed(DomUtils::floatFromDom(element, "speed", 1.0));
  setInterpolationPeriod(DomUtils::intFromDom(element, "period", 40));
  setClosedPath(DomUtils::boolFromDom(element, "closedPath", false));
  setLoopInterpolation(DomUtils::boolFromDom(element, "loop", false));

  // setFrame(NULL);
  pathIsValid_ = false;
  valuesAreValid_ = false;
  currentFrameValid_ = false;

  stopInterpolation();
}

#ifndef DOXYGEN

KeyFrameInterpolator::KeyFrame::KeyFrame(const Frame& fr, float t)
  : time_(t), frame_(NULL)
{
  p_ = fr.position();
  q_ = fr.orientation();
}

KeyFrameInterpolator::KeyFrame::KeyFrame(const Frame* fr, float t)
  : time_(t), frame_(fr)
{
  updateValuesFromPointer();
}

void KeyFrameInterpolator::KeyFrame::updateValuesFromPointer()
{
  p_ = frame()->position();
  q_ = frame()->orientation();
}

void KeyFrameInterpolator::KeyFrame::computeTangent(const KeyFrame* const prev, const KeyFrame* const next)
{
  tgP_ = 0.5 * (next->position() - prev->position());
  tgQ_ = Quaternion::squadTangent(prev->orientation(), q_, next->orientation());
}

void KeyFrameInterpolator::KeyFrame::flipOrientationIfNeeded(const Quaternion& prev)
{
  if (Quaternion::dot(prev, q_) < 0.0)
    q_.negate();
}

#endif //DOXYGEN