keyFrameInterpolator.cpp

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

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

 This file is part of the QGLViewer library version 2.5.2.

 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;

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);
    for (int i=0; i<4; ++i)
        currentFrame_[i] = new QMutableListIterator<KeyFrame*>(keyFrame_);
    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();
    qDeleteAll(keyFrame_);
    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 (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];
            int index = 1;
            kf_[2] = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;
            index++;
            kf_[3] = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;

            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];
                index++;
                kf_[3] = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;
            }
            // 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);
            Q_FOREACH (Frame fr, path_)
                glVertex3fv(fr.position());
            glEnd();
        }
        if (mask & 6)
        {
            int count = 0;
            if (nbFrames > nbSteps)
                nbFrames = nbSteps;
            float goal = 0.0f;
            Q_FOREACH (Frame fr, path_)
                if ((count++) >= goal)
                {
                    goal += nbSteps / static_cast<float>(nbFrames);
                    glPushMatrix();
                    glMultMatrixd(fr.matrix());
                    if (mask & 2) drawCamera(scale);
                    if (mask & 4) QGLViewer::drawAxis(scale/10.0);
                    glPopMatrix();
                }
        }
    }
}

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

    KeyFrame* prev = keyFrame_.first();
    kf = keyFrame_.first();
    int index = 1;
    while (kf)
    {
        KeyFrame* next = (index < keyFrame_.size()) ? keyFrame_.at(index) : NULL;
        index++;
        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
        currentFrame_[1]->toFront();

    while (currentFrame_[1]->peekNext()->time() > time)
    {
        currentFrameValid_ = false;
        if (!currentFrame_[1]->hasPrevious())
            break;
        currentFrame_[1]->previous();
    }

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

    while (currentFrame_[2]->peekNext()->time() < time)
    {
        currentFrameValid_ = false;
        if (!currentFrame_[2]->hasNext())
            break;
        currentFrame_[2]->next();
    }

    if (!currentFrameValid_)
    {
        *currentFrame_[1] = *currentFrame_[2];
        if ((currentFrame_[1]->hasPrevious()) && (time < currentFrame_[2]->peekNext()->time()))
            currentFrame_[1]->previous();

        *currentFrame_[0] = *currentFrame_[1];
        if (currentFrame_[0]->hasPrevious())
            currentFrame_[0]->previous();

        *currentFrame_[3] = *currentFrame_[2];
        if (currentFrame_[3]->hasNext())
            currentFrame_[3]->next();

        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;
    Q_FOREACH (KeyFrame* kf, keyFrame_)
    {
        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()));
    DomUtils::setBoolAttribute(de, "closedPath", closedPath());
    DomUtils::setBoolAttribute(de, "loop", loopInterpolation());
    return de;
}

void KeyFrameInterpolator::initFromDOMElement(const QDomElement& element)
{
    qDeleteAll(keyFrame_);
    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