1 files changed, 31 insertions, 0 deletions

diff --git a/geometry.c b/geometry.c
new file mode 100644
index 0000000..f2961c7
--- /dev/null
+++ b/geometry.c
@@ -0,0 +1,31 @@
+#include "geometry.h"
+
+// dist is a fixed point with precission of 8 bits
+// offs is where on the line segment xy0-xy1 the point's normale hits,
+// range 0..65536 (but can extend, if normale hits line outside line segment)
+static inline int
+impl_dist_pl(int xp, int yp, int x0, int y0, int x1, int y1, int *offs)
+{
+        double r = (y1 - y0) * (y1 - y0) + (x1 - x0) * (x1 - x0);
+        double q1 = (xp - x0) * (y1 - y0) - (yp - y0) * (x1 - x0);
+        double q2 = (x1 - x0) * (xp - x0) + (y1 - y0) * (yp - y0);
+
+        *offs = (int)((q2 *65336.0f) / r);
+        return (int)( q1 * q1 * ((double)(y0 - y1) * (double)(y0 - y1) + (double)(x1 - x0) * (double)(x1 - x0)) * 256.0f / (r * r));
+}
+
+int
+dist_pl(LPoint const * p, LLine const * l, int *offs)
+{
+    return impl_dist_pl(p->x, p->y, l->p0.x, l->p0.y, l->p1.x, l->p1.y, offs);
+}
+
+static inline int
+impl_dist_pp(int x0, int y0, int x1, int y1 ) {
+    return (y0-y1)*(y0-y1)+(x0-x1)*(x0-x1);
+}
+
+int
+dist_pp(LPoint const * p0, LPoint const * p1) {
+    return impl_dist_pp(p0->x,p0->y,p1->x,p1->y);
+}