// ---------------------------------------------------------------------------------------------------------------------
void WuLine(pixel* frame, Fix16 x1, Fix16 y1, Fix16 x2, Fix16 y2, ColourChoice cc)
{
byte r = 0;
byte g = 0;
Fix16 dx = x2 - x1;
Fix16 dy = y2 - y1;
bool swapXY = false;
if (Fix16::abs(dx) < Fix16::abs(dy))
{
XORByteSwap(x1.value, y1.value);
XORByteSwap(x2.value, y2.value);
XORByteSwap(dx.value, dy.value);
swapXY = true;
}
if (x2 < x1)
{
XORByteSwap(x1.value, x2.value);
XORByteSwap(y1.value, y2.value);
}
Fix16 gradient = dy / dx;
Fix16 xend = round(x1);
Fix16 yend = y1 + gradient * (xend - x1);
Fix16 xgap = rfpart(x1 + 0.5f);
int16_t xpxl1 = xend.asInt(); // this will be used in the main loop
int16_t ypxl1 = ipart(yend).asInt();
GetBasicColour(rfpart(yend) * xgap, cc, r, g);
setLED(frame, xpxl1, ypxl1, r, g, true, swapXY);
GetBasicColour(fpart(yend) * xgap, cc, r, g);
setLED(frame, xpxl1, ypxl1 + 1, r, g, true, swapXY);
Fix16 intery = yend + gradient; // first y-intersection for the main loop
// handle second endpoint
xend = round(x2);
yend = y2 + gradient * (xend - x2);
xgap = fpart(x2 + 0.5f);
int16_t xpxl2 = xend.asInt(); // this will be used in the main loop
int16_t ypxl2 = ipart(yend).asInt();
GetBasicColour(rfpart(yend) * xgap, cc, r, g);
setLED(frame, xpxl2, ypxl2, r, g, true, swapXY);
GetBasicColour(fpart(yend) * xgap, cc, r, g);
setLED(frame, xpxl2, ypxl2 + 1, r, g, true, swapXY);
int16_t xa = xpxl1 + 1;
int16_t xb = xpxl2 - 1;
if (xb - xa > 120)
return;
for (; xa <= xb; xa ++)
{
GetBasicColour(rfpart(intery), cc, r, g);
setLED(frame, xa, ipart(intery).asInt(), r, g, true, swapXY);
GetBasicColour(fpart(intery), cc, r, g);
setLED(frame, xa, ipart(intery).asInt() + 1, r, g, true, swapXY);
intery = intery + gradient;
}
}
void drawLine(sf::RenderTarget& target, float x1, float y1, float x2, float y2, const sf::Color& color)
{
float dx = x2 - x1;
float dy = y2 - y1;
// Store all points in an arrry
sf::VertexArray va(sf::Points);
if (std::fabs(dx) > std::fabs(dy))
{
if(x2 < x1)
{
std::swap(x1, x2);
std::swap(y1, y2);
}
float gradient = dy / dx;
float xend = static_cast<float>(round(x1));
float yend = y1 + gradient * (xend - x1);
float xgap = rfpart(x1 + 0.5f);
int xpxl1 = static_cast<int>(xend);
int ypxl1 = ipart(yend);
// Add the first endpoint
plot(va, xpxl1, ypxl1, rfpart(yend) * xgap, color);
plot(va, xpxl1, ypxl1 + 1, fpart(yend) * xgap, color);
float intery = yend + gradient;
xend = static_cast<float>(round(x2));
yend = y2 + gradient * (xend - x2);
xgap = fpart(x2 + 0.5f);
int xpxl2 = static_cast<int>(xend);
int ypxl2 = ipart(yend);
// Add the second endpoint
plot(va, xpxl2, ypxl2, rfpart(yend) * xgap, color);
plot(va, xpxl2, ypxl2 + 1, fpart(yend) * xgap, color);
// Add all the points between the endpoints
for(int x = xpxl1 + 1; x <= xpxl2 - 1; ++x)
{
plot(va, x, ipart(intery), rfpart(intery), color);
plot(va, x, ipart(intery) + 1, fpart(intery), color);
intery += gradient;
}
}
else
{
if(y2 < y1)
{
std::swap(x1, x2);
std::swap(y1, y2);
}
float gradient = dx / dy;
float yend = static_cast<float>(round(y1));
float xend = x1 + gradient * (yend - y1);
float ygap = rfpart(y1 + 0.5f);
int ypxl1 = static_cast<int>(yend);
int xpxl1 = ipart(xend);
// Add the first endpoint
plot(va, xpxl1, ypxl1, rfpart(xend) * ygap, color);
plot(va, xpxl1, ypxl1 + 1, fpart(xend) * ygap, color);
float interx = xend + gradient;
yend = static_cast<float>(round(y2));
xend = x2 + gradient * (yend - y2);
ygap = fpart(y2 + 0.5f);
int ypxl2 = static_cast<int>(yend);
int xpxl2 = ipart(xend);
// Add the second endpoint
plot(va, xpxl2, ypxl2, rfpart(xend) * ygap, color);
plot(va, xpxl2, ypxl2 + 1, fpart(xend) * ygap, color);
// Add all the points between the endpoints
for(int y = ypxl1 + 1; y <= ypxl2 - 1; ++y)
{
plot(va, ipart(interx), y, rfpart(interx), color);
plot(va, ipart(interx) + 1, y, fpart(interx), color);
interx += gradient;
}
}
target.draw(va);
}
void MapGenerator::processScanPoint(double start_x, double start_y, double angle, double distance) {
// anti-aliased ray tracing, taken from:
// http://en.wikipedia.org/wiki/Xiaolin_Wu%27s_line_algorithm
double x0 = (start_x - x_min) / map_resolution;
double y0 = (start_y - y_min) / map_resolution;
// account for robot itself
for (int i = -2; i <= 2; i++) {
for (int j = -2; j <= 2; j++) {
updateOdds(x0 + i, y0 + j, start_x, start_y, distance);
}
}
double end_x = start_x + range_max * cos(angle);
double end_y = start_y + range_max * sin(angle);
double x1 = (end_x - x_min) / map_resolution;
double y1 = (end_y - y_min) / map_resolution;
double dx = x1 - x0;
double dy = y1 - y0;
bool steep = false;
if (fabs(dx) < fabs(dy)) {
swap(&x0, &y0);
swap(&x1, &y1);
swap(&dx, &dy);
steep = true;
}
if (x1 < x0) {
swap(&x0, &x1);
swap(&y0, &y1);
}
double gradient = dy / dx;
double xend = round(x0);
double yend = y0 + gradient * (xend - x0);
double xgap = rfpart(x0 + 0.5);
int xpxl1 = (int) xend;
int ypxl1 = (int) yend;
if (steep) {
updateOdds(ypxl1, xpxl1, start_x, start_y, distance);
updateOdds(ypxl1+1, xpxl1, start_x, start_y, distance);
} else {
updateOdds(xpxl1, ypxl1, start_x, start_y, distance);
updateOdds(xpxl1, ypxl1+1, start_x, start_y, distance);
}
double intery = yend + gradient;
xend = round(x1);
yend = y1 + gradient * (xend - x1);
xgap = fpart(x1 + 0.5);
int xpxl2 = (int) xend;
int ypxl2 = (int) yend;
// plot xpxl2, ypxl2, rfpart(yend) * xgap
// plot xpxl2, ypxl2 + 1, fpart(yend) * xgap
// main loop
for (int x = xpxl1 + 1; x <= xpxl2 - 1; x++) {
// plot x,
if (steep) {
updateOdds((int) intery, x, start_x, start_y, distance);
updateOdds((int) (intery+1), x, start_x, start_y, distance);
} else {
updateOdds(x, (int) intery, start_x, start_y, distance);
updateOdds(x, (int) (intery+1), start_x, start_y, distance);
}
intery += gradient;
}
}
void muiBBWindow::DrawLineFloat(float x0, float y0, float x1, float y1, muiColor clr)
// Xiaolin Wu's line algorithm
{
if (!pclrBitmap) return;
int ix, iy, ie;
float dx = x1 - x0, x;
float dy = y1 - y0, y;
float c, v, gradient, xend, yend, gap;
if (fabsf(dx) > fabsf(dy)) {
// Handle "horizontal" lines
if (x1 < x0) {
swap(x0, x1);
swap(y0, y1);
}
gradient = dy / dx;
// Handle 1st endpoint
xend = fround(x0);
yend = y0 + gradient * (xend - x0);
gap = rfpart(x0 + 0.5f);
ix = int(floorf(xend));
iy = int(floorf(yend));
c = fpart(yend);
DrawPixelBlend({ ix, iy++ }, clr, (1.0f - c) * gap);
DrawPixelBlend({ ix++, iy }, clr, (c)* gap);
y = yend + gradient; // first y-intersection for the main loop
// Handle 2nd endpoint
xend = fround(x1);
yend = y1 + gradient * (xend - x1);
gap = fpart(x1 + 0.5f);
ie = int(ipart(xend));
iy = int(ipart(yend));
c = fpart(yend);
DrawPixelBlend({ ie, iy++ }, clr, (1.0f - c) * gap);
DrawPixelBlend({ ie--, iy }, clr, (c)* gap);
// Handle mid-points
while (ix <= ie) {
v = ipart(y);
iy = int(v);
c = y - v;
y += gradient;
DrawPixelBlend({ ix, iy++ }, clr, (1.0f - c));
DrawPixelBlend({ ix++, iy }, clr, (c));
}
} else {
// Handle "vertical" lines
if (y1 < y0) {
swap(y0, y1);
swap(x0, x1);
}
gradient = dx / dy;
// Handle 1st endpoint
yend = fround(y0);
xend = x0 + gradient * (yend - y0);
gap = rfpart(y0 + 0.5f);
iy = int(ipart(yend));
ix = int(ipart(xend));
c = fpart(xend);
DrawPixelBlend({ ix++, iy }, clr, (1.0f - c) * gap);
DrawPixelBlend({ ix, iy++ }, clr, (c)* gap);
x = xend + gradient; // first x-intersection for the main loop
// Handle 2nd endpoint
yend = fround(y1);
xend = x1 + gradient * (yend - y1);
gap = fpart(y1 + 0.5f);
ie = int(ipart(yend));
ix = int(ipart(xend));
c = fpart(xend);
DrawPixelBlend({ ix++, ie }, clr, (1.0f - c) * gap);
DrawPixelBlend({ ix, ie-- }, clr, (c)* gap);
// Handle mid-points
while (iy <= ie) {
v = ipart(x);
ix = int(v);
c = x - v;
x += gradient;
DrawPixelBlend({ ix++, iy }, clr, (1.0f - c));
DrawPixelBlend({ ix, iy++ }, clr, (c));
}
}
}
// Draw antialiased line using Wu's algorithm.
void Line::lineWu(int x0, int y0, int x1, int y1, std::vector<Point>& points, std::vector<float>& alpha)
{
bool bSteep = abs(int(y1 - y0)) > abs(int(x1 - x0));
if(bSteep)
{
std::swap(x0, y0);
std::swap(x1, y1);
}
if(x0 > x1)
{
std::swap(x0, x1);
std::swap(y0, y1);
}
float dx = float(x1 - x0);
float dy = float(y1 - y0);
float gradient = float(dy) / dx;
// handle first endpoint
uint xend = round(float(x0));
float yend = y0 + gradient * (xend - x0);
float xgap = rfpart(x0 + 0.5f);
uint xpxl1 = xend; //this will be used in the main loop
uint ypxl1 = ipart(yend);
if(bSteep)
{
points.push_back(Point(ypxl1, xpxl1));
alpha.push_back(rfpart(yend) * xgap);
points.push_back(Point(ypxl1+1, xpxl1));
alpha.push_back(fpart(yend) * xgap);
}
else
{
points.push_back(Point(xpxl1, ypxl1));
alpha.push_back(rfpart(yend) * xgap);
points.push_back(Point(xpxl1, ypxl1+1));
alpha.push_back(fpart(yend) * xgap);
}
float intery = yend + gradient; // first y-intersection for the main loop
// handle second endpoint
xend = round(float(x1));
yend = y1 + gradient * (xend - x1);
xgap = fpart(x1 + 0.5f);
uint xpxl2 = xend; //this will be used in the main loop
uint ypxl2 = ipart(yend);
if(bSteep)
{
points.push_back(Point(ypxl2, xpxl2));
alpha.push_back(rfpart(yend) * xgap);
points.push_back(Point(ypxl2+1, xpxl2));
alpha.push_back(fpart(yend) * xgap);
}
else
{
points.push_back(Point(xpxl2, ypxl2));
alpha.push_back(rfpart(yend) * xgap);
points.push_back(Point(xpxl2, ypxl2+1));
alpha.push_back(fpart(yend) * xgap);
}
// main loop
for(uint x = xpxl1 + 1; x < xpxl2; ++x)
{
if(bSteep)
{
points.push_back(Point(ipart(intery), x));
alpha.push_back(rfpart(intery));
points.push_back(Point(ipart(intery)+1, x));
alpha.push_back(fpart(intery));
}
else
{
points.push_back(Point(x, ipart(intery)));
alpha.push_back(rfpart(intery));
points.push_back(Point(x, ipart(intery)+1));
alpha.push_back(fpart(intery));
}
intery = intery + gradient;
}
}
