Vector3 CatmullRomSpline::interpolate(scalar_t t) const {
if(t > 1.0) t = 1.0;
if(t < 0.0) t = 0.0;
if(control_points.size() < 2) return Vector3(0, 0, 0);
if(arc_parametrize) {
t = ease(parametrize(t));
}
// find the appropriate segment of the spline that t lies and calculate the piecewise parameter
t = (scalar_t)(control_points.size() - 1) * t;
int seg = (int)t;
t -= (scalar_t)floor(t);
if(seg >= get_segment_count()) {
seg = get_segment_count() - 1;
t = 1.0f;
}
Vector3 cp[4];
ListNode<Vector3> *iter = const_cast<CatmullRomSpline*>(this)->control_points.begin();
for(int i=0; i<seg; i++) {
iter = iter->next;
}
cp[1] = iter->data;
cp[2] = iter->next->data;
if(!seg) {
cp[0] = cp[1];
} else {
cp[0] = iter->prev->data;
}
if(seg == control_points.size() - 2) {
cp[3] = cp[2];
} else {
cp[3] = iter->next->next->data;
}
Vector3 res;
res.x = catmull_rom_spline(cp[0].x, cp[1].x, cp[2].x, cp[3].x, t);
res.y = catmull_rom_spline(cp[0].y, cp[1].y, cp[2].y, cp[3].y, t);
res.z = catmull_rom_spline(cp[0].z, cp[1].z, cp[2].z, cp[3].z, t);
return res;
}
float interpolate(const float pos){
return catmull_rom_spline(lastInputSamples[3], lastInputSamples[2], lastInputSamples[1], lastInputSamples[0], pos);
}