diff --git a/vtm-tests/test/org/oscim/utils/GeometryTest.java b/vtm-tests/test/org/oscim/utils/GeometryTest.java
index bbf9a993..907c0ce6 100644
--- a/vtm-tests/test/org/oscim/utils/GeometryTest.java
+++ b/vtm-tests/test/org/oscim/utils/GeometryTest.java
@@ -7,17 +7,36 @@ import org.oscim.utils.geom.GeometryUtils;
 public class GeometryTest {
 
     @Test
-    public void testIsTrisClockwise() {
-        // Coordinate system is LHS
-        float[] pA = new float[]{0, 0};
-        float[] pB = new float[]{1, 0};
-        float[] pC = new float[]{1, 1};
+    public void testClosestPointOnLine2D() {
+        float[] pP = {4, 1};
+        float[] pL = {1, 2};
+        float[] vL = {1, -1};
 
-        float area = GeometryUtils.isTrisClockwise(pA, pB, pC);
-        Assert.assertTrue(area > 0);
+        float[] point = GeometryUtils.closestPointOnLine2D(pP, pL, vL);
+        Assert.assertEquals(point[0], 3, 0.00001);
+        Assert.assertEquals(point[1], 0, 0.00001);
+    }
 
-        area = GeometryUtils.isTrisClockwise(pA, pC, pB);
-        Assert.assertTrue(area < 0);
+    @Test
+    public void testDistancePointLine2D() {
+        float[] pP = {1, 0};
+        float[] pL = {0, 0};
+        float[] vL = {2, 2};
+
+        float distance = GeometryUtils.distancePointLine2D(pP, pL, vL);
+        Assert.assertEquals(distance, Math.sqrt(2) / 2, 0.00001);
+    }
+
+    @Test
+    public void testDotProduct() {
+        float[] p = {-1, 0, 0, 0, 0, 0};
+
+        for (int i = 0; i < 9; i++) {
+            p[4] = (float) Math.cos(Math.toRadians(i * 45));
+            p[5] = (float) Math.sin(Math.toRadians(i * 45));
+            System.out.println("\n> " + (i * 45) + " " + p[3] + ":" + p[4] + "\n="
+                    + GeometryUtils.dotProduct(p, 0, 2, 4));
+        }
     }
 
     @Test
@@ -34,14 +53,16 @@ public class GeometryTest {
     }
 
     @Test
-    public void testDotProduct() {
-        float[] p = {-1, 0, 0, 0, 0, 0};
+    public void testIsTrisClockwise() {
+        // Coordinate system is LHS
+        float[] pA = new float[]{0, 0};
+        float[] pB = new float[]{1, 0};
+        float[] pC = new float[]{1, 1};
 
-        for (int i = 0; i < 9; i++) {
-            p[4] = (float) Math.cos(Math.toRadians(i * 45));
-            p[5] = (float) Math.sin(Math.toRadians(i * 45));
-            System.out.println("\n> " + (i * 45) + " " + p[3] + ":" + p[4] + "\n="
-                    + GeometryUtils.dotProduct(p, 0, 2, 4));
-        }
+        float area = GeometryUtils.isTrisClockwise(pA, pB, pC);
+        Assert.assertTrue(area > 0);
+
+        area = GeometryUtils.isTrisClockwise(pA, pC, pB);
+        Assert.assertTrue(area < 0);
     }
 }
diff --git a/vtm/src/org/oscim/utils/geom/GeometryUtils.java b/vtm/src/org/oscim/utils/geom/GeometryUtils.java
index fae04ab5..b60d3bed 100644
--- a/vtm/src/org/oscim/utils/geom/GeometryUtils.java
+++ b/vtm/src/org/oscim/utils/geom/GeometryUtils.java
@@ -131,13 +131,9 @@ public final class GeometryUtils {
         if (out == null)
             out = new float[2];
 
-        // Calculate center
         for (int i = 0; i < n; i += 2, pointPos += 2) {
-            float x = points[pointPos];
-            float y = points[pointPos + 1];
-
-            out[0] += x;
-            out[1] += y;
+            out[0] += points[pointPos];
+            out[1] += points[pointPos + 1];
         }
         out[0] = out[0] * 2 / n;
         out[1] = out[1] * 2 / n;
@@ -145,6 +141,21 @@ public final class GeometryUtils {
         return out;
     }
 
+    /**
+     * Calculate the closest point on a line.
+     * See: https://en.wikipedia.org/wiki/Vector_projection#Vector_projection_2
+     *
+     * @param pP point
+     * @param pL point of line
+     * @param vL vector of line
+     * @return the closest point on line
+     */
+    public static float[] closestPointOnLine2D(float[] pP, float[] pL, float[] vL) {
+        float[] vPL = diffVec(pL, pP);
+        float[] vPS = diffVec(vPL, scale(vL, dotProduct(vPL, vL) / dotProduct(vL, vL)));
+        return sumVec(pP, vPS);
+    }
+
     /**
      * @param a first vector
      * @param b second vector
@@ -222,6 +233,9 @@ public final class GeometryUtils {
     }
 
     /**
+     * Calculate the distance from a point to a line.
+     * See: https://en.wikipedia.org/wiki/Vector_projection#Vector_projection_2
+     *
      * @param pP point
      * @param pL point of line
      * @param vL vector of line
@@ -229,7 +243,7 @@ public final class GeometryUtils {
      */
     public static float distancePointLine2D(float[] pP, float[] pL, float[] vL) {
         float[] vPL = diffVec(pL, pP);
-        float[] vPS = diffVec(vPL, scale(vL, dotProduct(vPL, vL)));
+        float[] vPS = diffVec(vPL, scale(vL, dotProduct(vPL, vL) / dotProduct(vL, vL)));
         return (float) Math.sqrt(dotProduct(vPS, vPS));
     }