fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 06:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix the fat, flat line bug

Browse Source 
git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5407 42af7a65-404d-4744-a932-0658087f49c3
This commit is contained in:
patacongo
2012-12-02 17:34:08 +00:00
parent 3f5c105156
commit d128c03666
7 changed files with 443 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
nuttx/graphics/nxglib/nxglib_splitline.c
+278 -87
View File
@@ -1,7 +1,7 @@
/****************************************************************************
* graphics/nxglib/nxglib_splitline.c
*
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
* Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
@@ -42,6 +42,7 @@
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include <debug.h>
#include <nuttx/nx/nxglib.h>
@@ -49,12 +50,16 @@
* Pre-Processor Definitions
****************************************************************************/
#define SMALL_SIN 1966 /* 1966/65536 = 0.03 */
/****************************************************************************
* Private Types
****************************************************************************/
struct b16point_s
{
b16_t x;
b16_t y;
};
/****************************************************************************
* Private Data
****************************************************************************/
@@ -67,6 +72,12 @@
* Private Functions
****************************************************************************/
static b16_t nxgl_interpolate(b16_t x, b16_t dy, b16_t dxdy)
{
b16_t dx = b16mulb16(dy, dxdy);
return x + dx;
}
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -117,15 +128,19 @@ int nxgl_splitline(FAR struct nxgl_vector_s *vector,
struct nxgl_vector_s line;
nxgl_coord_t iheight;
nxgl_coord_t iwidth;
nxgl_coord_t iy;
nxgl_coord_t triheight;
nxgl_coord_t halfheight;
b16_t adjwidth;
b16_t xoffset;
b16_t halfoffset;
nxgl_coord_t iyoffset;
struct b16point_s quad[4];
b16_t b16xoffset;
b16_t b16yoffset;
b16_t b16dxdy;
b16_t angle;
b16_t cosangle;
b16_t sinangle;
b16_t b16x;
b16_t b16y;
gvdbg("vector: (%d,%d)->(%d,%d) linewidth: %d\n",
vector->pt1.x, vector->pt1.y, vector->pt2.x, vector->pt2.y, linewidth);
/* First, check the linewidth */
@@ -153,7 +168,7 @@ int nxgl_splitline(FAR struct nxgl_vector_s *vector,
line.pt2.x = vector->pt1.x;
line.pt2.y = vector->pt1.y;
}
else
else /* if (vector->pt1.y == vector->pt2.y) */
{
/* First degenerate case: The line is horizontal. */
@@ -174,6 +189,10 @@ int nxgl_splitline(FAR struct nxgl_vector_s *vector,
rect->pt1.y = vector->pt1.y - (linewidth >> 1);
rect->pt2.y = rect->pt1.y + linewidth - 1;
gvdbg("Horizontal rect: (%d,%d),(%d,%d)\n",
rect->pt1.x, rect->pt1.y, rect->pt2.x, rect->pt2.y);
return 2;
}
@@ -188,6 +207,10 @@ int nxgl_splitline(FAR struct nxgl_vector_s *vector,
rect->pt1.x = line.pt1.x - (linewidth >> 1);
rect->pt2.x = rect->pt1.x + linewidth - 1;
gvdbg("Vertical rect: (%d,%d),(%d,%d)\n",
rect->pt1.x, rect->pt1.y, rect->pt2.x, rect->pt2.y);
return 2;
}
@@ -207,6 +230,11 @@ int nxgl_splitline(FAR struct nxgl_vector_s *vector,
traps[1].bot.x1 = itob16(line.pt2.x);
traps[1].bot.x2 = traps[1].bot.x1;
traps[1].bot.y = line.pt2.y;
gvdbg("Vertical traps[1]: (%08x,%08x,%d),(%08x,%08x, %d)\n",
traps[1].top.x1, traps[1].top.x2, traps[1].top.y,
traps[1].bot.x1, traps[1].bot.x2, traps[1].bot.y);
return 1;
}
@@ -226,103 +254,260 @@ int nxgl_splitline(FAR struct nxgl_vector_s *vector,
iwidth = line.pt1.x - line.pt2.x + 1;
}
/* Triangle height: linewidth * cosA
* Adjusted width: triheight / sinA
* X offset : linewidth * linewidth / adjusted line width
/* Applying the line width to the line results in a rotated, rectangle.
* Get the Y offset from an end of the original thin line to a corner of the fat line.
*
* Angle of line: angle = atan2(iheight, iwidth)
* Y offset from line: b16yoffset = linewidth * cos(angle)
*
* For near verical lines, b16yoffset is be nearly zero. For near horizontal
* lines, b16yOffset is be about the same as linewidth.
*/
angle = b16atan2(itob16(iheight), itob16(iwidth));
triheight = b16toi(linewidth * b16cos(angle) + b16HALF);
halfheight = (triheight >> 1);
angle = b16atan2(itob16(iheight), itob16(iwidth));
cosangle = b16cos(angle);
b16yoffset = (linewidth * cosangle + 1) >> 1;
/* If the sine of the angle is tiny (i.e., the line is nearly horizontal),
* then we cannot compute the adjusted width. In this case, just use
* the width of the line bounding box.
/* Get the X offset from an end of the original thin line to a corner of the fat line.
*
* For near vertical lines, b16xoffset is about the same as linewidth. For near
* horizontal lines, b16xoffset is nearly zero.
*/
sinangle = b16sin(angle);
if (sinangle < SMALL_SIN)
{
adjwidth = itob16(iwidth);
xoffset = 0;
}
else
{
adjwidth = b16divb16(itob16(linewidth), sinangle);
xoffset = itob16(linewidth * linewidth);
xoffset = b16divb16(xoffset, adjwidth);
}
sinangle = b16sin(angle);
b16xoffset = (linewidth * sinangle + 1) >> 1;
halfoffset = (xoffset >> 1);
gvdbg("height: %d width: %d angle: %08x b16yoffset: %08x b16xoffset: %08x\n",
iheight, iwidth, angle, b16yoffset, b16xoffset);
/* Return the top triangle (if there is one). NOTE that the horizontal
* (z) positions are represented with 16 bits of fraction. The vertical
* (y) positions, on the other hand, are integer.
*/
/* Now we know all four points of the rotated rectangle */
if (triheight > 0)
iyoffset = b16toi(b16yoffset + b16HALF);
if (iyoffset > 0)
{
/* Get the Y positions of each point */
b16y = itob16(line.pt1.y);
quad[0].y = b16y - b16yoffset;
quad[1].y = b16y + b16yoffset;
b16y = itob16(line.pt2.y);
quad[2].y = b16y - b16yoffset;
quad[3].y = b16y + b16yoffset;
if (line.pt1.x < line.pt2.x)
{
/* Line is going "south east" */
/* Line is going "south east". Get the X positions of each point */
b16x = itob16(line.pt1.x) - halfoffset;
iy = line.pt1.y + halfheight;
b16x = itob16(line.pt1.x);
quad[0].x = b16x + b16xoffset;
quad[1].x = b16x - b16xoffset;
traps[0].top.x1 = b16x + xoffset;
traps[0].top.x2 = traps[0].top.x1;
traps[0].top.y = iy - triheight + 1;
traps[0].bot.x1 = b16x;
traps[0].bot.x2 = b16x + adjwidth - b16ONE;
traps[0].bot.y = iy;
b16x = itob16(line.pt2.x);
quad[2].x = b16x + b16xoffset;
quad[3].x = b16x - b16xoffset;
b16x = itob16(line.pt2.x) + halfoffset;
iy = line.pt2.y - halfheight;
gvdbg("Southeast: quad (%08x,%08x),(%08x,%08x),(%08x,%08x),(%08x,%08x)\n",
quad[0].x, quad[0].y, quad[1].x, quad[1].y,
quad[2].x, quad[2].y, quad[3].x, quad[3].y);
traps[2].top.x1 = b16x - adjwidth + b16ONE;
traps[2].top.x2 = b16x;
traps[2].top.y = iy;
traps[2].bot.x1 = b16x - xoffset;
traps[2].bot.x2 = traps[2].bot.x1;
traps[2].bot.y = iy + triheight - 1;
/* Now we can form the trapezoids. The top of the first trapezoid
* (triangle) is at quad[0]
*/
traps[0].top.x1 = quad[0].x;
traps[0].top.x2 = quad[0].x;
traps[0].top.y = b16toi(quad[0].y + b16HALF);
/* The bottom of the first trapezoid (triangle) may be either at
* quad[1] or quad[2], depending upon orientation.
*/
if (quad[1]. y < quad[2].y)
{
/* quad[1] is at the bottom left of the triangle. Interpolate
* to get the corresponding point on the right side.
*
* Interpolation is from quad[0] along the line quad[0]->quad[2]
* which as the same slope as the line (positive)
*/
b16dxdy = itob16(iwidth) / iheight;
traps[0].bot.x1 = quad[1].x;
traps[0].bot.x2 = nxgl_interpolate(quad[0].x, quad[1].y - quad[0].y, b16dxdy);
traps[0].bot.y = b16toi(quad[1].y + b16HALF);
/* quad[1] is at the top left of the second trapezoid. quad[2} is
* at the bottom right of the second trapezoid. Interpolate to get
* corresponding point on the left side.
*
* Interpolation is from quad[1] along the line quad[1]->quad[3]
* which as the same slope as the line (positive)
*/
traps[1].top.x1 = traps[0].bot.x1;
traps[1].top.x2 = traps[0].bot.x2;
traps[1].top.y = traps[0].bot.y;
traps[1].bot.x1 = nxgl_interpolate(traps[1].top.x1, quad[2].y - quad[1].y, b16dxdy);
traps[1].bot.x2 = quad[2].x;
traps[1].bot.y = b16toi(quad[2].y + b16HALF);
}
else
{
/* quad[2] is at the bottom right of the triangle. Interpolate
* to get the corresponding point on the left side.
*
* Interpolation is from quad[0] along the line quad[0]->quad[1]
* which orthogonal to the slope of the line (and negative)
*/
b16dxdy = -itob16(iheight) / iwidth;
traps[0].bot.x1 = nxgl_interpolate(quad[0].x, quad[2].y - quad[0].y, b16dxdy);
traps[0].bot.x2 = quad[2].x;
traps[0].bot.y = b16toi(quad[2].y + b16HALF);
/* quad[2] is at the top right of the second trapezoid. quad[1} is
* at the bottom left of the second trapezoid. Interpolate to get
* corresponding point on the right side.
*
* Interpolation is from quad[2] along the line quad[2]->quad[3]
* which as the same slope as the previous interpolation.
*/
traps[1].top.x1 = traps[0].bot.x1;
traps[1].top.x2 = traps[0].bot.x2;
traps[1].top.y = traps[0].bot.y;
traps[1].bot.x1 = quad[1].x;
traps[1].bot.x2 = nxgl_interpolate(traps[1].top.x2, quad[1].y - quad[2].y, b16dxdy);
traps[1].bot.y = b16toi(quad[1].y + b16HALF);
}
/* The final trapezond (triangle) at the bottom is new well defined */
traps[2].top.x1 = traps[1].bot.x1;
traps[2].top.x2 = traps[1].bot.x2;
traps[2].top.y = traps[1].bot.y;
traps[2].bot.x1 = quad[3].x;
traps[2].bot.x2 = quad[3].x;
traps[2].bot.y = b16toi(quad[3].y + b16HALF);
}
else
{
/* Line is going "south west" */
/* Get the X positions of each point */
b16x = itob16(line.pt1.x) + halfoffset;
iy = line.pt1.y + halfheight;
b16x = itob16(line.pt1.x);
quad[0].x = b16x - b16xoffset;
quad[1].x = b16x + b16xoffset;
traps[0].top.x1 = b16x - xoffset;
traps[0].top.x2 = traps[0].top.x1;
traps[0].top.y = iy - triheight + 1;
traps[0].bot.x1 = b16x - adjwidth + b16ONE;
traps[0].bot.x2 = b16x;
traps[0].bot.y = iy;
b16x = itob16(line.pt2.x);
quad[2].x = b16x - b16xoffset;
quad[3].x = b16x + b16xoffset;
b16x = itob16(line.pt2.x) - halfoffset;
iy = line.pt2.y - halfheight;
gvdbg("Southwest: quad (%08x,%08x),(%08x,%08x),(%08x,%08x),(%08x,%08x)\n",
quad[0].x, quad[0].y, quad[1].x, quad[1].y,
quad[2].x, quad[2].y, quad[3].x, quad[3].y);
traps[2].top.x1 = b16x;
traps[2].top.x2 = b16x + adjwidth - b16ONE;
traps[2].top.y = iy;
traps[2].bot.x1 = b16x + xoffset;
traps[2].bot.x2 = traps[2].bot.x1;
traps[2].bot.y = iy + triheight - 1;
/* Now we can form the trapezoids. The top of the first trapezoid
* (triangle) is at quad[0]
*/
traps[0].top.x1 = quad[0].x;
traps[0].top.x2 = quad[0].x;
traps[0].top.y = b16toi(quad[0].y + b16HALF);
/* The bottom of the first trapezoid (triangle) may be either at
* quad[1] or quad[2], depending upon orientation.
*/
if (quad[1].y < quad[2].y)
{
/* quad[1] is at the bottom right of the triangle. Interpolate
* to get the corresponding point on the left side.
*
* Interpolation is from quad[0] along the line quad[0]->quad[2]
* which as the same slope as the line (negative)
*/
b16dxdy = -itob16(iwidth) / iheight;
traps[0].bot.x1 = nxgl_interpolate(traps[0].top.x1, quad[1].y - quad[0].y, b16dxdy);
traps[0].bot.x2 = quad[1].x;
traps[0].bot.y = b16toi(quad[1].y + b16HALF);
/* quad[1] is at the top right of the second trapezoid. quad[2} is
* at the bottom left of the second trapezoid. Interpolate to get
* corresponding point on the right side.
*
* Interpolation is from quad[1] along the line quad[1]->quad[3]
* which as the same slope as the line (negative)
*/
traps[1].top.x1 = traps[0].bot.x1;
traps[1].top.x2 = traps[0].bot.x2;
traps[1].top.y = traps[0].bot.y;
traps[1].bot.x1 = quad[2].x;
traps[1].bot.x2 = nxgl_interpolate(traps[1].top.x2, quad[2].y - quad[1].y, b16dxdy);
traps[1].bot.y = b16toi(quad[2].y + b16HALF);
}
else
{
/* quad[2] is at the bottom left of the triangle. Interpolate
* to get the corresponding point on the right side.
*
* Interpolation is from quad[0] along the line quad[0]->quad[1]
* which orthogonal to the slope of the line (and positive)
*/
b16dxdy = itob16(iheight) / iwidth;
traps[0].bot.x1 = quad[2].x;
traps[0].bot.x2 = nxgl_interpolate(traps[0].top.x2, quad[2].y - quad[0].y, b16dxdy);
traps[0].bot.y = b16toi(quad[2].y + b16HALF);
/* quad[2] is at the top left of the second trapezoid. quad[1} is
* at the bottom right of the second trapezoid. Interpolate to get
* corresponding point on the left side.
*
* Interpolation is from quad[2] along the line quad[2]->quad[3]
* which as the same slope as the previous interpolation.
*/
traps[1].top.x1 = traps[0].bot.x1;
traps[1].top.x2 = traps[0].bot.x2;
traps[1].top.y = traps[0].bot.y;
traps[1].bot.x1 = nxgl_interpolate(traps[1].top.x1, quad[1].y - quad[2].y, b16dxdy);
traps[1].bot.x2 = quad[1].x;
traps[1].bot.y = b16toi(quad[1].y + b16HALF);
}
/* The final trapezond (triangle) at the bottom is new well defined */
traps[2].top.x1 = traps[1].bot.x1;
traps[2].top.x2 = traps[1].bot.x2;
traps[2].top.y = traps[1].bot.y;
traps[2].bot.x1 = quad[3].x;
traps[2].bot.x2 = quad[3].x;
traps[2].bot.y = b16toi(quad[3].y + b16HALF);
}
/* The center parallelogram is the horizontal edge of each triangle.
* Note the minor inefficency: that horizontal edges are drawn twice.
*/
gvdbg("traps[0]: (%08x,%08x,%d),(%08x,%08x,%d)\n",
traps[0].top.x1, traps[0].top.x2, traps[0].top.y,
traps[0].bot.x1, traps[0].bot.x2, traps[0].bot.y);
gvdbg("traps[1]: (%08x,%08x,%d),(%08x,%08x,%d)\n",
traps[1].top.x1, traps[1].top.x2, traps[1].top.y,
traps[1].bot.x1, traps[1].bot.x2, traps[1].bot.y);
gvdbg("traps[2]: (%08x,%08x,%d),(%08x,%08x,%d)\n",
traps[2].top.x1, traps[2].top.x2, traps[2].top.y,
traps[2].bot.x1, traps[2].bot.x2, traps[2].bot.y);
traps[1].top.x1 = traps[0].bot.x1;
traps[1].top.x2 = traps[0].bot.x2;
traps[1].top.y = traps[0].bot.y;
traps[1].bot.x1 = traps[2].top.x1;
traps[1].bot.x2 = traps[2].top.x2;
traps[1].bot.y = traps[2].top.y;
return 0;
}
@@ -330,12 +515,18 @@ int nxgl_splitline(FAR struct nxgl_vector_s *vector,
* bottom. Just return the center parallelogram.
*/
traps[1].top.x1 = itob16(line.pt1.x) - halfoffset;
traps[1].top.x2 = traps[1].top.x1 + adjwidth - 1;
traps[1].top.x1 = itob16(line.pt1.x - (linewidth >> 1));
traps[1].top.x2 = traps[1].top.x1 + itob16(linewidth - 1);
traps[1].top.y = line.pt1.y;
traps[1].bot.x1 = itob16(line.pt2.x) - halfoffset;
traps[1].bot.x2 = traps[1].bot.x1 + adjwidth - 1;
traps[1].bot.x1 = itob16(line.pt2.x - (linewidth >> 1));
traps[1].bot.x2 = traps[1].bot.x1 + itob16(linewidth - 1);
traps[1].bot.y = line.pt2.y;
gvdbg("Horizontal traps[1]: (%08x,%08x,%d),(%08x,%08x, %d)\n",
traps[1].top.x1, traps[1].top.x2, traps[1].top.y,
traps[1].bot.x1, traps[1].bot.x2, traps[1].bot.y);
return 1;
}