void CubicSpline::addPoint(double time, double position,
double velocity)
{
_points.push_back({time,
position, velocity});
computeSplines();
}
void CubicSpline::subdivide(unsigned int divider)
{
std::vector<Point> newPoints;
for (size_t i=1;i<_points.size();i++) {
double t1 = _points[i-1].time;
double t2 = _points[i].time;
double length = fabs(t2 - t1);
if (length < 0.0001) {
continue;
}
double step = length/(divider + 1);
for (double t=t1;t<t2-step/2.0;t+=step) {
newPoints.push_back({
t, Spline::pos(t), Spline::vel(t)});
}
}
newPoints.push_back(_points.back());
//Replace points container
_points = newPoints;
//Recompute the splines
computeSplines();
}
void CubicSpline::randomNoise(
double stdDevPos, double stdDevVel, bool updateBounds)
{
//Initialize the generator
std::mt19937 generator(static_cast<long unsigned int>(
std::chrono::high_resolution_clock::now()
.time_since_epoch().count()));
std::normal_distribution<double> distPos(0.0, stdDevPos);
std::normal_distribution<double> distVel(0.0, stdDevVel);
//Apply noise on points
for (size_t i=0;i<_points.size();i++) {
if (!updateBounds && (i == 0 || i == _points.size()-1)) {
continue;
}
double deltaPos = distPos(generator);
double deltaVel = distVel(generator);
_points[i].position += deltaPos;
_points[i].velocity += deltaVel;
}
//Recompute the splines
computeSplines();
}
