00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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 }
00180
00181 #endif