/* CoordinateInterpolator.C: CoordinateInterpolator nodes (public source) */
#include <math.h>
#include "CoordinateInterpolator.H"

namespace xrml {

static SFVec3f SFVec3f_interpolate(const SFVec3f& v1, const SFVec3f& v2, float t)
{
  float s = 1.-t;
  return SFVec3f(v1.x * s + v2.x * t,
		 v1.y * s + v2.y * t,
		 v1.z * s + v2.z * t);
}

MFVec3f CoordinateInterpolator::interpolate(float t)
{
  MFVec3f v(values_per_key);
  int i = (int)floor(t);
  int ip1 = (int)ceil(t);
  if (i == ip1) {
    for (int j=0; j<values_per_key; j++)
      v[j] = keyValue[i*values_per_key + j];
  } else {
    for (int j=0; j<values_per_key; j++)
      v[j] = SFVec3f_interpolate(keyValue[i*values_per_key+j], keyValue[ip1*values_per_key+j], t);
  }
  return v;
}

void CoordinateInterpolator::set_fraction(const SFFloat& t)
{
  if (!initialised)
    return;

  float index = get_index(t);
  if (index >= 0. && index <= (float)(keyValue.size/values_per_key - 1))
    post_value_changed(interpolate(index));
}

void CoordinateInterpolator::render(void)
{
  values_per_key = key.size>0 ? (keyValue.size / key.size) : 0;
  if (keyValue.size != key.size * values_per_key) {
    Error(NULL, "%s: number of key values is not an integer multiple of the numver of key frames", name());
    return;
  }

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

}  // namespace xrml
