Radar.cpp

00001 //
00002 // Radar.h - example of graphic commands usage to create 2D graphics.
00003 //
00004 
00005 #include "Radar.h"
00006 #include "math.h"
00007 #include <cstdlib>
00008 
00009 Radar::Radar(Display* display)
00010 {
00011     _display = display;
00012 
00013     Initialize();
00014 }
00015 
00016 Radar::~Radar()
00017 {
00018     for (std::list<Target>::iterator it = targetsList.begin(); it != targetsList.end(); ++it) {
00019         Target& target = *it;
00020         it = targetsList.erase(it);
00021     }
00022     targetsList.clear();
00023 }
00024 
00025 
00026 void Radar::Initialize()
00027 {
00028     int margin = 4;
00029 
00030     // Set the size of display window
00031     window.y1 = margin;
00032     window.x1 = margin;
00033 
00034     window.y2 = _display->DisplayHeight() - 1 - margin;
00035     window.x2 = _display->DisplayWidth() - 1 - margin;
00036 
00037     // Set default center position
00038     SetCenter((window.x2 - window.x1) / 2 + window.x1, (window.y2 - window.y1) / 2 + window.y1);
00039 
00040     // Set default range
00041     SetRange(MAX_RANGE_INDEX);
00042 }
00043 
00044 
00045 void Radar::DrawTracks()
00046 {
00047     // Draw every track on the list if in range
00048     for (std::list<Target>::iterator it = targetsList.begin(); it != targetsList.end(); ++it) {
00049         Target& target = *it;
00050 
00051         if (target.GetDistance() <= GetMaxRange()) {
00052             DrawTarget(&target);
00053         }
00054     }
00055 }
00056 
00057 
00058 void Radar::DrawMarkers()
00059 {
00060     // Range markers
00061     //
00062     int localRange = (window.y2 - window.y1) / 2;
00063     float scale = localRange / GetRange();
00064 
00065     // First distance marker in local coordinates.
00066     int maxRadius = floor( 0.5f + GetMaxRange() * scale);
00067     int minRadius = floor( 0.5f + _rangeMarkersDelta[_rangeNumber] * scale);
00068     float radius = minRadius;
00069     while (radius <= maxRadius + 1) {
00070         _display->DrawCircle(_centerX, _centerY, floor( 0.5f + radius));
00071         radius += _rangeMarkersDelta[_rangeNumber] * scale;
00072     }
00073     _display->DrawCircle(_centerX, _centerY, maxRadius + 4);
00074 
00075     // DrawPoint in the center
00076     _display->DrawPoint(_centerX, _centerY, _display->GetDrawColor());
00077 
00078     // Azimuth markers, use the first and last marker radius calculated above.
00079     //
00080     double angle = 0;
00081     double angleDelta = M_PI / (double)azimuthMarkersCount;
00082     for (int i = 0; i<azimuthMarkersCount; i++) {
00083         float x = sin(angle);
00084         float y = cos(angle);
00085         _display->DrawLine(_centerX - x * minRadius, _centerY - y * minRadius, _centerX - x * maxRadius, _centerY - y * maxRadius);
00086         _display->DrawLine(_centerX + x * minRadius, _centerY + y * minRadius, _centerX + x * maxRadius, _centerY + y * maxRadius);
00087         angle += angleDelta;
00088     }
00089 }
00090 
00091 
00092 void Radar::DrawTarget(Target * target)
00093 {
00094     const int radius = 6;
00095     uint32_t colorMask = 0xFF00FF00;
00096     _display->SetDrawColor((colorMask & 0x00FF0000) >> 16, (colorMask & 0x0000FF00) >> 8, (colorMask & 0x000000FF), (colorMask & 0xFF000000) >> 24);
00097 
00098     // Convert world location to local coordinates
00099     int x = _centerX + target->GetX() * _scaleRatio;
00100     int y = _centerY - target->GetY() * _scaleRatio;
00101 
00102     // Draw track
00103     //_display->DrawLine(x-radius, y, x+radius, y);
00104     _display->DrawPoint(x - radius, y, colorMask);
00105     _display->DrawPoint(x + radius, y, colorMask);
00106     for (int i = 1; i < radius; i++) {
00107         //_display->DrawLine(x - radius + i, y-i, x + radius - i, y-i);
00108         _display->DrawPoint(x - radius + i, y - i, colorMask);
00109         _display->DrawPoint(x + radius - i, y - i, colorMask);
00110         //_display->DrawLine(x - radius + i, y+i, x + radius - i, y+i);
00111         _display->DrawPoint(x - radius + i, y + i, colorMask);
00112         _display->DrawPoint(x + radius - i, y + i, colorMask);
00113     }
00114     _display->DrawPoint(x, y + radius, colorMask);
00115     _display->DrawPoint(x, y - radius, colorMask);
00116 
00117     // Draw vector
00118     const float maxSpeed = 800; // [km/h]
00119     float maxVectorLen = 20;
00120     float vectorLen = target->GetSpeed() * maxVectorLen / maxSpeed;
00121 
00122     // Convert world location to local coordinates
00123     int x1 = x + sin(target->GetDirection()) * vectorLen;
00124     int y1 = y - cos(target->GetDirection()) * vectorLen;
00125     _display->DrawLine(x, y, x1, y1);
00126 }
00127 
00128 
00129 void Radar::DrawPlot(Location* plot)
00130 {
00131     const int size = 5;
00132 
00133     // Convert world location to local coordinates
00134     int x = _centerX + plot->GetX() * _scaleRatio;
00135     int y = _centerY - plot->GetY() * _scaleRatio;
00136 
00137     // Draw plot
00138     _display->DrawLine(x - size / 2, y, x + size / 2, y);
00139     _display->DrawLine(x, y - size / 2, x, y + size / 2);
00140 }
00141 
00142 
00143 bool Radar::UpdateTargetsLocation(float startAngle, float endAngle, uint32_t currentTime)
00144 {
00145     int count = 0;
00146     for (std::list<Target>::iterator it = targetsList.begin(); it != targetsList.end(); ++it) {
00147         Target& target = *it;
00148 
00149         if( startAngle < endAngle ) {
00150             if (target.GetAzimuth() > startAngle && target.GetAzimuth() <= endAngle) {
00151                 count++;
00152                 target.UpdateLocationForTime(currentTime);
00153             }
00154         } else {
00155             if (target.GetAzimuth() > startAngle || target.GetAzimuth() <= endAngle) {
00156                 count++;
00157                 target.UpdateLocationForTime(currentTime);
00158             }
00159         }
00160     }
00161 
00162     return count > 0;
00163 }
00164 
00165 
00166 void Radar::DrawBorder()
00167 {
00168     // Set display window coordinates
00169     int x1 = 0;
00170     int y1 = 0;
00171     int x2 = _display->DisplayWidth() - 1;
00172     int y2 = _display->DisplayHeight() - 1;
00173 
00174     _display->DrawRectangle(x1, y1, x2, y2);
00175     _display->DrawRectangle(window.x1, window.y1, window.x2, window.y2);
00176 
00177     float maxLen = 3;
00178     float delta = M_PI / (8 * 5);
00179 
00180     // top scale, y = 0;
00181     float angle = -M_PI / 2;
00182     while ((angle += delta) < M_PI / 2) {
00183         int yStart = y1;
00184         int xStart = _centerX + (yStart - _centerY) * tan(angle);
00185 
00186         if (xStart >= window.x1 && xStart <= window.x2) {
00187             int yEnd = yStart + maxLen;
00188             int xEnd = _centerX + (yEnd - _centerY) * tan(angle);
00189 
00190             _display->DrawLine(xStart, yStart, xEnd, yEnd);
00191         }
00192     }
00193 
00194     // bottom scale, y = y2;
00195     angle = M_PI / 2;
00196     while ((angle += delta) < 3 * M_PI / 2) {
00197         int yStart = y2;
00198         int xStart = _centerX + (yStart - _centerY) * tan(angle);
00199 
00200         if (xStart >= window.x1 && xStart <= window.x2) {
00201             int yEnd = yStart - maxLen;
00202             int xEnd = _centerX + (yEnd - _centerY) * tan(angle);
00203 
00204             _display->DrawLine(xStart, yStart, xEnd, yEnd);
00205         }
00206     }
00207 
00208     // left scale, x = 0;
00209     angle = -M_PI / 2;
00210     while ((angle += delta) < M_PI / 2) {
00211         int xStart = x1;
00212         int yStart = _centerY + (xStart - _centerX) * tan(angle);
00213 
00214         if (yStart >= window.y1 && yStart <= window.y2) {
00215             int xEnd = xStart + maxLen;
00216             int yEnd = _centerY + (xEnd - _centerX) * tan(angle);
00217 
00218             _display->DrawLine(xStart, yStart, xEnd, yEnd);
00219         }
00220     }
00221 
00222     // right scale, x = x2;
00223     angle = M_PI / 2;
00224     while ((angle += delta) < 3 * M_PI / 2) {
00225         int xStart = x2;
00226         int yStart = _centerY + (xStart - _centerX) * tan(angle);
00227 
00228         if (yStart >= window.y1 && yStart <= window.y2) {
00229             int xEnd = xStart - maxLen;
00230             int yEnd = _centerY + (xEnd - _centerX) * tan(angle);
00231 
00232             _display->DrawLine(xStart, yStart, xEnd, yEnd);
00233         }
00234     }
00235 }
00236 
00237 
00238 void Radar::DrawRadarBeam(float azimuth)
00239 {
00240     // Maximum radius length i local coordinates
00241     int maxRadius = floor( 0.5f + GetMaxRange() * _scaleRatio);
00242 
00243     int endX = GetCenterX() + maxRadius * sin(azimuth);
00244     int endY = GetCenterY() - maxRadius * cos(azimuth);
00245 
00246     _display->SetDrawColor(0xFF, 0xFF, 0xFF, 0xFF);
00247     _display->DrawLine(_centerX, _centerY, endX, endY);
00248 }
00249 
00250 Display* Radar::GetDisplay()
00251 {
00252     return _display;
00253 }
00254 
00255 void Radar::DrawRangeButtons(uint16_t raSize)
00256 {
00257     const uint16_t signSize = 5;
00258     uint16_t centerX;
00259     uint16_t centerY;
00260 
00261     // Increase buton on right top
00262     if(_rangeNumber < MAX_RANGE_INDEX) {
00263         centerX = window.x2-raSize/2;
00264         centerY = raSize/2;
00265         
00266         _display->DrawRectangle(window.x2-raSize, window.y1, window.x2, raSize);
00267         _display->DrawLine(centerX-signSize, centerY, centerX+signSize, centerY);
00268     }
00269 
00270     // Decrease button on right bottom
00271     if(_rangeNumber > 0) {
00272         centerX = window.x2-raSize/2;
00273         centerY = window.y2-raSize/2;
00274         
00275         _display->DrawRectangle(window.x2-raSize, window.y2-raSize, window.x2, window.y2);
00276         _display->DrawLine(centerX-signSize, centerY, centerX+signSize, centerY);
00277         _display->DrawLine(centerX, centerY-signSize, centerX, centerY+signSize);
00278     }
00279 }
00280 
00281 
00282 void Radar::SetRange(uint8_t rangeNumber)
00283 {
00284     if (rangeNumber <= MAX_RANGE_INDEX) {
00285         _rangeNumber = rangeNumber;
00286 
00287         int localRange = (window.y2 - window.y1) / 2;
00288         _scaleRatio = localRange / GetRange();
00289     }
00290 }
00291 
00292 
00293 bool Radar::ChangeRange(int change)
00294 {
00295     if (change > 0 && _rangeNumber < MAX_RANGE_INDEX) {
00296         SetRange( _rangeNumber + 1);
00297         return true;
00298     }
00299     if (change < 0 && _rangeNumber > 0) {
00300         SetRange( _rangeNumber - 1);
00301         return true;
00302     }
00303     return false;
00304 }
00305 
00306 
00307 int Radar::_centerX;
00308 int Radar::_centerY;
00309 uint8_t Radar::_rangeNumber;
00310 float Radar::_scaleRatio;