// Only drawn in 2d at the moment void LocalizationGL::drawRelativeObjects(WorldObjectBlock* worldObjects, RobotStateBlock* robotState) { if (worldObjects == NULL || robotState == NULL){ //cout << "no wo or robotstate to draw seen objects" << endl; return; } WorldObject* wo; WorldObject* self = &(worldObjects->objects_[robotState->WO_SELF]); for (int i = 0; i < NUM_WORLD_OBJS; i++){ wo = &(worldObjects->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->isGoal() && wo->isGoalPost()) { drawObservationLine(start,end,basicGL.yellowRGB); objectsGL.drawYellowPost(obsLocFd,0.25); } else if (wo->isGoal() && !wo->isGoalPost()) { drawObservationLine(start,end,basicGL.yellowRGB); objectsGL.drawYellowGoal(obsLocFd,0.25); } else if (wo->isUnknownIntersection()) { end.z=0.0; drawObservationLine(start,end,basicGL.pinkRGB); objectsGL.drawIntersection(obsLocFd,0.5); } else if (wo->isIntersection() && !wo->isUnknownIntersection()){ end.z=0.0; drawObservationLine(start,end,basicGL.whiteRGB); objectsGL.drawIntersection(obsLocFd,0.5); } else if (wo->isLine() || wo->isUnknownLine()) { end.z=0.0; drawObservationLine(start,end,basicGL.whiteRGB); objectsGL.drawLinePoint(obsLocFd,0.5); //draw the line segment // unknown - black if (wo->isUnknownLine()) basicGL.colorRGBAlpha(basicGL.blackRGB,1.0); // known - red else basicGL.colorRGBAlpha(basicGL.redRGB,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->isBall()) { end.z=BALL_RADIUS; drawObservationLine(start,end,basicGL.orangeRGB); objectsGL.drawBall(obsLocFd,0.5); } else if (wo->isCenterCircle()){ // draw center cirlce end.z = 0.0; drawObservationLine(start,end,basicGL.whiteRGB); objectsGL.drawCenterCircle(obsLocFd,0.5); } else if (wo->isUnknownPenaltyCross() || wo->isKnownPenaltyCross()){ // draw penalty cross end.z = 0.0; drawObservationLine(start,end,basicGL.whiteRGB); objectsGL.drawPenaltyCross(obsLocFd,0.5); } } } }