From 2823c3605a09276263cdee953341c524ba8dbf44 Mon Sep 17 00:00:00 2001 From: Corentin Barman Date: Sat, 12 Nov 2016 16:45:51 +0100 Subject: Fixed and improved the get_normal_vector function --- src/gdisp/gdisp.c | 88 ++++++++++++++++++------------------------------------- 1 file changed, 28 insertions(+), 60 deletions(-) (limited to 'src/gdisp') 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) { -- cgit v1.2.3