1.1 --- a/src/curve.cc	Sun Dec 20 07:21:32 2015 +0200
     1.2 +++ b/src/curve.cc	Sun Dec 20 08:22:24 2015 +0200
     1.3 @@ -262,69 +262,69 @@
     1.4  	inval_bounds();
     1.5  }
     1.6  
     1.7 -/* Projection to the curve is not correct, but should be good enough for
     1.8 - * most purposes.
     1.9 - *
    1.10 - * First we find the nearest segment (pair of control points), and then
    1.11 - * we subdivide between them to find the nearest interpolated point in that
    1.12 - * segment.
    1.13 - * The incorrect assumption here is that the nearest segment as defined by
    1.14 - * the distance of its control points to p, will contain the nearest point
    1.15 - * on the curve. This only holds for polylines, and *possibly* bsplines, but
    1.16 - * certainly not hermite splines.
    1.17 - */
    1.18 -Vector3 Curve::proj_point(const Vector3 &p) const
    1.19 +Vector3 Curve::proj_point(const Vector3 &p, float refine_thres) const
    1.20  {
    1.21 -	// TODO fix: select nearest segment based on point-line distance, not distance from the CPs
    1.22 +	// first step through the curve a few times and find the nearest of them
    1.23  	int num_cp = size();
    1.24 -	if(num_cp <= 0) return p;
    1.25 -	if(num_cp == 1) return cp[0];
    1.26 +	int num_steps = num_cp * 5;	// arbitrary number; sounds ok
    1.27 +	float dt = 1.0f / (float)(num_steps - 1);
    1.28  
    1.29 -	int idx0 = nearest_point(p);
    1.30 -	int next_idx = idx0 + 1;
    1.31 -	int prev_idx = idx0 - 1;
    1.32 +	float best_distsq = FLT_MAX;
    1.33 +	float best_t = 0.0f;
    1.34 +	Vector3 best_pp;
    1.35  
    1.36 -	float next_distsq = next_idx >= num_cp ? FLT_MAX : (get_point3(next_idx) - p).length_sq();
    1.37 -	float prev_distsq = prev_idx < 0 ? FLT_MAX : (get_point3(prev_idx) - p).length_sq();
    1.38 -	int idx1 = next_distsq < prev_distsq ? next_idx : prev_idx;
    1.39 -	assert(idx1 >= 0 && idx1 < num_cp - 1);
    1.40 -	if(idx0 > idx1) std::swap(idx0, idx1);
    1.41 +	float t = 0.0f;
    1.42 +	for(int i=0; i<num_steps; i++) {
    1.43 +		Vector3 pp = interpolate(t);
    1.44 +		float distsq = (pp - p).length_sq();
    1.45 +		if(distsq < best_distsq) {
    1.46 +			best_distsq = distsq;
    1.47 +			best_pp = pp;
    1.48 +			best_t = t;
    1.49 +		}
    1.50 +		t += dt;
    1.51 +	}
    1.52  
    1.53 -	float t0 = 0.0f, t1 = 1.0f;
    1.54 -	Vector3 pp0 = interpolate_segment(idx0, idx1, 0.0f);
    1.55 -	Vector3 pp1 = interpolate_segment(idx0, idx1, 1.0f);
    1.56 -	float dist0 = (pp0 - p).length_sq();
    1.57 -	float dist1 = (pp1 - p).length_sq();
    1.58 -	Vector3 pp;
    1.59 +	// refine by gradient descent
    1.60 +	float dist = best_distsq;
    1.61 +	t = best_t;
    1.62 +	dt *= 0.05;
    1.63 +	for(;;) {
    1.64 +		float tn = t + dt;
    1.65 +		float tp = t - dt;
    1.66  
    1.67 -	for(int i=0; i<32; i++) {	// max iterations
    1.68 -		float t = (t0 + t1) / 2.0;
    1.69 -		pp = interpolate_segment(idx0, idx1, t);
    1.70 -		float dist = (pp - p).length_sq();
    1.71 +		float dn = (interpolate(tn) - p).length_sq();
    1.72 +		float dp = (interpolate(tp) - p).length_sq();
    1.73  
    1.74 -		// mid point more distant than both control points, nearest cp is closest
    1.75 -		if(dist > dist0 && dist > dist1) {
    1.76 -			pp = dist0 < dist1 ? pp0 : pp1;
    1.77 +		if(fabs(dn - dp) < refine_thres * refine_thres) {
    1.78  			break;
    1.79  		}
    1.80  
    1.81 -		if(dist0 < dist1) {
    1.82 -			t1 = t;
    1.83 -			dist1 = dist;
    1.84 -			pp1 = pp;
    1.85 +		if(dn < dist) {
    1.86 +			t = tn;
    1.87 +			dist = dn;
    1.88 +		} else if(dp < dist) {
    1.89 +			t = tp;
    1.90 +			dist = dp;
    1.91  		} else {
    1.92 -			t0 = t;
    1.93 -			dist0 = dist;
    1.94 -			pp0 = pp;
    1.95 -		}
    1.96 -
    1.97 -		if(fabs(dist0 - dist1) < 1e-4) {
    1.98 -			break;
    1.99 +			break;	// found the minimum
   1.100  		}
   1.101  	}
   1.102 -	return pp;
   1.103 +
   1.104 +	return interpolate(t);
   1.105  }
   1.106  
   1.107 +float Curve::distance(const Vector3 &p) const
   1.108 +{
   1.109 +	return (proj_point(p) - p).length();
   1.110 +}
   1.111 +
   1.112 +float Curve::distance_sq(const Vector3 &p) const
   1.113 +{
   1.114 +	return fabs((proj_point(p) - p).length_sq());
   1.115 +}
   1.116 +
   1.117 +
   1.118  Vector3 Curve::interpolate_segment(int a, int b, float t) const
   1.119  {
   1.120  	int num_cp = size();
   1.121 @@ -346,6 +346,7 @@
   1.122  			if(res.w != 0.0f) {
   1.123  				res.x /= res.w;
   1.124  				res.y /= res.w;
   1.125 +				res.z /= res.w;
   1.126  			}
   1.127  		}
   1.128  	}
   1.129 @@ -364,6 +365,9 @@
   1.130  		return Vector3(cp[0].x, cp[0].y, cp[0].z);
   1.131  	}
   1.132  
   1.133 +	if(t < 0.0) t = 0.0;
   1.134 +	if(t > 1.0) t = 1.0;
   1.135 +
   1.136  	int idx0 = std::min((int)floor(t * (num_cp - 1)), num_cp - 2);
   1.137  	int idx1 = idx0 + 1;
   1.138