/*
 * PVVMUD a 3D MUD
 * Copyright (C) 1998-1999  Programvareverkstedet (pvv@pvv.org)
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
#include "pvvmud.H"
#include "spline.H"

////////////////////////////////////////////////////////////////////////////////
//
// CSpline
//
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Constructor
CSpline::CSpline(CMatrix * basis){
  m_basis = basis;
  m_basis->transpose();
}

////////////////////////////////////////////////////////////////////////////////
// interpolate
CVector CSpline::interpolate(double time, ControlVector_t * controlVector){
  int size = (*controlVector).size();
  if (size < 4) return CVector(0.0,0.0,0.0);

  double numSeg = floor(size/4);
  double seg = floor(numSeg*time) ;
  if (seg == numSeg) seg--;

  int start = (int)seg*4;

  time = time*numSeg - seg;

  CMatrix control((*controlVector)[start],(*controlVector)[start+1],
                  (*controlVector)[start+2],(*controlVector)[start+3]);

  return interpolate(time,control);
}
 
////////////////////////////////////////////////////////////////////////////////
// interpolate
CVector CSpline::interpolate(double time, CMatrix & control){
  CMatrix coefficients = control*(*m_basis);

  CVector u(time*time*time,time*time,time,1);

  return coefficients*u;
}

////////////////////////////////////////////////////////////////////////////////
//
// CCatmulRom
//
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Constructor
CCatmullRom::CCatmullRom(double s)
  :CSpline(new CMatrix(    -s, 2.0-s,     s-2.0,   s,
                        2.0*s, s-3.0, 3.0-2.0*s,  -s,
                           -s,   0.0,         s, 0.0,
                          0.0,   1.0,       0.0, 0.0))
{
}

////////////////////////////////////////////////////////////////////////////////
// addControlVector
void CCatmullRom::addControlVector(CKeyFrame * keyFrame, 
                                   const ControlVector_t & controlVector)
{
  for (int index = 0; index < (controlVector.size()-3); index++){
    keyFrame->addControlVector(controlVector[index]);
    keyFrame->addControlVector(controlVector[index+1]);
    keyFrame->addControlVector(controlVector[index+2]);
    keyFrame->addControlVector(controlVector[index+3]);
  }
}

////////////////////////////////////////////////////////////////////////////////
//
// Bezier
//
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Constructor
CBezier::CBezier():CSpline(&m_bezier){
}

////////////////////////////////////////////////////////////////////////////////
// addControlVector
void CBezier::addControlVector(CKeyFrame * keyFrame, 
                      const ControlVector_t & controlVector)
{
  for (int index = 0; index < ((controlVector.size()-4)/3 +1); index++){
    keyFrame->addControlVector(controlVector[index*3]);
    keyFrame->addControlVector(controlVector[index*3+1]);
    keyFrame->addControlVector(controlVector[index*3+2]);
    keyFrame->addControlVector(controlVector[index*3+3]);
  }
}

////////////////////////////////////////////////////////////////////////////////
// static basis
CMatrix CBezier::m_bezier( -1.0,  3.0, -3.0,  1.0,
                            3.0, -6.0,  3.0,  0.0,
                           -3.0,  3.0,  0.0,  0.0,
                            1.0,  0.0,  0.0,  0.0);

////////////////////////////////////////////////////////////////////////////////
//
// Hermite
//
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Constructor
CHermite::CHermite():CSpline(&m_hermite){
}

////////////////////////////////////////////////////////////////////////////////
// addControlVector
void CHermite::addControlVector(CKeyFrame * keyFrame, 
                      const ControlVector_t & controlVector)
{
  for (int index = 0; index < ((controlVector.size()-4)/2 +1); index++){
    keyFrame->addControlVector(controlVector[index*2]);
    keyFrame->addControlVector(controlVector[index*2+2]);
    keyFrame->addControlVector(controlVector[index*2+1]);
    keyFrame->addControlVector(controlVector[index*2+3]);
  }
}

////////////////////////////////////////////////////////////////////////////////
// static basis
CMatrix CHermite::m_hermite(  2.0, -2.0,  1.0,  1.0,
                             -3.0,  3.0, -2.0, -1.0,
                              0.0,  0.0,  1.0,  0.0,
                              1.0,  0.0,  0.0,  0.0);