r5705 - developers/werner/cncmap/zmap

[werner at docs.openmoko.org]

Author: werner
Date: 2009-10-23 01:52:25 +0200 (Fri, 23 Oct 2009)
New Revision: 5705

Modified:
   developers/werner/cncmap/zmap/zmap.c
Log:
- zmap/zmap.c (zmap_point): handle the special case of all the closest points 
  being on a single line, so we don't get a plane



Modified: developers/werner/cncmap/zmap/zmap.c
===================================================================
--- developers/werner/cncmap/zmap/zmap.c	2009-10-22 23:27:55 UTC (rev 5704)
+++ developers/werner/cncmap/zmap/zmap.c	2009-10-22 23:52:25 UTC (rev 5705)
@@ -117,12 +117,17 @@
  * least square approximation (../rect/lr.c) on them.
  */
 
+#define EPSILON	0.001
+
+
 double zmap_point(struct xyz a, struct xyz b, struct xyz c, double x, double y)
 {
 	double xp, yp;
 	double xu, yu, zu;
 	double xv, yv, zv;
 	double det, s, t;
+	double d, d2;
+	double best;
 
 	DEBUG("point (%g, %g) -> (%g, %g) (%g, %g) (%g, %g)\n",
 	    x, y, a.x, a.y, b.x, b.y, c.x, c.y);
@@ -138,8 +143,31 @@
 	zv = c.z-a.z;
 
 	det = xu*yv-yu*xv;
-	s = (xp*yv-yp*xv)/det;
-	t = (xu*yp-yu*xp)/det;
+	if (fabs(det) > EPSILON) {
+		s = (xp*yv-yp*xv)/det;
+		t = (xu*yp-yu*xp)/det;
+		return a.z+s*zu+t*zv;
+	}
 
-	return a.z+s*zu+t*zv;
+	/*
+	 * We can sometimes end up with all the closest points on a line. A
+	 * typical case would be if the line we're mapping goes through a point
+	 * of a regularly spaced zmap.
+	 *
+	 * In this case, we simply have to find the closest point and use its
+	 * z coordinate. Note that this doesn't work so well if the zmap is not
+	 * a regular grid, but let's worry about such cases another time.
+	 */
+
+	d = hypot(xp, yp);
+	best = a.z;
+	d2 = hypot(x-b.x, y-b.y);
+	if (d2 < d) {
+		best = b.z;
+		d = d2;
+	}
+	d2 = hypot(x-c.x, y-c.y);
+	if (d2 < d)
+		best = c.z;
+	return best;
 }