Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * sinusoidal.cpp - Sinusoidal interpolator 00004 * 00005 * Created: Tue Nov 18 11:27:44 2008 00006 * Copyright 2008 Tim Niemueller [www.niemueller.de] 00007 * 2008 Graeme McPhillips 00008 * 2008 Stephen Marais 00009 * 00010 ****************************************************************************/ 00011 00012 /* This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU General Public License as published by 00014 * the Free Software Foundation; either version 2 of the License, or 00015 * (at your option) any later version. A runtime exception applies to 00016 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00017 * 00018 * This program is distributed in the hope that it will be useful, 00019 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00020 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00021 * GNU Library General Public License for more details. 00022 * 00023 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00024 */ 00025 00026 #include <utils/math/interpolation/sinusoidal.h> 00027 00028 #include <cmath> 00029 00030 namespace fawkes { 00031 #if 0 /* just to make Emacs auto-indent happy */ 00032 } 00033 #endif 00034 00035 /** @class SinusoidalInterpolator <utils/math/interpolation/linear.h> 00036 * Sinusoidal value interpolator. 00037 * The interpolator creates intermediate points given a starting and and 00038 * end point and time constraints. Times are supplied in a discrete unit like 00039 * miliseconds or microseconds. 00040 * The values are interpolated on a sinusoidal curve with a slow start, the 00041 * greatest slope in the middle and then a slow down in the end. This 00042 * interpolation is useful for example for smooth servo movements. 00043 * 00044 * The calculation is executed with the following equation: 00045 * \f[ 00046 * \left(\frac{1}{2} \sin\left(\frac{1}{2} + \frac{t_\mathrm{current}}{t_\mathrm{end}} \pi \right) + \frac{1}{2}\right) \cdot (v_\mathrm{end} - v_\mathrm{start}) + v_\mathrm{start} 00047 * \f] 00048 * 00049 * @author Tim Niemueller 00050 * @author Graeme McPhillips 00051 * @author Stephen Marais 00052 */ 00053 00054 float 00055 SinusoidalInterpolator::interpolate(float t_current, float t_end, float t_step, 00056 float v_start, float v_end) 00057 { 00058 return ( sin((-0.5 + (t_current / t_end)) * M_PI) / 2.0 + 0.5) * (v_end - v_start) + v_start; 00059 } 00060 00061 00062 } // end namespace fawkes