1 files changed, 28 insertions, 60 deletions

diff --git a/src/gdisp/gdisp.c b/src/gdisp/gdisp.c
index ec7fef40..e03971a4 100644
--- a/src/gdisp/gdisp.c
+++ b/src/gdisp/gdisp.c
@@ -2991,75 +2991,43 @@ void gdispGDrawBox(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, co
 	 * equal to 'norm'. */
 	static void get_normal_vector(coord_t dx, coord_t dy, coord_t norm, coord_t *nx, coord_t *ny)
 	{
-		int32_t dx2, dy2, len_sq, norm_sq, norm_sq2;
-		int div, step, best, delta, abs_delta;
+		coord_t absDx, absDy;
+		int32_t len_n, len, len2;
+		char maxSteps;
 
-		dx2 = dx; dy2 = dy;
-		norm_sq = (int32_t)norm * norm;
-		norm_sq2 = norm_sq * 512;
+		/* Take the absolute value of dx and dy, multiplied by 2 for precision */
+		absDx = (dx >= 0 ? dx : -dx) * 2;
+		absDy = (dy >= 0 ? dy : -dy) * 2;
 
-		/* Scale dx2 and dy2 so that
-		 *     len_sq / 2 <= norm_sq * 512 <= len_sq * 2.
-		 * The scaling by 512 is to yield higher accuracy in division later. */
-		len_sq = dx2 * dx2 + dy2 * dy2;
+		/* Compute the quadrate length */
+		len2 = absDx * absDx + absDy * absDy;
 
-		if (len_sq < norm_sq2)
-		{
-			while (len_sq && len_sq < norm_sq2)
-			{
-				len_sq <<= 2; dx2 <<= 1; dy2 <<= 1;
-			}
-		}
-		else if (len_sq > norm_sq2)
-		{
-			while (len_sq && len_sq > norm_sq2)
-			{
-				len_sq >>= 2; dx2 >>= 1; dy2 >>= 1;
-			}
-		}
+		/* First aproximation : length = |dx| + |dy| */
+		len = absDx + absDy;
 
-		/* Now find the divider div so that
-		 *     len_sq / div^2 == norm_sq   i.e.  div = sqrt(len_sq / norm_sq)
-		 *
-		 * This is done using bisection search to avoid the need for floating
-		 * point sqrt.
-		 *
-		 * Based on previous scaling, we know that
-		 *     len_sq / 2 <= norm_sq * 512   <=>   div <= sqrt(1024) = 32
-		 *     len_sq * 2 >= norm_sq * 512   <=>   div >= sqrt(256) = 16
-		 */
-		div = 24; step = 8;
-		best = 256;
-
-		for (;;)
+		/* Give a max number of steps, the calculation usually takes 3 or 4 */
+		for(maxSteps = 8; maxSteps > 0; maxSteps--)
 		{
-			dx = dx2 / div;
-			dy = dy2 / div;
-			len_sq = dx*dx + dy*dy;
-
-			delta = len_sq - norm_sq;
-
-			abs_delta = (delta >= 0) ? delta : -delta;
-
-			if (abs_delta < best)
-			{
-				*nx = dy;
-				*ny = -dx;
-				best = abs_delta;
-			}
+			/* Use an adapted version of Newton's algorithm to find the correct length 
+			 * This calculation converge quadratically towards the correct length
+			 * n(x+1) = (n(x) + len^2 / n(x)) / 2 
+			 */
+			len_n = (len + len2 / len) / 2;
 
-			if (delta > 0)
-				div += step;
-			else if (delta < 0)
-				div -= step;
-			else if (delta == 0)
+			/* We reach max precision when the last result is equal or greater than the previous one */ 
+			if(len_n >= len){
 				break;
+			}
 
-			if (step == 0)
-				break;
-			else
-				step >>= 1; /* Do one round with step = 0 to calculate final result. */
+			len = len_n;
 		}
+
+		/* Compute the normal vector using nx = dy * desired length / vector length
+		 * The solution is rounded to the nearest integer
+		 */
+		*nx = rounding_div(dy * norm * 2, len);
+		*ny = rounding_div(-dx * norm * 2, len);
+		return;
 	}
 
 	void gdispGDrawThickLine(GDisplay *g, coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color, coord_t width, bool_t round) {