1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/src/common/curves.cpp	Thu Oct 23 01:46:07 2014 +0300
     1.3 @@ -0,0 +1,240 @@
     1.4 +#include <cmath>
     1.5 +#include "curves.h"
     1.6 +
     1.7 +Curve::Curve() {
     1.8 +	ArcParametrize = false;
     1.9 +	ease_curve = 0;
    1.10 +	Samples = 0;
    1.11 +
    1.12 +	SetEaseSampleCount(100);
    1.13 +}
    1.14 +
    1.15 +Curve::~Curve() {
    1.16 +	delete [] Samples;
    1.17 +	
    1.18 +}
    1.19 +
    1.20 +void Curve::SetArcParametrization(bool state) {
    1.21 +	ArcParametrize = state;
    1.22 +}
    1.23 +
    1.24 +#define Param	0
    1.25 +#define ArcLen	1
    1.26 +
    1.27 +void Curve::SampleArcLengths() {
    1.28 +	const int SamplesPerSegment = 30;
    1.29 +	SampleCount = GetSegmentCount() * SamplesPerSegment;
    1.30 +
    1.31 +	ArcParametrize = false;	// to be able to interpolate with the original values
    1.32 +
    1.33 +	Samples = new Vector2[SampleCount];
    1.34 +	Vector3 prevpos;
    1.35 +	float step = 1.0f / (float)(SampleCount-1);
    1.36 +	for(int i=0; i<SampleCount; i++) {
    1.37 +		float t = step * (float)i;
    1.38 +		Vector3 pos = Interpolate(t);
    1.39 +		Samples[i][Param] = t;
    1.40 +		if(!i) {
    1.41 +			Samples[i][ArcLen] = 0.0f;
    1.42 +		} else {
    1.43 +			Samples[i][ArcLen] = (pos - prevpos).Length() + Samples[i-1][ArcLen];
    1.44 +		}
    1.45 +		prevpos = pos;
    1.46 +	}
    1.47 +
    1.48 +	// normalize arc lenghts
    1.49 +	float maxlen = Samples[SampleCount-1][ArcLen];
    1.50 +	for(int i=0; i<SampleCount; i++) {
    1.51 +		Samples[i][ArcLen] /= maxlen;
    1.52 +	}
    1.53 +
    1.54 +	ArcParametrize = true;
    1.55 +}
    1.56 +
    1.57 +int BinarySearch(Vector2 *array, float key, int begin, int end) {
    1.58 +	int middle = begin + ((end - begin)>>1);
    1.59 +
    1.60 +	if(array[middle][ArcLen] == key) return middle;
    1.61 +	if(end == begin) return middle;
    1.62 +
    1.63 +	if(key < array[middle][ArcLen]) return BinarySearch(array, key, begin, middle);
    1.64 +	if(key > array[middle][ArcLen]) return BinarySearch(array, key, middle+1, end);
    1.65 +	return -1;	// just to make the compiler shut the fuck up
    1.66 +}
    1.67 +
    1.68 +float Curve::Parametrize(float t) {
    1.69 +	if(!Samples) SampleArcLengths();
    1.70 +
    1.71 +	int samplepos = BinarySearch(Samples, t, 0, SampleCount);
    1.72 +	float par = Samples[samplepos][Param];
    1.73 +	float len = Samples[samplepos][ArcLen];
    1.74 +	if((len - t) < XSmallNumber) return par;
    1.75 +
    1.76 +	if(len < t) {
    1.77 +		if(!samplepos) return par;
    1.78 +		float prevlen = Samples[samplepos-1][ArcLen];
    1.79 +		float prevpar = Samples[samplepos-1][Param];
    1.80 +		float p = (t - prevlen) / (len - prevlen);
    1.81 +		return prevpar + (par - prevpar) * p;
    1.82 +	} else {
    1.83 +		if(samplepos >= SampleCount) return par;
    1.84 +		float nextlen = Samples[samplepos+1][ArcLen];
    1.85 +		float nextpar = Samples[samplepos+1][Param];
    1.86 +		float p = (t - len) / (nextlen - len);
    1.87 +		return par + (nextpar - par) * p;
    1.88 +	}
    1.89 +
    1.90 +	return par;	// not gonna happen
    1.91 +}
    1.92 +
    1.93 +
    1.94 +#define MIN(a, b) ((a) < (b) ? (a) : (b))
    1.95 +#define MAX(a, b) ((a) > (b) ? (a) : (b))
    1.96 +
    1.97 +float Curve::Ease(float t) {
    1.98 +	if(!ease_curve) return t;
    1.99 +
   1.100 +	ease_curve->SetArcParametrization(true);
   1.101 +	float et = ease_curve->Interpolate(t).y;
   1.102 +
   1.103 +	return MIN(MAX(et, 0.0f), 1.0f);
   1.104 +}
   1.105 +
   1.106 +
   1.107 +void Curve::AddControlPoint(const Vector3 &cp) {
   1.108 +	ControlPoints.PushBack(cp);
   1.109 +	delete [] Samples;
   1.110 +	Samples = 0;
   1.111 +}
   1.112 +
   1.113 +void Curve::SetEaseCurve(Curve *curve) {
   1.114 +	ease_curve = curve;
   1.115 +}
   1.116 +
   1.117 +void Curve::SetEaseSampleCount(int count) {
   1.118 +	ease_sample_count = count;
   1.119 +	ease_step = 1.0f / ease_sample_count;
   1.120 +}
   1.121 +
   1.122 +
   1.123 +///////////////// B-Spline implementation ////////////////////
   1.124 +
   1.125 +int BSpline::GetSegmentCount() const {
   1.126 +	return ControlPoints.Size() - 3;
   1.127 +}
   1.128 +
   1.129 +Vector3 BSpline::Interpolate(float t) {
   1.130 +
   1.131 +	if(ControlPoints.Size() < 4) return Vector3(0, 0, 0);
   1.132 +
   1.133 +	if(ArcParametrize) {
   1.134 +		t = Ease(Parametrize(t));
   1.135 +	}
   1.136 +
   1.137 +	// find the appropriate segment of the spline that t lies and calculate the piecewise parameter
   1.138 +	t = (float)(ControlPoints.Size() - 3) * t;
   1.139 +	int seg = (int)t;
   1.140 +	t -= (float)floor(t);
   1.141 +	if(seg >= GetSegmentCount()) {
   1.142 +		seg = GetSegmentCount() - 1;
   1.143 +		t = 1.0f;
   1.144 +	}
   1.145 +	
   1.146 +	ListNode<Vector3> *iter = ControlPoints.Begin();
   1.147 +	for(int i=0; i<seg; i++) iter = iter->next;
   1.148 +
   1.149 +	Vector3 Cp[4];
   1.150 +	for(int i=0; i<4; i++) {
   1.151 +        Cp[i] = iter->data;
   1.152 +		iter = iter->next;
   1.153 +	}
   1.154 +
   1.155 +	Matrix4x4 BSplineMat(-1, 3, -3, 1, 3, -6, 3, 0, -3, 0, 3, 0, 1, 4, 1, 0);
   1.156 +	BSplineMat.Transpose();
   1.157 +	Vector4 Params(t*t*t, t*t, t, 1);
   1.158 +	Vector4 CpX(Cp[0].x, Cp[1].x, Cp[2].x, Cp[3].x);
   1.159 +	Vector4 CpY(Cp[0].y, Cp[1].y, Cp[2].y, Cp[3].y);
   1.160 +	Vector4 CpZ(Cp[0].z, Cp[1].z, Cp[2].z, Cp[3].z);
   1.161 +
   1.162 +	CpX.Transform(BSplineMat);
   1.163 +	CpY.Transform(BSplineMat);
   1.164 +	CpZ.Transform(BSplineMat);
   1.165 +
   1.166 +	CpX /= 6.0f;
   1.167 +	CpY /= 6.0f;
   1.168 +	CpZ /= 6.0f;
   1.169 +
   1.170 +	Vector3 res;
   1.171 +
   1.172 +	res.x = Params.DotProduct(CpX);
   1.173 +	res.y = Params.DotProduct(CpY);
   1.174 +	res.z = Params.DotProduct(CpZ);
   1.175 +
   1.176 +	return res;
   1.177 +}
   1.178 +
   1.179 +//////////////// Catmull-Rom Spline implementation //////////////////
   1.180 +
   1.181 +int CatmullRomSpline::GetSegmentCount() const {
   1.182 +	return ControlPoints.Size() - 1;
   1.183 +}
   1.184 +
   1.185 +Vector3 CatmullRomSpline::Interpolate(float t) {
   1.186 +
   1.187 +	if(ControlPoints.Size() < 2) return Vector3(0, 0, 0);
   1.188 +
   1.189 +	if(ArcParametrize) {
   1.190 +		t = Ease(Parametrize(t));
   1.191 +	}
   1.192 +
   1.193 +	// find the appropriate segment of the spline that t lies and calculate the piecewise parameter
   1.194 +	t = (float)(ControlPoints.Size() - 1) * t;
   1.195 +	int seg = (int)t;
   1.196 +	t -= (float)floor(t);
   1.197 +	if(seg >= GetSegmentCount()) {
   1.198 +		seg = GetSegmentCount() - 1;
   1.199 +		t = 1.0f;
   1.200 +	}
   1.201 +
   1.202 +	Vector3 Cp[4];
   1.203 +	ListNode<Vector3> *iter = ControlPoints.Begin();
   1.204 +	for(int i=0; i<seg; i++) iter = iter->next;
   1.205 +
   1.206 +	Cp[1] = iter->data;
   1.207 +	Cp[2] = iter->next->data;
   1.208 +	
   1.209 +	if(!seg) {
   1.210 +		Cp[0] = Cp[1];
   1.211 +	} else {
   1.212 +		Cp[0] = iter->prev->data;
   1.213 +	}
   1.214 +	
   1.215 +	if(seg == ControlPoints.Size() - 2) {
   1.216 +		Cp[3] = Cp[2];
   1.217 +	} else {
   1.218 +		Cp[3] = iter->next->next->data;
   1.219 +	}
   1.220 +
   1.221 +	Matrix4x4 BSplineMat(-1, 3, -3, 1, 2, -5, 4, -1, -1, 0, 1, 0, 0, 2, 0, 0);
   1.222 +	BSplineMat.Transpose();
   1.223 +	Vector4 Params(t*t*t, t*t, t, 1);
   1.224 +	Vector4 CpX(Cp[0].x, Cp[1].x, Cp[2].x, Cp[3].x);
   1.225 +	Vector4 CpY(Cp[0].y, Cp[1].y, Cp[2].y, Cp[3].y);
   1.226 +	Vector4 CpZ(Cp[0].z, Cp[1].z, Cp[2].z, Cp[3].z);
   1.227 +
   1.228 +	CpX.Transform(BSplineMat);
   1.229 +	CpY.Transform(BSplineMat);
   1.230 +	CpZ.Transform(BSplineMat);
   1.231 +
   1.232 +	CpX /= 2.0f;
   1.233 +	CpY /= 2.0f;
   1.234 +	CpZ /= 2.0f;
   1.235 +
   1.236 +	Vector3 res;
   1.237 +
   1.238 +	res.x = Params.DotProduct(CpX);
   1.239 +	res.y = Params.DotProduct(CpY);
   1.240 +	res.z = Params.DotProduct(CpZ);
   1.241 +
   1.242 +	return res;
   1.243 +}
   1.244 \ No newline at end of file