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);
 }