/* OrientationInterpolator.C: OrientationInterpolator nodes (public source) */

#include <math.h>
#include "OrientationInterpolator.H"
#include "vector.H"

namespace xrml {

static SFRotation SFRotation_interpolate(const SFRotation& r1, const SFRotation& r2, float t)
{
  // TODO: should interpolate along an arc instead of along a line
  float s = 1.-t;
  Vec3 d = Vec3(r1.x * s + r2.x * t,
		r1.y * s + r2.y * t,
		r1.z * s + r2.z * t).normalize();
  return SFRotation(d.x, d.y, d.z,
		    r1.radians * s + r2.radians * t);
}

SFRotation OrientationInterpolator::interpolate(float t)
{
  int i = (int)floor(t);
  int ip1 = (int)ceil(t);
  if (i == ip1)
    return keyValue[i];
  else
    return SFRotation_interpolate(keyValue[i], keyValue[ip1], t - floor(t));
}

void OrientationInterpolator::set_fraction(const SFFloat& t)
{
  if (!initialised)
    return;
  float index = get_index(t);
  if (index >= 0. && index <= (float)(keyValue.size-1))
    post_value_changed(interpolate(index));
}

void OrientationInterpolator::render(void)
{
  if (key.size != keyValue.size) {
    Error(NULL, "%s: number of key frames and values doesn't match", name());
    return;
  }

  if (!initialised) {
    if (keyValue.size > 0)
      value_changed = keyValue[0];
    initialised = true;
  }
}

}  // namespace xrml
