void getInputSegments(){
segments.push_back(Line(20, 55, 60, 55));
segments.push_back(Line(20, 5, 20, 55)); //TODO: real input
segments.push_back(Line(60, 5, 20, 5));
segments.push_back(Line(60, 55, 60, 5));
//segments.push_back(Line(20, 5, 40, 63));
planner = PathPlanner(segments);
planner.populateTrajectory();
StateTrajectory* traj = planner.getPathTrajectory();
for (int i = 0; i < traj->size(); i++){
cout << traj->at(i).toString() << endl;
}
}
void QSegmentWidget::clearSegment(int cyl, qreal startAngle, qreal stopAngle)
{
Segment s(startAngle, stopAngle, QColor(Qt::black));
SegmentList sl;
sl.push_back(s);
clearSegments(cyl, sl);
}
static void
bufferLinesToLines( const GeoShape& input, double b, GeoShape& output )
{
// buffering lines turns them into polygons
for( GeoPartList::const_iterator i = input.getParts().begin(); i != input.getParts().end(); i++ )
{
const GeoPointList& part = *i;
if ( part.size() < 2 ) continue;
GeoPointList new_part;
// collect all the shifted segments:
SegmentList segments;
for( GeoPointList::const_iterator j = part.begin(); j != part.end()-1; j++ )
{
const osg::Vec3d& p0 = *j;
const osg::Vec3d& p1 = *(j+1);
osg::Vec3d d = p1-p0; d.normalize();
osg::Vec3d b0( p0.x() + b*d.y(), p0.y() - b*d.x(), p1.z() );
osg::Vec3d b1( p1.x() + b*d.y(), p1.y() - b*d.x(), p1.z() );
segments.push_back( Segment( b0, b1 ) );
}
// then intersect each pair of shifted segments to find the new verts:
for( SegmentList::iterator k = segments.begin(); k != segments.end()-1; k++ )
{
Segment& s0 = *k;
Segment& s1 = *(k+1); //(k+1) != segments.end()? *(k+1) : *segments.begin();
if ( k == segments.begin() )
{
GeoPoint first( s0.p0, part[0].getSRS() );
first.setDim( part[0].getDim() );
new_part.push_back( first );
}
osg::Vec3d isect;
if ( getLineIntersection( s0, s1, isect ) )
{
GeoPoint r( isect, part[0].getSRS() );
r.setDim( part[0].getDim() );
new_part.push_back( r );
}
if ( k == segments.end()-2 )
{
GeoPoint last( s1.p1, part[0].getSRS() );
last.setDim( part[0].getDim() );
new_part.push_back( last );
}
}
if ( new_part.size() > 1 )
output.getParts().push_back( new_part );
}
}
void QSegmentWidget::addSegment(int cyl, qreal startAngle, qreal stopAngle, QColor color)
{
// Segment s(startAngle, stopAngle, color);
QSegmentWidget::Segment s;
s.first = startAngle;
s.second = stopAngle;
s.color = color;
SegmentList sl;
sl.push_back(s);
addSegments(cyl, sl);
}
SegmentList SegmentList::clone() const
{
SegmentList dl;
Segment* s = _first;
for (int i = 0; i < _size; ++i) {
Segment* ns = s->clone();
dl.push_back(ns);
s = s->next();
}
dl.check();
return dl;
}
static void
bufferPolygons( const GeoShape& shape, double b, GeoPartList& output )
{
for( GeoPartList::const_iterator i = shape.getParts().begin(); i != shape.getParts().end(); i++ )
{
const GeoPointList& part = *i;
if ( part.size() < 3 )
continue;
GeoPointList new_part;
// first build the buffered line segments:
SegmentList segments;
for( GeoPointList::const_iterator j = part.begin(); j != part.end(); j++ )
{
const osg::Vec3d& p0 = *j;
const osg::Vec3d& p1 = (j+1) != part.end()? *(j+1) : *part.begin();
osg::Vec3d d = p1-p0;
d.normalize();
osg::Vec3d b0( p0.x() + b*d.y(), p0.y() - b*d.x(), p1.z() );
osg::Vec3d b1( p1.x() + b*d.y(), p1.y() - b*d.x(), p1.z() );
segments.push_back( Segment( b0, b1 ) );
}
// then intersect each pair of segments to find the new verts:
for( SegmentList::iterator k = segments.begin(); k != segments.end(); k++ )
{
Segment& s0 = *k;
Segment& s1 = (k+1) != segments.end()? *(k+1) : *segments.begin();
osg::Vec3d isect;
if ( getLineIntersection( s0, s1, isect ) )
{
GeoPoint r( isect, part[0].getSRS() );
r.setDim( part[0].getDim() );
new_part.push_back( r );
}
}
if ( new_part.size() > 2 )
output.push_back( new_part );
}
}
BlobImageProcessorYUV::SegmentList BlobImageProcessorYUV::getSegments(int y, /*const*/ Image &image) const {
#else
BlobImageProcessorYUV::SegmentList BlobImageProcessorYUV::getSegments(int y, const Image &image) const {
#endif
SegmentList segments;
Segment cursegment = { -1, -1 };
int gapcount = 0;
int x;
for (x=0; x<image.getWidth(); x++) {
Pixel pixel = image.getPixel(x, y);
if (checkModels(pixel)) { // if the pixel is valid
#ifdef ENABLE_BLUEMARKER
if(enable_bluemarker)
{
// Added by Jeremy
//Pixel *bluemarker = new Pixel(0, 0, 255);
Pixel *bluemarker = new Pixel(29, 255, 107); // YUV for blue
image.setPixel(x, y, *bluemarker); // Mark pixel as bright blue for a match
delete(bluemarker);
}
#endif
if (cursegment.start == -1) { // if its the first pixel in a row
cursegment.start = x; // start a new segment
if (debug)
cout << "New segment starting at (" << x << "," << y << ")" << endl;
}
gapcount = 0; // any valid pixel resets the gap counter
} else if (cursegment.start >= 0) { // pixel not valid, but we're in a row
if (++gapcount > maxgapdist) { // if we've exceeded the maximum gap count
cursegment.end = x - maxgapdist; // set the end of the segment
if (cursegment.end - cursegment.start >= minsegmentsize) { // if the segment is large enough
segments.push_back(cursegment); // save it
if (debug)
cout << "segment saved, ends at " << cursegment.end << endl;
} else {
if (debug)
cout << "segment rejected due to width (" << cursegment.end - cursegment.start << ")" << endl;
}
cursegment.start = -1;
}
}
}
// if a segment goes off the side of the image, but is wide enough
if (cursegment.start > 0) {
if (image.getWidth() - cursegment.start >= minsegmentsize) {
cursegment.end = image.getWidth()-1-gapcount;
segments.push_back(cursegment);
if (debug)
cout << "Saving segment, hit edge of screen" << endl;
} else {
if (debug)
cout << "Segment hit edge of screen, rejected due to width (" << cursegment.end - cursegment.start << "(" << endl;
}
}
return segments;
}
void initializeDefaultSegments(){
segments.push_back(Line(20, 55, 60, 55));
segments.push_back(Line(20, 5, 20, 55)); //TODO: real input
segments.push_back(Line(60, 5, 20, 5));
segments.push_back(Line(60, 55, 60, 5));
}
static void
bufferLinesToPolygons( const GeoShape& input, double b, GeoShape& output )
{
// buffering lines turns them into polygons
for( GeoPartList::const_iterator i = input.getParts().begin(); i != input.getParts().end(); i++ )
{
const GeoPointList& part = *i;
if ( part.size() < 2 ) continue;
GeoPointList new_part;
// collect segments in one direction and then the other.
SegmentList segments;
for( GeoPointList::const_iterator j = part.begin(); j != part.end()-1; j++ )
{
const osg::Vec3d& p0 = *j;
const osg::Vec3d& p1 = *(j+1);
osg::Vec3d d = p1-p0;
d.normalize();
osg::Vec3d b0( p0.x() + b*d.y(), p0.y() - b*d.x(), p1.z() );
osg::Vec3d b1( p1.x() + b*d.y(), p1.y() - b*d.x(), p1.z() );
segments.push_back( Segment( b0, b1 ) );
// after the last seg, add an end-cap:
if ( j == part.end()-2 )
{
osg::Vec3d b2( p1.x() - b*d.y(), p1.y() + b*d.x(), p1.z() );
segments.push_back( Segment( b1, b2 ) );
}
}
// now back the other way:
for( GeoPointList::const_reverse_iterator j = part.rbegin(); j != part.rend()-1; j++ )
{
const osg::Vec3d& p0 = *j;
const osg::Vec3d& p1 = *(j+1);
osg::Vec3d d = p1-p0;
d.normalize();
osg::Vec3d b0( p0.x() + b*d.y(), p0.y() - b*d.x(), p1.z() );
osg::Vec3d b1( p1.x() + b*d.y(), p1.y() - b*d.x(), p1.z() );
segments.push_back( Segment( b0, b1 ) );
// after the last seg, add an end-cap:
if ( j == part.rend()-2 )
{
osg::Vec3d b2( p1.x() - b*d.y(), p1.y() + b*d.x(), p1.z() );
segments.push_back( Segment( b1, b2 ) );
}
}
// then intersect each pair of segments to find the new verts:
for( SegmentList::iterator k = segments.begin(); k != segments.end(); k++ )
{
Segment& s0 = *k;
Segment& s1 = (k+1) != segments.end()? *(k+1) : *segments.begin();
osg::Vec3d isect;
if ( getLineIntersection( s0, s1, isect ) )
{
GeoPoint r( isect, part[0].getSRS() );
r.setDim( part[0].getDim() );
new_part.push_back( r );
}
}
if ( new_part.size() > 2 )
output.getParts().push_back( new_part );
}
}
