absence_thelab

annotate src/common/curves.cpp @ 0:1cffe3409164

find changesets by author, revision, files, or words in the commit message
initial commit
author John Tsiombikas <nuclear@member.fsf.org>
date Thu, 23 Oct 2014 01:46:07 +0300
parents
children
rev   line source
nuclear@0 1 #include <cmath>
nuclear@0 2 #include "curves.h"
nuclear@0 3
nuclear@0 4 Curve::Curve() {
nuclear@0 5 ArcParametrize = false;
nuclear@0 6 ease_curve = 0;
nuclear@0 7 Samples = 0;
nuclear@0 8
nuclear@0 9 SetEaseSampleCount(100);
nuclear@0 10 }
nuclear@0 11
nuclear@0 12 Curve::~Curve() {
nuclear@0 13 delete [] Samples;
nuclear@0 14
nuclear@0 15 }
nuclear@0 16
nuclear@0 17 void Curve::SetArcParametrization(bool state) {
nuclear@0 18 ArcParametrize = state;
nuclear@0 19 }
nuclear@0 20
nuclear@0 21 #define Param 0
nuclear@0 22 #define ArcLen 1
nuclear@0 23
nuclear@0 24 void Curve::SampleArcLengths() {
nuclear@0 25 const int SamplesPerSegment = 30;
nuclear@0 26 SampleCount = GetSegmentCount() * SamplesPerSegment;
nuclear@0 27
nuclear@0 28 ArcParametrize = false; // to be able to interpolate with the original values
nuclear@0 29
nuclear@0 30 Samples = new Vector2[SampleCount];
nuclear@0 31 Vector3 prevpos;
nuclear@0 32 float step = 1.0f / (float)(SampleCount-1);
nuclear@0 33 for(int i=0; i<SampleCount; i++) {
nuclear@0 34 float t = step * (float)i;
nuclear@0 35 Vector3 pos = Interpolate(t);
nuclear@0 36 Samples[i][Param] = t;
nuclear@0 37 if(!i) {
nuclear@0 38 Samples[i][ArcLen] = 0.0f;
nuclear@0 39 } else {
nuclear@0 40 Samples[i][ArcLen] = (pos - prevpos).Length() + Samples[i-1][ArcLen];
nuclear@0 41 }
nuclear@0 42 prevpos = pos;
nuclear@0 43 }
nuclear@0 44
nuclear@0 45 // normalize arc lenghts
nuclear@0 46 float maxlen = Samples[SampleCount-1][ArcLen];
nuclear@0 47 for(int i=0; i<SampleCount; i++) {
nuclear@0 48 Samples[i][ArcLen] /= maxlen;
nuclear@0 49 }
nuclear@0 50
nuclear@0 51 ArcParametrize = true;
nuclear@0 52 }
nuclear@0 53
nuclear@0 54 int BinarySearch(Vector2 *array, float key, int begin, int end) {
nuclear@0 55 int middle = begin + ((end - begin)>>1);
nuclear@0 56
nuclear@0 57 if(array[middle][ArcLen] == key) return middle;
nuclear@0 58 if(end == begin) return middle;
nuclear@0 59
nuclear@0 60 if(key < array[middle][ArcLen]) return BinarySearch(array, key, begin, middle);
nuclear@0 61 if(key > array[middle][ArcLen]) return BinarySearch(array, key, middle+1, end);
nuclear@0 62 return -1; // just to make the compiler shut the fuck up
nuclear@0 63 }
nuclear@0 64
nuclear@0 65 float Curve::Parametrize(float t) {
nuclear@0 66 if(!Samples) SampleArcLengths();
nuclear@0 67
nuclear@0 68 int samplepos = BinarySearch(Samples, t, 0, SampleCount);
nuclear@0 69 float par = Samples[samplepos][Param];
nuclear@0 70 float len = Samples[samplepos][ArcLen];
nuclear@0 71 if((len - t) < XSmallNumber) return par;
nuclear@0 72
nuclear@0 73 if(len < t) {
nuclear@0 74 if(!samplepos) return par;
nuclear@0 75 float prevlen = Samples[samplepos-1][ArcLen];
nuclear@0 76 float prevpar = Samples[samplepos-1][Param];
nuclear@0 77 float p = (t - prevlen) / (len - prevlen);
nuclear@0 78 return prevpar + (par - prevpar) * p;
nuclear@0 79 } else {
nuclear@0 80 if(samplepos >= SampleCount) return par;
nuclear@0 81 float nextlen = Samples[samplepos+1][ArcLen];
nuclear@0 82 float nextpar = Samples[samplepos+1][Param];
nuclear@0 83 float p = (t - len) / (nextlen - len);
nuclear@0 84 return par + (nextpar - par) * p;
nuclear@0 85 }
nuclear@0 86
nuclear@0 87 return par; // not gonna happen
nuclear@0 88 }
nuclear@0 89
nuclear@0 90
nuclear@0 91 #define MIN(a, b) ((a) < (b) ? (a) : (b))
nuclear@0 92 #define MAX(a, b) ((a) > (b) ? (a) : (b))
nuclear@0 93
nuclear@0 94 float Curve::Ease(float t) {
nuclear@0 95 if(!ease_curve) return t;
nuclear@0 96
nuclear@0 97 ease_curve->SetArcParametrization(true);
nuclear@0 98 float et = ease_curve->Interpolate(t).y;
nuclear@0 99
nuclear@0 100 return MIN(MAX(et, 0.0f), 1.0f);
nuclear@0 101 }
nuclear@0 102
nuclear@0 103
nuclear@0 104 void Curve::AddControlPoint(const Vector3 &cp) {
nuclear@0 105 ControlPoints.PushBack(cp);
nuclear@0 106 delete [] Samples;
nuclear@0 107 Samples = 0;
nuclear@0 108 }
nuclear@0 109
nuclear@0 110 void Curve::SetEaseCurve(Curve *curve) {
nuclear@0 111 ease_curve = curve;
nuclear@0 112 }
nuclear@0 113
nuclear@0 114 void Curve::SetEaseSampleCount(int count) {
nuclear@0 115 ease_sample_count = count;
nuclear@0 116 ease_step = 1.0f / ease_sample_count;
nuclear@0 117 }
nuclear@0 118
nuclear@0 119
nuclear@0 120 ///////////////// B-Spline implementation ////////////////////
nuclear@0 121
nuclear@0 122 int BSpline::GetSegmentCount() const {
nuclear@0 123 return ControlPoints.Size() - 3;
nuclear@0 124 }
nuclear@0 125
nuclear@0 126 Vector3 BSpline::Interpolate(float t) {
nuclear@0 127
nuclear@0 128 if(ControlPoints.Size() < 4) return Vector3(0, 0, 0);
nuclear@0 129
nuclear@0 130 if(ArcParametrize) {
nuclear@0 131 t = Ease(Parametrize(t));
nuclear@0 132 }
nuclear@0 133
nuclear@0 134 // find the appropriate segment of the spline that t lies and calculate the piecewise parameter
nuclear@0 135 t = (float)(ControlPoints.Size() - 3) * t;
nuclear@0 136 int seg = (int)t;
nuclear@0 137 t -= (float)floor(t);
nuclear@0 138 if(seg >= GetSegmentCount()) {
nuclear@0 139 seg = GetSegmentCount() - 1;
nuclear@0 140 t = 1.0f;
nuclear@0 141 }
nuclear@0 142
nuclear@0 143 ListNode<Vector3> *iter = ControlPoints.Begin();
nuclear@0 144 for(int i=0; i<seg; i++) iter = iter->next;
nuclear@0 145
nuclear@0 146 Vector3 Cp[4];
nuclear@0 147 for(int i=0; i<4; i++) {
nuclear@0 148 Cp[i] = iter->data;
nuclear@0 149 iter = iter->next;
nuclear@0 150 }
nuclear@0 151
nuclear@0 152 Matrix4x4 BSplineMat(-1, 3, -3, 1, 3, -6, 3, 0, -3, 0, 3, 0, 1, 4, 1, 0);
nuclear@0 153 BSplineMat.Transpose();
nuclear@0 154 Vector4 Params(t*t*t, t*t, t, 1);
nuclear@0 155 Vector4 CpX(Cp[0].x, Cp[1].x, Cp[2].x, Cp[3].x);
nuclear@0 156 Vector4 CpY(Cp[0].y, Cp[1].y, Cp[2].y, Cp[3].y);
nuclear@0 157 Vector4 CpZ(Cp[0].z, Cp[1].z, Cp[2].z, Cp[3].z);
nuclear@0 158
nuclear@0 159 CpX.Transform(BSplineMat);
nuclear@0 160 CpY.Transform(BSplineMat);
nuclear@0 161 CpZ.Transform(BSplineMat);
nuclear@0 162
nuclear@0 163 CpX /= 6.0f;
nuclear@0 164 CpY /= 6.0f;
nuclear@0 165 CpZ /= 6.0f;
nuclear@0 166
nuclear@0 167 Vector3 res;
nuclear@0 168
nuclear@0 169 res.x = Params.DotProduct(CpX);
nuclear@0 170 res.y = Params.DotProduct(CpY);
nuclear@0 171 res.z = Params.DotProduct(CpZ);
nuclear@0 172
nuclear@0 173 return res;
nuclear@0 174 }
nuclear@0 175
nuclear@0 176 //////////////// Catmull-Rom Spline implementation //////////////////
nuclear@0 177
nuclear@0 178 int CatmullRomSpline::GetSegmentCount() const {
nuclear@0 179 return ControlPoints.Size() - 1;
nuclear@0 180 }
nuclear@0 181
nuclear@0 182 Vector3 CatmullRomSpline::Interpolate(float t) {
nuclear@0 183
nuclear@0 184 if(ControlPoints.Size() < 2) return Vector3(0, 0, 0);
nuclear@0 185
nuclear@0 186 if(ArcParametrize) {
nuclear@0 187 t = Ease(Parametrize(t));
nuclear@0 188 }
nuclear@0 189
nuclear@0 190 // find the appropriate segment of the spline that t lies and calculate the piecewise parameter
nuclear@0 191 t = (float)(ControlPoints.Size() - 1) * t;
nuclear@0 192 int seg = (int)t;
nuclear@0 193 t -= (float)floor(t);
nuclear@0 194 if(seg >= GetSegmentCount()) {
nuclear@0 195 seg = GetSegmentCount() - 1;
nuclear@0 196 t = 1.0f;
nuclear@0 197 }
nuclear@0 198
nuclear@0 199 Vector3 Cp[4];
nuclear@0 200 ListNode<Vector3> *iter = ControlPoints.Begin();
nuclear@0 201 for(int i=0; i<seg; i++) iter = iter->next;
nuclear@0 202
nuclear@0 203 Cp[1] = iter->data;
nuclear@0 204 Cp[2] = iter->next->data;
nuclear@0 205
nuclear@0 206 if(!seg) {
nuclear@0 207 Cp[0] = Cp[1];
nuclear@0 208 } else {
nuclear@0 209 Cp[0] = iter->prev->data;
nuclear@0 210 }
nuclear@0 211
nuclear@0 212 if(seg == ControlPoints.Size() - 2) {
nuclear@0 213 Cp[3] = Cp[2];
nuclear@0 214 } else {
nuclear@0 215 Cp[3] = iter->next->next->data;
nuclear@0 216 }
nuclear@0 217
nuclear@0 218 Matrix4x4 BSplineMat(-1, 3, -3, 1, 2, -5, 4, -1, -1, 0, 1, 0, 0, 2, 0, 0);
nuclear@0 219 BSplineMat.Transpose();
nuclear@0 220 Vector4 Params(t*t*t, t*t, t, 1);
nuclear@0 221 Vector4 CpX(Cp[0].x, Cp[1].x, Cp[2].x, Cp[3].x);
nuclear@0 222 Vector4 CpY(Cp[0].y, Cp[1].y, Cp[2].y, Cp[3].y);
nuclear@0 223 Vector4 CpZ(Cp[0].z, Cp[1].z, Cp[2].z, Cp[3].z);
nuclear@0 224
nuclear@0 225 CpX.Transform(BSplineMat);
nuclear@0 226 CpY.Transform(BSplineMat);
nuclear@0 227 CpZ.Transform(BSplineMat);
nuclear@0 228
nuclear@0 229 CpX /= 2.0f;
nuclear@0 230 CpY /= 2.0f;
nuclear@0 231 CpZ /= 2.0f;
nuclear@0 232
nuclear@0 233 Vector3 res;
nuclear@0 234
nuclear@0 235 res.x = Params.DotProduct(CpX);
nuclear@0 236 res.y = Params.DotProduct(CpY);
nuclear@0 237 res.z = Params.DotProduct(CpZ);
nuclear@0 238
nuclear@0 239 return res;
nuclear@0 240 }