// 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);
}
}
}
}
