// Only drawn in 2d at the moment
void LocalizationGL::drawRelativeObjects(WorldObjectBlock* gtObjects, WorldObjectBlock* beliefObjects, RobotStateBlock* robotState) {
if (beliefObjects == NULL || robotState == NULL){
//cout << "no wo or robotstate to draw seen objects" << endl;
return;
}
WorldObject* wo;
WorldObject* self = &(gtObjects->objects_[robotState->WO_SELF]);
for (int i = 0; i < NUM_WORLD_OBJS; i++){
wo = &(beliefObjects->objects_[i]);
// if seen, get vision dist and bearing
if (wo->seen){
// use vision distance and bearing to figure out image loc
AngRad orient = (self->orientation + wo->visionBearing);
Point2D obsLocFd (wo->visionDistance, orient, POLAR);
obsLocFd += self->loc;
Vector3<float> start(self->loc.x, self->loc.y, 250);
Vector3<float> end(obsLocFd.x, obsLocFd.y,250);
if (wo->isOwnGoal() && wo->isGoalPost()) {
drawObservationLine(start,end,Colors::Blue);
objectsGL.drawPost(obsLocFd,0.25);
} else if (wo->isOwnGoal() && !wo->isGoalPost()) {
drawObservationLine(start,end,Colors::Blue);
objectsGL.drawGoal(obsLocFd,0.25);
} else if (wo->isBall()) {
drawObservationLine(start,end,Colors::Orange);
objectsGL.drawBall(obsLocFd,0.5);
} else if (wo->isBeacon()) {
vector<RGB> colors;
switch(wo->type) {
case WO_BEACON_BLUE_YELLOW: colors = { Colors::Blue, Colors::Yellow }; break;
case WO_BEACON_YELLOW_BLUE: colors = { Colors::Yellow, Colors::Blue }; break;
case WO_BEACON_BLUE_PINK: colors = { Colors::Blue, Colors::Pink }; break;
case WO_BEACON_PINK_BLUE: colors = { Colors::Pink, Colors::Blue }; break;
case WO_BEACON_PINK_YELLOW: colors = { Colors::Pink, Colors::Yellow }; break;
case WO_BEACON_YELLOW_PINK: colors = { Colors::Yellow, Colors::Pink }; break;
}
drawObservationLine(start,end,Colors::White);
objectsGL.drawBeacon(obsLocFd, colors[0], colors[1],0.25);
/*
} else if (wo->isIntersection() && !wo->isUnknownIntersection()){
drawObservationLine(start,end,Colors::White);
objectsGL.drawIntersection(obsLocFd,0.5);
} else if (wo->isLine() || wo->isUnknownLine()) {
Point2D closest = wo->visionLine.relativeToGlobal(self->loc, self->orientation).getPointOnSegmentClosestTo(self->loc);
drawObservationLine(start,Vector3<float>(closest.x, closest.y, 250),Colors::White);
objectsGL.drawLinePoint(closest,0.5);
//draw the line segment
// unknown - black
if (wo->isUnknownLine())
basicGL.colorRGBAlpha(Colors::Black,1.0);
// known - red
else
basicGL.colorRGBAlpha(Colors::Red,1.0);
Point2D sP=wo->visionPt1;
Point2D eP=wo->visionPt2;
sP=sP.relativeToGlobal(self->loc,self->orientation);
eP=eP.relativeToGlobal(self->loc,self->orientation);
basicGL.drawLine(sP,eP,2.0);
} else if (wo->isUnknownIntersection()) {
drawObservationLine(start,end,Colors::Pink);
objectsGL.drawIntersection(obsLocFd,0.5);
} else if (wo->isCenterCircle()){
// draw center cirlce
drawObservationLine(start,end,Colors::White);
objectsGL.drawCenterCircle(obsLocFd,0.5);
} else if (wo->isUnknownPenaltyCross() || wo->isKnownPenaltyCross()){
// draw penalty cross
drawObservationLine(start,end,Colors::White);
objectsGL.drawPenaltyCross(obsLocFd,0.5);
*/
}
}
}
}
