spline_smoother: spline_smoother: splines.h Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00038 #ifndef SPLINE_SMOOTHER_SPLINES_H_
00039 #define SPLINE_SMOOTHER_SPLINES_H_
00040 
00041 #include <vector>
00042 
00043 namespace spline_smoother
00044 {
00045 
00056 void getQuinticSplineCoefficients(double start_pos, double start_vel, double start_acc,
00057     double end_pos, double end_vel, double end_acc, double time, std::vector<double>& coefficients);
00058 
00069 void getCubicSplineCoefficients(double start_pos, double start_vel,
00070     double end_pos, double end_vel, double time, std::vector<double>& coefficients);
00071 
00075 void sampleQuinticSpline(const std::vector<double>& coefficients, double time,
00076     double& position, double& velocity, double& acceleration);
00077 
00081 void sampleCubicSpline(const std::vector<double>& coefficients, double time,
00082     double& position, double& velocity, double& acceleration);
00083 
00084 
00086 
00087 
00088 static inline void generatePowers(int n, double x, double* powers)
00089 {
00090   powers[0] = 1.0;
00091   for (int i=1; i<=n; i++)
00092   {
00093     powers[i] = powers[i-1]*x;
00094   }
00095 }
00096 
00097 inline void getQuinticSplineCoefficients(double start_pos, double start_vel, double start_acc,
00098     double end_pos, double end_vel, double end_acc, double time, std::vector<double>& coefficients)
00099 {
00100   coefficients.resize(6);
00101 
00102   double T[6];
00103   generatePowers(5, time, T);
00104 
00105   coefficients[0] = start_pos;
00106   coefficients[1] = start_vel;
00107   coefficients[2] = 0.5*start_acc;
00108   coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
00109       12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
00110   coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
00111       16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
00112   coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
00113       6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
00114 
00115 }
00116 
00120 inline void sampleQuinticSpline(const std::vector<double>& coefficients, double time,
00121     double& position, double& velocity, double& acceleration)
00122 {
00123   // create powers of time:
00124   double t[6];
00125   generatePowers(5, time, t);
00126 
00127   position = t[0]*coefficients[0] +
00128       t[1]*coefficients[1] +
00129       t[2]*coefficients[2] +
00130       t[3]*coefficients[3] +
00131       t[4]*coefficients[4] +
00132       t[5]*coefficients[5];
00133 
00134   velocity = t[0]*coefficients[1] +
00135       2.0*t[1]*coefficients[2] +
00136       3.0*t[2]*coefficients[3] +
00137       4.0*t[3]*coefficients[4] +
00138       5.0*t[4]*coefficients[5];
00139 
00140   acceleration = 2.0*t[0]*coefficients[2] +
00141       6.0*t[1]*coefficients[3] +
00142       12.0*t[2]*coefficients[4] +
00143       20.0*t[3]*coefficients[5];
00144 }
00145 
00146 inline void getCubicSplineCoefficients(double start_pos, double start_vel,
00147     double end_pos, double end_vel, double time, std::vector<double>& coefficients)
00148 {
00149   coefficients.resize(4);
00150 
00151   double T[4];
00152   generatePowers(3, time, T);
00153 
00154   coefficients[0] = start_pos;
00155   coefficients[1] = start_vel;
00156   coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
00157   coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
00158 }
00159 
00160 inline void sampleCubicSpline(const std::vector<double>& coefficients, double time,
00161     double& position, double& velocity, double& acceleration)
00162 {
00163   double t[4];
00164   generatePowers(3, time, t);
00165 
00166   position = t[0]*coefficients[0] +
00167       t[1]*coefficients[1] +
00168       t[2]*coefficients[2] +
00169       t[3]*coefficients[3];
00170 
00171   velocity = t[0]*coefficients[1] +
00172       2.0*t[1]*coefficients[2] +
00173       3.0*t[2]*coefficients[3];
00174 
00175   acceleration = 2.0*t[0]*coefficients[2] +
00176       6.0*t[1]*coefficients[3];
00177 }
00178 
00179 } // namespace spline_smoother
00180 
00181 #endif /* SPLINES_H_ */