+#include <cmath>
+#include <msp/datafile/collection.h>
+#include <msp/time/units.h>
+#include "animation.h"
+#include "keyframe.h"
+
+using namespace std;
+
+#include <msp/io/print.h>
+
+namespace Msp {
+namespace GL {
+
+Animation::Animation():
+ looping(false)
+{ }
+
+void Animation::add_keyframe(const Time::TimeDelta &t, const KeyFrame &kf)
+{
+ if(!keyframes.empty() && t<keyframes.back().time)
+ throw invalid_argument("Animation::add_keyframe");
+
+ TimedKeyFrame tkf;
+ tkf.time = t;
+ tkf.keyframe = &kf;
+ tkf.keyframe.keep();
+ prepare_keyframe(tkf);
+ keyframes.push_back(tkf);
+}
+
+void Animation::prepare_keyframe(TimedKeyFrame &tkf)
+{
+ tkf.prev = (keyframes.empty() ? 0 : &keyframes.back());
+ if(!tkf.prev)
+ return;
+
+ tkf.delta_t = tkf.time-tkf.prev->time;
+
+ const double *m1_data = tkf.prev->keyframe->get_matrix().data();
+ const double *m2_data = tkf.keyframe->get_matrix().data();
+ for(unsigned i=0; i<3; ++i)
+ {
+ const double *m1_col = m1_data+i*4;
+ const double *m2_col = m2_data+i*4;
+
+ // Compute a normalized vector halfway between the two endpoints
+ double half[3];
+ double len = 0;
+ for(unsigned j=0; j<3; ++j)
+ {
+ half[j] = (m1_col[j]+m2_col[j])/2;
+ len += half[j]*half[j];
+ }
+ len = sqrt(len);
+ for(unsigned j=0; j<3; ++j)
+ half[j] /= len;
+
+ // Compute correction factors for smooth interpolation
+ double cos_half = m1_col[0]*half[0]+m1_col[1]*half[1]+m1_col[2]*half[2];
+ double angle = acos(cos_half);
+ tkf.axes[i].slope = (angle ? angle/tan(angle) : 1);
+ tkf.axes[i].scale = cos_half;
+ }
+}
+
+Matrix Animation::compute_matrix(const TimedKeyFrame &tkf, const Time::TimeDelta &dt) const
+{
+ if(!dt)
+ return tkf.keyframe->get_matrix();
+ if(!tkf.prev)
+ throw invalid_argument("Animation::compute_matrix");
+ const TimedKeyFrame &prev = *tkf.prev;
+
+ float t = dt/tkf.delta_t;
+ float u = t*2.0f-1.0f;
+
+ double matrix[16];
+ const double *m1_data = prev.keyframe->get_matrix().data();
+ const double *m2_data = tkf.keyframe->get_matrix().data();
+ for(unsigned i=0; i<4; ++i)
+ {
+ const double *m1_col = m1_data+i*4;
+ const double *m2_col = m2_data+i*4;
+ double *out_col = matrix+i*4;
+
+ if(i<3)
+ {
+ /* Linear interpolation will produce vectors that fall on the line
+ between the two endpoints, and has a higher angular velocity near the
+ middle. We compensate for the velocity by interpolating the angle
+ around the halfway point and computing its tangent. This is
+ approximated by a third degree polynomial, scaled so that the result
+ will be in the range [-1, 1]. */
+ float w = (tkf.axes[i].slope+(1-tkf.axes[i].slope)*u*u)*u*0.5f+0.5f;
+
+ /* The interpolate vectors will also be shorter than unit length. At
+ the halfway point the length will be equal to the cosine of half the
+ angle, which was computed earlier. Use a second degree polynomial to
+ approximate. */
+ float n = (tkf.axes[i].scale+(1-tkf.axes[i].scale)*u*u);
+
+ for(unsigned j=0; j<3; ++j)
+ out_col[j] = ((1-w)*m1_col[j]+w*m2_col[j])/n;
+ }
+ else
+ {
+ for(unsigned j=0; j<3; ++j)
+ out_col[j] = (1-t)*m1_col[j]+t*m2_col[j];
+ }
+ }
+
+ matrix[3] = 0;
+ matrix[7] = 0;
+ matrix[11] = 0;
+ matrix[15] = 1;
+
+ return matrix;
+}
+
+
+Animation::AxisInterpolation::AxisInterpolation():
+ slope(0),
+ scale(0)
+{ }
+
+
+Animation::Iterator::Iterator(const Animation &a):
+ animation(a),
+ iter(animation.keyframes.begin()),
+ end(false)
+{ }
+
+Animation::Iterator &Animation::Iterator::operator+=(const Time::TimeDelta &t)
+{
+ time_since_keyframe += t;
+ while(time_since_keyframe>iter->delta_t)
+ {
+ KeyFrameList::const_iterator next = iter;
+ ++next;
+ if(next==animation.keyframes.end())
+ {
+ if(animation.looping)
+ next = animation.keyframes.begin();
+ else
+ {
+ end = true;
+ time_since_keyframe = iter->delta_t;
+ break;
+ }
+ }
+
+ time_since_keyframe -= iter->delta_t;
+ iter = next;
+ }
+
+ return *this;
+}
+
+Matrix Animation::Iterator::get_matrix() const
+{
+ return animation.compute_matrix(*iter, time_since_keyframe);
+}
+
+
+Animation::Loader::Loader(Animation &a):
+ DataFile::CollectionObjectLoader<Animation>(a, 0)
+{
+ init();
+}
+
+Animation::Loader::Loader(Animation &a, Collection &c):
+ DataFile::CollectionObjectLoader<Animation>(a, &c)
+{
+ init();
+}
+
+void Animation::Loader::init()
+{
+ add("interval", &Loader::interval);
+ add("keyframe", &Loader::keyframe);
+ add("keyframe", &Loader::keyframe_inline);
+ add("looping", &Animation::looping);
+}
+
+void Animation::Loader::keyframe(const string &n)
+{
+ obj.add_keyframe(current_time, get_collection().get<KeyFrame>(n));
+}
+
+void Animation::Loader::keyframe_inline()
+{
+ RefPtr<KeyFrame> kf = new KeyFrame;
+ load_sub(*kf);
+
+ TimedKeyFrame tkf;
+ tkf.time = current_time;
+ tkf.keyframe = kf;
+ obj.prepare_keyframe(tkf);
+ obj.keyframes.push_back(tkf);
+}
+
+void Animation::Loader::interval(float t)
+{
+ current_time += t*Time::sec;
+}
+
+} // namespace GL
+} // namespace Msp