unsigned
PolygonToTriangles(const RasterPoint *points, unsigned num_points,
AllocatedArray<GLushort> &triangles, unsigned min_distance)
{
triangles.GrowDiscard(3 * (num_points - 2));
return _PolygonToTriangles(points, num_points, triangles.begin(),
min_distance);
}
FLARM::MessageType
FlarmDevice::WaitForACKOrNACK(uint16_t sequence_number,
AllocatedArray<uint8_t> &data, uint16_t &length,
OperationEnvironment &env, unsigned timeout_ms)
{
const TimeoutClock timeout(timeout_ms);
// Receive frames until timeout or expected frame found
while (!timeout.HasExpired()) {
// Wait until the next start byte comes around
if (!WaitForStartByte(env, timeout.GetRemainingOrZero()))
continue;
// Read the following FrameHeader
FLARM::FrameHeader header;
if (!ReceiveFrameHeader(header, env, timeout.GetRemainingOrZero()))
continue;
// Read and check length of the FrameHeader
length = header.GetLength();
if (length <= sizeof(header))
continue;
// Calculate payload length
length -= sizeof(header);
// Read payload and check length
data.GrowDiscard(length);
if (!ReceiveEscaped(data.begin(), length,
env, timeout.GetRemainingOrZero()))
continue;
// Verify CRC
if (header.GetCRC() != FLARM::CalculateCRC(header, data.begin(), length))
continue;
// Check message type
if (header.type != FLARM::MT_ACK && header.type != FLARM::MT_NACK)
continue;
// Check payload length
if (length < 2)
continue;
// Check whether the received ACK is for the right sequence number
if (FromLE16(*((const uint16_t *)(const void *)data.begin())) ==
sequence_number)
return (FLARM::MessageType)header.type;
}
return FLARM::MT_ERROR;
}
unsigned
LineToTriangles(const PT *points, unsigned num_points,
AllocatedArray<PT> &strip,
unsigned line_width, bool loop, bool tcap)
{
// A line has to have at least two points
if (num_points < 2)
return 0;
// allocate memory for triangle vertices
// max. size: 2*(num_points + (int)(loop || tcap))
strip.GrowDiscard(2 * (num_points + 1));
// A closed line path needs to have at least three points
if (loop && num_points < 3)
// .. otherwise don't close it
loop = false;
float half_line_width = line_width * 0.5f;
// strip will point to the start of the output array
// s is the working pointer
PT *s = strip.begin();
// a, b and c point to three consecutive points which are used to iterate
// through the line given in 'points'. Where b is the current position,
// a the previous point and c the next point.
const PT *a, *b, *c;
// pointer to the end of the original points array
// used for faster loop conditions
const auto points_end = points + num_points;
// initialize a, b and c vertices
if (loop) {
b = points + num_points - 1;
a = b - 1;
// skip identical points before b
while (a >= points && *a == *b)
a--;
if (a < points)
// all points in the array are identical
return 0;
c = points;
} else {
a = points;
b = a + 1;
// skip identical points after a
while (b != points_end && *a == *b)
b++;
if (b == points_end)
// all points in the array are identical
return 0;
c = b + 1;
}
// skip identical points after b
while (c != points_end && *b == *c)
c++;
if (!loop) {
// add flat or triangle cap at beginning of line
PT ba = *a - *b;
Normalize(&ba, half_line_width);
if (tcap)
// add triangle cap coordinate to the output array
AppendPoint(s, a->x + ba.x, a->y + ba.y);
// add flat cap coordinates to the output array
PT p;
p.x = ba.y;
p.y = -ba.x;
AppendPoint(s, a->x - p.x, a->y - p.y);
AppendPoint(s, a->x + p.x, a->y + p.y);
}
// add points by calculating the angle bisector of ab and bc
int sign = 1;
if (num_points >= 3) {
while (c != points_end) {
// skip zero or 180 degree bends
// TODO: support 180 degree bends!
if (!TriangleEmpty(*a, *b, *c)) {
PT g = *b - *a, h = *c - *b;
Normalize(&g, 1000.);
Normalize(&h, 1000.);
typename PT::product_type bisector_x = -g.y - h.y;
typename PT::product_type bisector_y = g.x + h.x;
float projected_length = (-g.y * bisector_x + g.x * bisector_y) *
(1.f / 1000.f);
if (projected_length < 400.f) {
// acute angle, use the normal of the bisector instead
projected_length = (g.x * bisector_x + g.y * bisector_y) *
(1.f / 1000.f);
std::swap(bisector_x, bisector_y);
bisector_y *= -1;
// the order of the triangles switches. keep track with 'sign'
sign *= -1;
}
float scale = half_line_width / projected_length;
if(std::is_integral<typename PT::product_type>::value) {
bisector_x = sign * (typename PT::product_type) lround(bisector_x * scale);
bisector_y = sign * (typename PT::product_type) lround(bisector_y * scale);
} else {
bisector_x = sign * bisector_x * scale;
bisector_y = sign * bisector_y * scale;
}
AppendPoint(s, b->x - bisector_x, b->y - bisector_y);
AppendPoint(s, b->x + bisector_x, b->y + bisector_y);
}
a = b;
b = c;
c++;
while (c != points_end && *b == *c)
// skip identical points
c++;
}
}
if (loop) {
// repeat first two points at the end
if (sign == 1) {
AppendPoint(s, strip[0].x, strip[0].y);
AppendPoint(s, strip[1].x, strip[1].y);
} else {
AppendPoint(s, strip[1].x, strip[1].y);
AppendPoint(s, strip[0].x, strip[0].y);
}
} else {
// add flat or triangle cap at end of line
PT ab = *b - *a;
Normalize(&ab, half_line_width);
PT p;
p.x = sign * -ab.y;
p.y = sign * ab.x;
AppendPoint(s, b->x - p.x, b->y - p.y);
AppendPoint(s, b->x + p.x, b->y + p.y);
if (tcap)
AppendPoint(s, b->x + ab.x, b->y + ab.y);
}
return s - strip.begin();
}
void Begin(unsigned n) {
assert(num_points == 0);
points.GrowDiscard(((n - 1) | 0x3ff) + 1);
}
