nuclear@0: #include <cmath>
nuclear@0: #include "curves.h"
nuclear@0: 
nuclear@0: Curve::Curve() {
nuclear@0: 	ArcParametrize = false;
nuclear@0: 	ease_curve = 0;
nuclear@0: 	Samples = 0;
nuclear@0: 
nuclear@0: 	SetEaseSampleCount(100);
nuclear@0: }
nuclear@0: 
nuclear@0: Curve::~Curve() {
nuclear@0: 	delete [] Samples;
nuclear@0: 	
nuclear@0: }
nuclear@0: 
nuclear@0: void Curve::SetArcParametrization(bool state) {
nuclear@0: 	ArcParametrize = state;
nuclear@0: }
nuclear@0: 
nuclear@0: #define Param	0
nuclear@0: #define ArcLen	1
nuclear@0: 
nuclear@0: void Curve::SampleArcLengths() {
nuclear@0: 	const int SamplesPerSegment = 30;
nuclear@0: 	SampleCount = GetSegmentCount() * SamplesPerSegment;
nuclear@0: 
nuclear@0: 	ArcParametrize = false;	// to be able to interpolate with the original values
nuclear@0: 
nuclear@0: 	Samples = new Vector2[SampleCount];
nuclear@0: 	Vector3 prevpos;
nuclear@0: 	float step = 1.0f / (float)(SampleCount-1);
nuclear@0: 	for(int i=0; i<SampleCount; i++) {
nuclear@0: 		float t = step * (float)i;
nuclear@0: 		Vector3 pos = Interpolate(t);
nuclear@0: 		Samples[i][Param] = t;
nuclear@0: 		if(!i) {
nuclear@0: 			Samples[i][ArcLen] = 0.0f;
nuclear@0: 		} else {
nuclear@0: 			Samples[i][ArcLen] = (pos - prevpos).Length() + Samples[i-1][ArcLen];
nuclear@0: 		}
nuclear@0: 		prevpos = pos;
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	// normalize arc lenghts
nuclear@0: 	float maxlen = Samples[SampleCount-1][ArcLen];
nuclear@0: 	for(int i=0; i<SampleCount; i++) {
nuclear@0: 		Samples[i][ArcLen] /= maxlen;
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	ArcParametrize = true;
nuclear@0: }
nuclear@0: 
nuclear@0: int BinarySearch(Vector2 *array, float key, int begin, int end) {
nuclear@0: 	int middle = begin + ((end - begin)>>1);
nuclear@0: 
nuclear@0: 	if(array[middle][ArcLen] == key) return middle;
nuclear@0: 	if(end == begin) return middle;
nuclear@0: 
nuclear@0: 	if(key < array[middle][ArcLen]) return BinarySearch(array, key, begin, middle);
nuclear@0: 	if(key > array[middle][ArcLen]) return BinarySearch(array, key, middle+1, end);
nuclear@0: 	return -1;	// just to make the compiler shut the fuck up
nuclear@0: }
nuclear@0: 
nuclear@0: float Curve::Parametrize(float t) {
nuclear@0: 	if(!Samples) SampleArcLengths();
nuclear@0: 
nuclear@0: 	int samplepos = BinarySearch(Samples, t, 0, SampleCount);
nuclear@0: 	float par = Samples[samplepos][Param];
nuclear@0: 	float len = Samples[samplepos][ArcLen];
nuclear@0: 	if((len - t) < XSmallNumber) return par;
nuclear@0: 
nuclear@0: 	if(len < t) {
nuclear@0: 		if(!samplepos) return par;
nuclear@0: 		float prevlen = Samples[samplepos-1][ArcLen];
nuclear@0: 		float prevpar = Samples[samplepos-1][Param];
nuclear@0: 		float p = (t - prevlen) / (len - prevlen);
nuclear@0: 		return prevpar + (par - prevpar) * p;
nuclear@0: 	} else {
nuclear@0: 		if(samplepos >= SampleCount) return par;
nuclear@0: 		float nextlen = Samples[samplepos+1][ArcLen];
nuclear@0: 		float nextpar = Samples[samplepos+1][Param];
nuclear@0: 		float p = (t - len) / (nextlen - len);
nuclear@0: 		return par + (nextpar - par) * p;
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	return par;	// not gonna happen
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: #define MIN(a, b) ((a) < (b) ? (a) : (b))
nuclear@0: #define MAX(a, b) ((a) > (b) ? (a) : (b))
nuclear@0: 
nuclear@0: float Curve::Ease(float t) {
nuclear@0: 	if(!ease_curve) return t;
nuclear@0: 
nuclear@0: 	ease_curve->SetArcParametrization(true);
nuclear@0: 	float et = ease_curve->Interpolate(t).y;
nuclear@0: 
nuclear@0: 	return MIN(MAX(et, 0.0f), 1.0f);
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: void Curve::AddControlPoint(const Vector3 &cp) {
nuclear@0: 	ControlPoints.PushBack(cp);
nuclear@0: 	delete [] Samples;
nuclear@0: 	Samples = 0;
nuclear@0: }
nuclear@0: 
nuclear@0: void Curve::SetEaseCurve(Curve *curve) {
nuclear@0: 	ease_curve = curve;
nuclear@0: }
nuclear@0: 
nuclear@0: void Curve::SetEaseSampleCount(int count) {
nuclear@0: 	ease_sample_count = count;
nuclear@0: 	ease_step = 1.0f / ease_sample_count;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: ///////////////// B-Spline implementation ////////////////////
nuclear@0: 
nuclear@0: int BSpline::GetSegmentCount() const {
nuclear@0: 	return ControlPoints.Size() - 3;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 BSpline::Interpolate(float t) {
nuclear@0: 
nuclear@0: 	if(ControlPoints.Size() < 4) return Vector3(0, 0, 0);
nuclear@0: 
nuclear@0: 	if(ArcParametrize) {
nuclear@0: 		t = Ease(Parametrize(t));
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	// find the appropriate segment of the spline that t lies and calculate the piecewise parameter
nuclear@0: 	t = (float)(ControlPoints.Size() - 3) * t;
nuclear@0: 	int seg = (int)t;
nuclear@0: 	t -= (float)floor(t);
nuclear@0: 	if(seg >= GetSegmentCount()) {
nuclear@0: 		seg = GetSegmentCount() - 1;
nuclear@0: 		t = 1.0f;
nuclear@0: 	}
nuclear@0: 	
nuclear@0: 	ListNode<Vector3> *iter = ControlPoints.Begin();
nuclear@0: 	for(int i=0; i<seg; i++) iter = iter->next;
nuclear@0: 
nuclear@0: 	Vector3 Cp[4];
nuclear@0: 	for(int i=0; i<4; i++) {
nuclear@0:         Cp[i] = iter->data;
nuclear@0: 		iter = iter->next;
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	Matrix4x4 BSplineMat(-1, 3, -3, 1, 3, -6, 3, 0, -3, 0, 3, 0, 1, 4, 1, 0);
nuclear@0: 	BSplineMat.Transpose();
nuclear@0: 	Vector4 Params(t*t*t, t*t, t, 1);
nuclear@0: 	Vector4 CpX(Cp[0].x, Cp[1].x, Cp[2].x, Cp[3].x);
nuclear@0: 	Vector4 CpY(Cp[0].y, Cp[1].y, Cp[2].y, Cp[3].y);
nuclear@0: 	Vector4 CpZ(Cp[0].z, Cp[1].z, Cp[2].z, Cp[3].z);
nuclear@0: 
nuclear@0: 	CpX.Transform(BSplineMat);
nuclear@0: 	CpY.Transform(BSplineMat);
nuclear@0: 	CpZ.Transform(BSplineMat);
nuclear@0: 
nuclear@0: 	CpX /= 6.0f;
nuclear@0: 	CpY /= 6.0f;
nuclear@0: 	CpZ /= 6.0f;
nuclear@0: 
nuclear@0: 	Vector3 res;
nuclear@0: 
nuclear@0: 	res.x = Params.DotProduct(CpX);
nuclear@0: 	res.y = Params.DotProduct(CpY);
nuclear@0: 	res.z = Params.DotProduct(CpZ);
nuclear@0: 
nuclear@0: 	return res;
nuclear@0: }
nuclear@0: 
nuclear@0: //////////////// Catmull-Rom Spline implementation //////////////////
nuclear@0: 
nuclear@0: int CatmullRomSpline::GetSegmentCount() const {
nuclear@0: 	return ControlPoints.Size() - 1;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 CatmullRomSpline::Interpolate(float t) {
nuclear@0: 
nuclear@0: 	if(ControlPoints.Size() < 2) return Vector3(0, 0, 0);
nuclear@0: 
nuclear@0: 	if(ArcParametrize) {
nuclear@0: 		t = Ease(Parametrize(t));
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	// find the appropriate segment of the spline that t lies and calculate the piecewise parameter
nuclear@0: 	t = (float)(ControlPoints.Size() - 1) * t;
nuclear@0: 	int seg = (int)t;
nuclear@0: 	t -= (float)floor(t);
nuclear@0: 	if(seg >= GetSegmentCount()) {
nuclear@0: 		seg = GetSegmentCount() - 1;
nuclear@0: 		t = 1.0f;
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	Vector3 Cp[4];
nuclear@0: 	ListNode<Vector3> *iter = ControlPoints.Begin();
nuclear@0: 	for(int i=0; i<seg; i++) iter = iter->next;
nuclear@0: 
nuclear@0: 	Cp[1] = iter->data;
nuclear@0: 	Cp[2] = iter->next->data;
nuclear@0: 	
nuclear@0: 	if(!seg) {
nuclear@0: 		Cp[0] = Cp[1];
nuclear@0: 	} else {
nuclear@0: 		Cp[0] = iter->prev->data;
nuclear@0: 	}
nuclear@0: 	
nuclear@0: 	if(seg == ControlPoints.Size() - 2) {
nuclear@0: 		Cp[3] = Cp[2];
nuclear@0: 	} else {
nuclear@0: 		Cp[3] = iter->next->next->data;
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	Matrix4x4 BSplineMat(-1, 3, -3, 1, 2, -5, 4, -1, -1, 0, 1, 0, 0, 2, 0, 0);
nuclear@0: 	BSplineMat.Transpose();
nuclear@0: 	Vector4 Params(t*t*t, t*t, t, 1);
nuclear@0: 	Vector4 CpX(Cp[0].x, Cp[1].x, Cp[2].x, Cp[3].x);
nuclear@0: 	Vector4 CpY(Cp[0].y, Cp[1].y, Cp[2].y, Cp[3].y);
nuclear@0: 	Vector4 CpZ(Cp[0].z, Cp[1].z, Cp[2].z, Cp[3].z);
nuclear@0: 
nuclear@0: 	CpX.Transform(BSplineMat);
nuclear@0: 	CpY.Transform(BSplineMat);
nuclear@0: 	CpZ.Transform(BSplineMat);
nuclear@0: 
nuclear@0: 	CpX /= 2.0f;
nuclear@0: 	CpY /= 2.0f;
nuclear@0: 	CpZ /= 2.0f;
nuclear@0: 
nuclear@0: 	Vector3 res;
nuclear@0: 
nuclear@0: 	res.x = Params.DotProduct(CpX);
nuclear@0: 	res.y = Params.DotProduct(CpY);
nuclear@0: 	res.z = Params.DotProduct(CpZ);
nuclear@0: 
nuclear@0: 	return res;
nuclear@0: }