From 2823c3605a09276263cdee953341c524ba8dbf44 Mon Sep 17 00:00:00 2001
From: Corentin Barman <corentin.barman@hotmail.com>
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')
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