/************************************************************************************
libsj
Copyright (c) 2010, Segway Japan, Ltd.
All rights reserved.

Contact us if you use this software for sell.
If you use this software not for sell, you can use this software under BSD lisence.
See the files LICENSE.TXT and LICENSE-BSD.TXT for more details.                     
************************************************************************************/
#ifndef __CURVE_H__
#define __CURVE_H__





double curve_fergusoncoons(double x0, double v0, double x1, double v1, double t)
{
  return 
      ( 2.0*x0 - 2.0*x1 +     v0 + v1)*t*t*t
    + (-3.0*x0 + 3.0*x1 - 2.0*v0 - v1)*t*t
                        +     v0      *t
          + x0;
}


double curve_cutmullrom(double p0, double p1, double p2, double p3, double t)
{
  return 0.5 * (
      (-1.0*p0 + 3.0*p1 - 3.0*p2 + 1.0*p3)*t*t*t
    + ( 2.0*p0 - 5.0*p1 + 4.0*p2 - 1.0*p3)*t*t
    + (-1.0*p0          + 1.0*p2         )*t
               + 2.0*p1
  );
}






#endif
