1.1 --- a/src/curve.cc	Sat Dec 19 22:39:18 2015 +0200
     1.2 +++ b/src/curve.cc	Sun Dec 20 07:21:32 2015 +0200
     1.3 @@ -1,10 +1,39 @@
     1.4  #include <float.h>
     1.5 +#include <assert.h>
     1.6  #include <algorithm>
     1.7  #include "curve.h"
     1.8  
     1.9  Curve::Curve(CurveType type)
    1.10  {
    1.11  	this->type = type;
    1.12 +	bbvalid = true;
    1.13 +}
    1.14 +
    1.15 +Curve::Curve(const Vector4 *cp, int numcp, CurveType type)
    1.16 +	: Curve(type)
    1.17 +{
    1.18 +	this->cp.resize(numcp);
    1.19 +	for(int i=0; i<numcp; i++) {
    1.20 +		this->cp[i] = cp[i];
    1.21 +	}
    1.22 +}
    1.23 +
    1.24 +Curve::Curve(const Vector3 *cp, int numcp, CurveType type)
    1.25 +	: Curve(type)
    1.26 +{
    1.27 +	this->cp.resize(numcp);
    1.28 +	for(int i=0; i<numcp; i++) {
    1.29 +		this->cp[i] = Vector4(cp[i].x, cp[i].y, cp[i].z, 1.0f);
    1.30 +	}
    1.31 +}
    1.32 +
    1.33 +Curve::Curve(const Vector2 *cp, int numcp, CurveType type)
    1.34 +	: Curve(type)
    1.35 +{
    1.36 +	this->cp.resize(numcp);
    1.37 +	for(int i=0; i<numcp; i++) {
    1.38 +		this->cp[i] = Vector4(cp[i].x, cp[i].y, 0.0f, 1.0f);
    1.39 +	}
    1.40  }
    1.41  
    1.42  void Curve::set_type(CurveType type)
    1.43 @@ -17,9 +46,20 @@
    1.44  	return type;
    1.45  }
    1.46  
    1.47 +void Curve::add_point(const Vector4 &p)
    1.48 +{
    1.49 +	cp.push_back(p);
    1.50 +	inval_bounds();
    1.51 +}
    1.52 +
    1.53 +void Curve::add_point(const Vector3 &p, float weight)
    1.54 +{
    1.55 +	add_point(Vector4(p.x, p.y, p.z, weight));
    1.56 +}
    1.57 +
    1.58  void Curve::add_point(const Vector2 &p, float weight)
    1.59  {
    1.60 -	cp.push_back(Vector3(p.x, p.y, weight));
    1.61 +	add_point(Vector4(p.x, p.y, 0.0f, weight));
    1.62  }
    1.63  
    1.64  bool Curve::remove_point(int idx)
    1.65 @@ -28,23 +68,14 @@
    1.66  		return false;
    1.67  	}
    1.68  	cp.erase(cp.begin() + idx);
    1.69 +	inval_bounds();
    1.70  	return true;
    1.71  }
    1.72  
    1.73 -int Curve::nearest_point(const Vector2 &p)
    1.74 +void Curve::clear()
    1.75  {
    1.76 -	int res = -1;
    1.77 -	float bestsq = FLT_MAX;
    1.78 -
    1.79 -	for(size_t i=0; i<cp.size(); i++) {
    1.80 -		float d = (get_point(i) - p).length_sq();
    1.81 -		if(d < bestsq) {
    1.82 -			bestsq = d;
    1.83 -			res = i;
    1.84 -		}
    1.85 -	}
    1.86 -
    1.87 -	return res;
    1.88 +	cp.clear();
    1.89 +	inval_bounds();
    1.90  }
    1.91  
    1.92  bool Curve::empty() const
    1.93 @@ -57,29 +88,45 @@
    1.94  	return (int)cp.size();
    1.95  }
    1.96  
    1.97 -Vector3 &Curve::operator [](int idx)
    1.98 +Vector4 &Curve::operator [](int idx)
    1.99 +{
   1.100 +	inval_bounds();
   1.101 +	return cp[idx];
   1.102 +}
   1.103 +
   1.104 +const Vector4 &Curve::operator [](int idx) const
   1.105  {
   1.106  	return cp[idx];
   1.107  }
   1.108  
   1.109 -const Vector3 &Curve::operator [](int idx) const
   1.110 +const Vector4 &Curve::get_point(int idx) const
   1.111  {
   1.112  	return cp[idx];
   1.113  }
   1.114  
   1.115 -const Vector3 &Curve::get_homo_point(int idx) const
   1.116 +Vector3 Curve::get_point3(int idx) const
   1.117  {
   1.118 -	return cp[idx];
   1.119 +	return Vector3(cp[idx].x, cp[idx].y, cp[idx].z);
   1.120  }
   1.121  
   1.122 -Vector2 Curve::get_point(int idx) const
   1.123 +Vector2 Curve::get_point2(int idx) const
   1.124  {
   1.125  	return Vector2(cp[idx].x, cp[idx].y);
   1.126  }
   1.127  
   1.128  float Curve::get_weight(int idx) const
   1.129  {
   1.130 -	return cp[idx].z;
   1.131 +	return cp[idx].w;
   1.132 +}
   1.133 +
   1.134 +bool Curve::set_point(int idx, const Vector3 &p, float weight)
   1.135 +{
   1.136 +	if(idx < 0 || idx >= (int)cp.size()) {
   1.137 +		return false;
   1.138 +	}
   1.139 +	cp[idx] = Vector4(p.x, p.y, p.z, weight);
   1.140 +	inval_bounds();
   1.141 +	return true;
   1.142  }
   1.143  
   1.144  bool Curve::set_point(int idx, const Vector2 &p, float weight)
   1.145 @@ -87,7 +134,8 @@
   1.146  	if(idx < 0 || idx >= (int)cp.size()) {
   1.147  		return false;
   1.148  	}
   1.149 -	cp[idx] = Vector3(p.x, p.y, weight);
   1.150 +	cp[idx] = Vector4(p.x, p.y, 0.0, weight);
   1.151 +	inval_bounds();
   1.152  	return true;
   1.153  }
   1.154  
   1.155 @@ -96,7 +144,17 @@
   1.156  	if(idx < 0 || idx >= (int)cp.size()) {
   1.157  		return false;
   1.158  	}
   1.159 -	cp[idx].z = weight;
   1.160 +	cp[idx].w = weight;
   1.161 +	return true;
   1.162 +}
   1.163 +
   1.164 +bool Curve::move_point(int idx, const Vector3 &p)
   1.165 +{
   1.166 +	if(idx < 0 || idx >= (int)cp.size()) {
   1.167 +		return false;
   1.168 +	}
   1.169 +	cp[idx] = Vector4(p.x, p.y, p.z, cp[idx].w);
   1.170 +	inval_bounds();
   1.171  	return true;
   1.172  }
   1.173  
   1.174 @@ -105,22 +163,207 @@
   1.175  	if(idx < 0 || idx >= (int)cp.size()) {
   1.176  		return false;
   1.177  	}
   1.178 -	cp[idx] = Vector3(p.x, p.y, cp[idx].z);
   1.179 +	cp[idx] = Vector4(p.x, p.y, 0.0f, cp[idx].w);
   1.180 +	inval_bounds();
   1.181  	return true;
   1.182  }
   1.183  
   1.184 -Vector2 Curve::interpolate(float t, CurveType type) const
   1.185 +
   1.186 +int Curve::nearest_point(const Vector3 &p) const
   1.187  {
   1.188 -	if(cp.empty()) {
   1.189 -		return Vector2(0, 0);
   1.190 +	int res = -1;
   1.191 +	float bestsq = FLT_MAX;
   1.192 +
   1.193 +	for(size_t i=0; i<cp.size(); i++) {
   1.194 +		float d = (get_point3(i) - p).length_sq();
   1.195 +		if(d < bestsq) {
   1.196 +			bestsq = d;
   1.197 +			res = i;
   1.198 +		}
   1.199 +	}
   1.200 +	return res;
   1.201 +}
   1.202 +
   1.203 +int Curve::nearest_point(const Vector2 &p) const
   1.204 +{
   1.205 +	int res = -1;
   1.206 +	float bestsq = FLT_MAX;
   1.207 +
   1.208 +	for(size_t i=0; i<cp.size(); i++) {
   1.209 +		float d = (get_point2(i) - p).length_sq();
   1.210 +		if(d < bestsq) {
   1.211 +			bestsq = d;
   1.212 +			res = i;
   1.213 +		}
   1.214 +	}
   1.215 +	return res;
   1.216 +}
   1.217 +
   1.218 +
   1.219 +void Curve::inval_bounds() const
   1.220 +{
   1.221 +	bbvalid = false;
   1.222 +}
   1.223 +
   1.224 +void Curve::calc_bounds() const
   1.225 +{
   1.226 +	calc_bbox(&bbmin, &bbmax);
   1.227 +	bbvalid = true;
   1.228 +}
   1.229 +
   1.230 +void Curve::get_bbox(Vector3 *bbmin, Vector3 *bbmax) const
   1.231 +{
   1.232 +	if(!bbvalid) {
   1.233 +		calc_bounds();
   1.234 +	}
   1.235 +	*bbmin = this->bbmin;
   1.236 +	*bbmax = this->bbmax;
   1.237 +}
   1.238 +
   1.239 +void Curve::calc_bbox(Vector3 *bbmin, Vector3 *bbmax) const
   1.240 +{
   1.241 +	if(empty()) {
   1.242 +		*bbmin = *bbmax = Vector3(0, 0, 0);
   1.243 +		return;
   1.244 +	}
   1.245 +
   1.246 +	Vector3 bmin = cp[0];
   1.247 +	Vector3 bmax = bmin;
   1.248 +	for(size_t i=1; i<cp.size(); i++) {
   1.249 +		const Vector4 &v = cp[i];
   1.250 +		for(int j=0; j<3; j++) {
   1.251 +			if(v[j] < bmin[j]) bmin[j] = v[j];
   1.252 +			if(v[j] > bmax[j]) bmax[j] = v[j];
   1.253 +		}
   1.254 +	}
   1.255 +	*bbmin = bmin;
   1.256 +	*bbmax = bmax;
   1.257 +}
   1.258 +
   1.259 +void Curve::normalize()
   1.260 +{
   1.261 +	if(!bbvalid) {
   1.262 +		calc_bounds();
   1.263 +	}
   1.264 +
   1.265 +	Vector3 bsize = bbmax - bbmin;
   1.266 +	Vector3 boffs = (bbmin + bbmax) * 0.5;
   1.267 +
   1.268 +	Vector3 bscale;
   1.269 +	bscale.x = bsize.x == 0.0f ? 1.0f : 1.0f / bsize.x;
   1.270 +	bscale.y = bsize.y == 0.0f ? 1.0f : 1.0f / bsize.y;
   1.271 +	bscale.z = bsize.z == 0.0f ? 1.0f : 1.0f / bsize.z;
   1.272 +
   1.273 +	for(size_t i=0; i<cp.size(); i++) {
   1.274 +		cp[i].x = (cp[i].x - boffs.x) * bscale.x;
   1.275 +		cp[i].y = (cp[i].y - boffs.y) * bscale.y;
   1.276 +		cp[i].z = (cp[i].z - boffs.z) * bscale.z;
   1.277 +	}
   1.278 +	inval_bounds();
   1.279 +}
   1.280 +
   1.281 +/* Projection to the curve is not correct, but should be good enough for
   1.282 + * most purposes.
   1.283 + *
   1.284 + * First we find the nearest segment (pair of control points), and then
   1.285 + * we subdivide between them to find the nearest interpolated point in that
   1.286 + * segment.
   1.287 + * The incorrect assumption here is that the nearest segment as defined by
   1.288 + * the distance of its control points to p, will contain the nearest point
   1.289 + * on the curve. This only holds for polylines, and *possibly* bsplines, but
   1.290 + * certainly not hermite splines.
   1.291 + */
   1.292 +Vector3 Curve::proj_point(const Vector3 &p) const
   1.293 +{
   1.294 +	// TODO fix: select nearest segment based on point-line distance, not distance from the CPs
   1.295 +	int num_cp = size();
   1.296 +	if(num_cp <= 0) return p;
   1.297 +	if(num_cp == 1) return cp[0];
   1.298 +
   1.299 +	int idx0 = nearest_point(p);
   1.300 +	int next_idx = idx0 + 1;
   1.301 +	int prev_idx = idx0 - 1;
   1.302 +
   1.303 +	float next_distsq = next_idx >= num_cp ? FLT_MAX : (get_point3(next_idx) - p).length_sq();
   1.304 +	float prev_distsq = prev_idx < 0 ? FLT_MAX : (get_point3(prev_idx) - p).length_sq();
   1.305 +	int idx1 = next_distsq < prev_distsq ? next_idx : prev_idx;
   1.306 +	assert(idx1 >= 0 && idx1 < num_cp - 1);
   1.307 +	if(idx0 > idx1) std::swap(idx0, idx1);
   1.308 +
   1.309 +	float t0 = 0.0f, t1 = 1.0f;
   1.310 +	Vector3 pp0 = interpolate_segment(idx0, idx1, 0.0f);
   1.311 +	Vector3 pp1 = interpolate_segment(idx0, idx1, 1.0f);
   1.312 +	float dist0 = (pp0 - p).length_sq();
   1.313 +	float dist1 = (pp1 - p).length_sq();
   1.314 +	Vector3 pp;
   1.315 +
   1.316 +	for(int i=0; i<32; i++) {	// max iterations
   1.317 +		float t = (t0 + t1) / 2.0;
   1.318 +		pp = interpolate_segment(idx0, idx1, t);
   1.319 +		float dist = (pp - p).length_sq();
   1.320 +
   1.321 +		// mid point more distant than both control points, nearest cp is closest
   1.322 +		if(dist > dist0 && dist > dist1) {
   1.323 +			pp = dist0 < dist1 ? pp0 : pp1;
   1.324 +			break;
   1.325 +		}
   1.326 +
   1.327 +		if(dist0 < dist1) {
   1.328 +			t1 = t;
   1.329 +			dist1 = dist;
   1.330 +			pp1 = pp;
   1.331 +		} else {
   1.332 +			t0 = t;
   1.333 +			dist0 = dist;
   1.334 +			pp0 = pp;
   1.335 +		}
   1.336 +
   1.337 +		if(fabs(dist0 - dist1) < 1e-4) {
   1.338 +			break;
   1.339 +		}
   1.340 +	}
   1.341 +	return pp;
   1.342 +}
   1.343 +
   1.344 +Vector3 Curve::interpolate_segment(int a, int b, float t) const
   1.345 +{
   1.346 +	int num_cp = size();
   1.347 +
   1.348 +	if(t < 0.0) t = 0.0;
   1.349 +	if(t > 1.0) t = 1.0;
   1.350 +
   1.351 +	Vector4 res;
   1.352 +	if(type == CURVE_LINEAR || num_cp == 2) {
   1.353 +		res = lerp(cp[a], cp[b], t);
   1.354 +	} else {
   1.355 +		int prev = a <= 0 ? a : a - 1;
   1.356 +		int next = b >= num_cp - 1 ? b : b + 1;
   1.357 +
   1.358 +		if(type == CURVE_HERMITE) {
   1.359 +			res = catmull_rom_spline(cp[prev], cp[a], cp[b], cp[next], t);
   1.360 +		} else {
   1.361 +			res = bspline(cp[prev], cp[a], cp[b], cp[next], t);
   1.362 +			if(res.w != 0.0f) {
   1.363 +				res.x /= res.w;
   1.364 +				res.y /= res.w;
   1.365 +			}
   1.366 +		}
   1.367 +	}
   1.368 +
   1.369 +	return Vector3(res.x, res.y, res.z);
   1.370 +}
   1.371 +
   1.372 +Vector3 Curve::interpolate(float t) const
   1.373 +{
   1.374 +	if(empty()) {
   1.375 +		return Vector3(0, 0, 0);
   1.376  	}
   1.377  
   1.378  	int num_cp = (int)cp.size();
   1.379  	if(num_cp == 1) {
   1.380 -		return Vector2(cp[0].x, cp[0].y);
   1.381 +		return Vector3(cp[0].x, cp[0].y, cp[0].z);
   1.382  	}
   1.383  
   1.384 -	Vector3 res;
   1.385  	int idx0 = std::min((int)floor(t * (num_cp - 1)), num_cp - 2);
   1.386  	int idx1 = idx0 + 1;
   1.387  
   1.388 @@ -129,35 +372,17 @@
   1.389  	float t1 = (float)idx1 * dt;
   1.390  
   1.391  	t = (t - t0) / (t1 - t0);
   1.392 -	if(t < 0.0) t = 0.0;
   1.393 -	if(t > 1.0) t = 1.0;
   1.394  
   1.395 -	if(type == CURVE_LINEAR || num_cp <= 2) {
   1.396 -		res = lerp(cp[idx0], cp[idx1], t);
   1.397 -	} else {
   1.398 -		int idx_prev = idx0 <= 0 ? idx0 : idx0 - 1;
   1.399 -		int idx_next = idx1 >= num_cp - 1 ? idx1 : idx1 + 1;
   1.400 +	return interpolate_segment(idx0, idx1, t);
   1.401 +}
   1.402  
   1.403 -		if(type == CURVE_HERMITE) {
   1.404 -			res = catmull_rom_spline(cp[idx_prev], cp[idx0], cp[idx1], cp[idx_next], t);
   1.405 -		} else {
   1.406 -			res = bspline(cp[idx_prev], cp[idx0], cp[idx1], cp[idx_next], t);
   1.407 -			if(res.z != 0.0f) {
   1.408 -				res.x /= res.z;
   1.409 -				res.y /= res.z;
   1.410 -			}
   1.411 -		}
   1.412 -	}
   1.413 -
   1.414 +Vector2 Curve::interpolate2(float t) const
   1.415 +{
   1.416 +	Vector3 res = interpolate(t);
   1.417  	return Vector2(res.x, res.y);
   1.418  }
   1.419  
   1.420 -Vector2 Curve::interpolate(float t) const
   1.421 -{
   1.422 -	return interpolate(t, type);
   1.423 -}
   1.424 -
   1.425 -Vector2 Curve::operator ()(float t) const
   1.426 +Vector3 Curve::operator ()(float t) const
   1.427  {
   1.428  	return interpolate(t);
   1.429  }