Diff: RadarDemo/Radar.cpp

Revision:

0:d8b9955d2b36

Child:

1:5e49b46de1b0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RadarDemo/Radar.cpp	Fri Nov 04 01:02:37 2016 +0000
@@ -0,0 +1,261 @@
+//
+// Radar.h - example of graphic commands usage to create 2D graphics.
+//
+
+#include "Radar.h"
+#include "math.h"
+#include <cstdlib>
+
+Radar::Radar(Display* display)
+{
+    _display = display;
+
+    Initialize();
+}
+
+Radar::~Radar()
+{
+    for (std::list<Target>::iterator it = targetsList.begin(); it != targetsList.end(); ++it) {
+        Target& target = *it;
+        it = targetsList.erase(it);
+    }
+    targetsList.clear();
+}
+
+
+void Radar::Initialize()
+{
+    int margin = 4;
+
+    // Set the size of display window
+    window.y1 = margin;
+    window.x1 = margin;
+
+    window.y2 = _display->DisplayHeight() - 1 - margin;
+    window.x2 = _display->DisplayWidth() - 1 - margin;
+
+    // Set default center position
+    SetCenter((window.x2 - window.x1) / 2 + window.x1, (window.y2 - window.y1) / 2 + window.y1);
+
+    // Set default range
+    SetRange(MAX_RANGE_INDEX);
+}
+
+
+void Radar::DrawTracks()
+{
+    // Draw every track on the list if in range
+    for (std::list<Target>::iterator it = targetsList.begin(); it != targetsList.end(); ++it) {
+        Target& target = *it;
+
+        if (target.GetDistance() <= GetMaxRange()) {
+            DrawTarget(&target);
+        }
+    }
+}
+
+
+void Radar::DrawMarkers()
+{
+    // Range markers
+    //
+    int localRange = (window.y2 - window.y1) / 2;
+    float scale = localRange / GetRange();
+
+    // First distance marker in local coordinates.
+    int maxRadius = floor( 0.5f + GetMaxRange() * scale);
+    int minRadius = floor( 0.5f + _rangeMarkersDelta[_rangeNumber] * scale);
+    float radius = minRadius;
+    while (radius <= maxRadius + 1) {
+        _display->DrawCircle(_centerX, _centerY, floor( 0.5f + radius));
+        radius += _rangeMarkersDelta[_rangeNumber] * scale;
+    }
+    _display->DrawCircle(_centerX, _centerY, maxRadius + 4);
+
+    // DrawPoint in the center
+    _display->DrawPoint(_centerX, _centerY, _display->GetDrawColor());
+
+    // Azimuth markers, use the first and last marker radius calculated above.
+    //
+    double angle = 0;
+    double angleDelta = M_PI / (double)azimuthMarkersCount;
+    for (int i = 0; i<azimuthMarkersCount; i++) {
+        float x = sin(angle);
+        float y = cos(angle);
+        _display->DrawLine(_centerX - x * minRadius, _centerY - y * minRadius, _centerX - x * maxRadius, _centerY - y * maxRadius);
+        _display->DrawLine(_centerX + x * minRadius, _centerY + y * minRadius, _centerX + x * maxRadius, _centerY + y * maxRadius);
+        angle += angleDelta;
+    }
+}
+
+
+void Radar::DrawTarget(Target * target)
+{
+    const int radius = 6;
+    uint32_t colorMask = 0xFF00FF00;
+    _display->SetDrawColor((colorMask & 0x00FF0000) >> 16, (colorMask & 0x0000FF00) >> 8, (colorMask & 0x000000FF), (colorMask & 0xFF000000) >> 24);
+
+    // Convert world location to local coordinates
+    int x = _centerX + target->GetX() * _scaleRatio;
+    int y = _centerY - target->GetY() * _scaleRatio;
+
+    // Draw track
+    //_display->DrawLine(x-radius, y, x+radius, y);
+    _display->DrawPoint(x - radius, y, colorMask);
+    _display->DrawPoint(x + radius, y, colorMask);
+    for (int i = 1; i < radius; i++) {
+        //_display->DrawLine(x - radius + i, y-i, x + radius - i, y-i);
+        _display->DrawPoint(x - radius + i, y - i, colorMask);
+        _display->DrawPoint(x + radius - i, y - i, colorMask);
+        //_display->DrawLine(x - radius + i, y+i, x + radius - i, y+i);
+        _display->DrawPoint(x - radius + i, y + i, colorMask);
+        _display->DrawPoint(x + radius - i, y + i, colorMask);
+    }
+    _display->DrawPoint(x, y + radius, colorMask);
+    _display->DrawPoint(x, y - radius, colorMask);
+
+    // Draw vector
+    const float maxSpeed = 800; // [km/h]
+    float maxVectorLen = 20;
+    float vectorLen = target->GetSpeed() * maxVectorLen / maxSpeed;
+
+    // Convert world location to local coordinates
+    int x1 = x + sin(target->GetDirection()) * vectorLen;
+    int y1 = y - cos(target->GetDirection()) * vectorLen;
+    _display->DrawLine(x, y, x1, y1);
+}
+
+
+void Radar::DrawPlot(Location* plot)
+{
+    const int size = 5;
+
+    // Convert world location to local coordinates
+    int x = _centerX + plot->GetX() * _scaleRatio;
+    int y = _centerY - plot->GetY() * _scaleRatio;
+
+    // Draw plot
+    _display->DrawLine(x - size / 2, y, x + size / 2, y);
+    _display->DrawLine(x, y - size / 2, x, y + size / 2);
+}
+
+
+void Radar::UpdateTargetsLocation(float startAngle, float endAngle, uint32_t currentTime)
+{
+    int count = 0;
+    for (std::list<Target>::iterator it = targetsList.begin(); it != targetsList.end(); ++it) {
+        Target& target = *it;
+
+        if( startAngle < endAngle ) {
+            if (target.GetAzimuth() > startAngle && target.GetAzimuth() <= endAngle) {
+                count++;
+                target.UpdateLocationForTime(currentTime);
+            }
+        } else {
+            if (target.GetAzimuth() > startAngle || target.GetAzimuth() <= endAngle) {
+                count++;
+                target.UpdateLocationForTime(currentTime);
+            }
+        }
+    }
+
+//    if(count > 0) {
+//        pc.printf("%d targets updated for angle (%3.1f, %3.1f>\r\n", count, startAngle * 180.f/M_PI, endAngle * 180.f/M_PI);
+//    }
+}
+
+
+void Radar::DrawBorder()
+{
+    // Set display window coordinates
+    int x1 = 0;
+    int y1 = 0;
+    int x2 = _display->DisplayWidth() - 1;
+    int y2 = _display->DisplayHeight() - 1;
+
+    _display->DrawRectangle(x1, y1, x2, y2);
+    _display->DrawRectangle(window.x1, window.y1, window.x2, window.y2);
+
+    float maxLen = 3;
+    float delta = M_PI / (8 * 5);
+
+    // top scale, y = 0;
+    float angle = -M_PI / 2;
+    while ((angle += delta) < M_PI / 2) {
+        int yStart = y1;
+        int xStart = _centerX + (yStart - _centerY) * tan(angle);
+
+        if (xStart >= window.x1 && xStart <= window.x2) {
+            int yEnd = yStart + maxLen;
+            int xEnd = _centerX + (yEnd - _centerY) * tan(angle);
+
+            _display->DrawLine(xStart, yStart, xEnd, yEnd);
+        }
+    }
+
+    // bottom scale, y = y2;
+    angle = M_PI / 2;
+    while ((angle += delta) < 3 * M_PI / 2) {
+        int yStart = y2;
+        int xStart = _centerX + (yStart - _centerY) * tan(angle);
+
+        if (xStart >= window.x1 && xStart <= window.x2) {
+            int yEnd = yStart - maxLen;
+            int xEnd = _centerX + (yEnd - _centerY) * tan(angle);
+
+            _display->DrawLine(xStart, yStart, xEnd, yEnd);
+        }
+    }
+
+    // left scale, x = 0;
+    angle = -M_PI / 2;
+    while ((angle += delta) < M_PI / 2) {
+        int xStart = x1;
+        int yStart = _centerY + (xStart - _centerX) * tan(angle);
+
+        if (yStart >= window.y1 && yStart <= window.y2) {
+            int xEnd = xStart + maxLen;
+            int yEnd = _centerY + (xEnd - _centerX) * tan(angle);
+
+            _display->DrawLine(xStart, yStart, xEnd, yEnd);
+        }
+    }
+
+    // right scale, x = x2;
+    angle = M_PI / 2;
+    while ((angle += delta) < 3 * M_PI / 2) {
+        int xStart = x2;
+        int yStart = _centerY + (xStart - _centerX) * tan(angle);
+
+        if (yStart >= window.y1 && yStart <= window.y2) {
+            int xEnd = xStart - maxLen;
+            int yEnd = _centerY + (xEnd - _centerX) * tan(angle);
+
+            _display->DrawLine(xStart, yStart, xEnd, yEnd);
+        }
+    }
+}
+
+
+void Radar::DrawRadarBeam(float azimuth)
+{
+    // Maximum radius length i local coordinates
+    int maxRadius = floor( 0.5f + GetMaxRange() * _scaleRatio);
+
+    int endX = GetCenterX() + maxRadius * sin(azimuth);
+    int endY = GetCenterY() - maxRadius * cos(azimuth);
+
+    _display->SetDrawColor(0xFF, 0xFF, 0xFF, 0xFF);
+    _display->DrawLine(_centerX, _centerY, endX, endY);
+}
+
+Display* Radar::GetDisplay()
+{
+    return _display;
+}
+
+
+int Radar::_centerX;
+int Radar::_centerY;
+uint8_t Radar::_rangeNumber;
+float Radar::_scaleRatio;
\ No newline at end of file