00001 using System;
00002 using System.Collections.Generic;
00003 using System.Linq;
00004 using System.Text;
00005 using UtmConvert;
00006 using System.Drawing;
00007 using System.IO;
00008
00009 namespace RacePlotter {
00010 class RacePlotterStrategy : RacePlotterData {
00011
00012 public RacePlotterStrategy(string racePath) {
00013 _racePath = racePath;
00014 InitializationParameters.readIniParameters(_racePath);
00015 _raceMap = new Bitmap(_racePath + "map.jpg");
00016 _click1 = new System.Drawing.Point();
00017 _click2 = new System.Drawing.Point();
00018 _statePosition = new UtmPoint(new UtmConvert.Point
00019 (488816.66, 4874829.39), "10T", "state", "state position");
00020 _gpsPosition = new UtmPoint(new UtmConvert.Point
00021 (488815.8, 4874822.61), "10T", "gps", "gps position");
00022 _fiducial1 = new UtmPoint();
00023 _fiducial2 = new UtmPoint();
00024 _fiducials = new List<UtmPoint>();
00025 Waypoints = new List<UtmPoint>();
00026 parseFiducialList();
00027 if (InitializationParameters.IniFileParameters.Count == 4) {
00028 _scaleDeltaX = double.Parse(InitializationParameters.IniFileParameters["scaleDeltaX"]);
00029 _scaleDeltaY = double.Parse(InitializationParameters.IniFileParameters["scaleDeltaY"]);
00030 _scaleX = double.Parse(InitializationParameters.IniFileParameters["scaleX"]);
00031 _scaleY = double.Parse(InitializationParameters.IniFileParameters["scaleY"]);
00032 _fiducial1 = _fiducials[0];
00033 _fiducial2 = _fiducials[1];
00034 drawFiducials();
00035 }
00036 parseWaypointList();
00037 }
00038
00039 public void redrawMap() {
00040 drawFiducials();
00041 drawRacePath(ref _raceMap);
00042 drawRobotPosition(ref _raceMap, _gpsPosition);
00043 drawRobotPosition(ref _raceMap, _statePosition);
00044 }
00045
00046 private void drawGpsLocation() {
00047
00048 }
00049
00050
00051 private void parseFiducialList() {
00052 if (File.Exists(_racePath + "fiducials.xml")) {
00053 XmlParser x = new XmlParser();
00054 x.readFile(_racePath + "fiducials.xml");
00055 x.Parse("ddd_dd");
00056 _fiducials = x.UtmPointSetList[0].Points;
00057 }
00058 }
00059
00060
00061 private void parseWaypointList() {
00062 if (File.Exists(_racePath + "waypoints.xml")) {
00063 XmlParser x = new XmlParser();
00064 x.readFile(_racePath + "waypoints.xml");
00065 x.Parse("ddd_dd");
00066 _waypoints = x.UtmPointSetList[0].Points;
00067 }
00068 }
00069
00070
00071 public void saveWaypoints() {
00072 List<UtmPointSet> s = new List<UtmPointSet>();
00073 UtmPointSet ps = new UtmPointSet(_racePath + "waypoints"
00074 , _waypoints, "NAD83/WGS84");
00075 s.Add(ps);
00076 Printer p = new Printer();
00077 p.writeUtmXmlFile(s);
00078 }
00079
00080
00081
00082
00083
00084
00085
00086 private void drawFiducials() {
00087 _raceMap = new Bitmap(_racePath + "map.jpg");
00088 Graphics g = Graphics.FromImage(_raceMap);
00089 Pen p = new Pen(Color.Red, 1);
00090 float x = (float)(_fiducials[0].Point.X * _scaleX);
00091 float y = (float)(_fiducials[0].Point.Y * _scaleY);
00092 _click1 = new System.Drawing.Point((int)x, (int)y);
00093 g.DrawLine(p, x - 7, y, x + 7, y);
00094 g.DrawLine(p, x, y - 7, x, y + 7);
00095 x += (float)((_fiducials[1].Point.X - _fiducials[0].Point.X) * _scaleDeltaX);
00096 y += (float)((_fiducials[1].Point.Y - _fiducials[0].Point.Y) * _scaleDeltaY);
00097 _click2 = new System.Drawing.Point((int)x, (int)y);
00098 g.DrawLine(p, x - 7, y, x + 7, y);
00099 g.DrawLine(p, x, y - 7, x, y + 7);
00100 }
00101
00102 public void drawRacePath(ref Bitmap map) {
00103 Graphics g = Graphics.FromImage(map);
00104 Pen pen = new Pen(Color.Red, 2);
00105 SolidBrush brush = new SolidBrush(Color.Orange);
00106 SolidBrush gBrush = new SolidBrush(Color.Gray);
00107 float nextX = 0, nextY = 0, deltaX = 0, deltaY = 0, x1 = 0, y1 = 0;
00108 for (int i = 0; i < Waypoints.Count; ++i) {
00109 deltaX = (float)(Waypoints[i].Point.X - _fiducial1.Point.X);
00110 deltaY = (float)(Waypoints[i].Point.Y - _fiducial1.Point.Y);
00111 x1 = (float)Waypoints[i].Point.X;
00112 y1 = (float)Waypoints[i].Point.Y;
00113 if (Waypoints[i].Note == "dummy")
00114 g.FillEllipse(gBrush, x1 * (float)_scaleX + deltaX * (float)_scaleDeltaX
00115 - 7, y1 * (float)_scaleY + deltaY * (float)_scaleDeltaY - 7, 15, 15);
00116 else
00117 g.FillEllipse(brush, x1 * (float)_scaleX + deltaX * (float)_scaleDeltaX
00118 - 7, y1 * (float)_scaleY + deltaY * (float)_scaleDeltaY - 7, 15, 15);
00119 if (i > 0) {
00120 g.DrawLine(pen, x1 * (float)_scaleX + nextX, y1 * (float)_scaleY + nextY
00121 , x1 * (float)_scaleX + deltaX * (float)_scaleDeltaX
00122 , y1 * (float)_scaleY + deltaY * (float)_scaleDeltaY);
00123 }
00124 nextX = (float)_scaleX + deltaX * (float)_scaleDeltaX;
00125 nextY = (float)_scaleY + deltaY * (float)_scaleDeltaY;
00126 }
00127 }
00128
00129 public void drawRobotPosition(ref Bitmap map, UtmConvert.UtmPoint p) {
00130 Graphics g = Graphics.FromImage(map);
00131 Pen pen = new Pen(Color.Red, 2);
00132 SolidBrush hBrush = new SolidBrush(Color.HotPink);
00133 SolidBrush yBrush = new SolidBrush(Color.Yellow);
00134 float deltaX = 0, deltaY = 0, x1 = 0, y1 = 0;
00135 deltaX = (float)(p.Point.X - _fiducial1.Point.X);
00136 deltaY = (float)(p.Point.Y - _fiducial1.Point.Y);
00137 x1 = (float)p.Point.X;
00138 y1 = (float)p.Point.Y;
00139 if (p.Note == "gps")
00140 g.FillEllipse(hBrush, x1 * (float)_scaleX + deltaX * (float)_scaleDeltaX
00141 - 7, y1 * (float)_scaleY + deltaY * (float)_scaleDeltaY - 7, 15, 15);
00142 else
00143 g.FillEllipse(yBrush, x1 * (float)_scaleX + deltaX * (float)_scaleDeltaX
00144 - 7, y1 * (float)_scaleY + deltaY * (float)_scaleDeltaY - 7, 15, 15);
00145 }
00146
00147
00148 public void resetFiducial() {
00149 _fiducial1 = new UtmPoint();
00150 _fiducial2 = new UtmPoint();
00151 _click1 = new System.Drawing.Point();
00152 _click2 = new System.Drawing.Point();
00153 }
00154
00155 public void drawClickedWaypoint(ref Bitmap map, int x, int y) {
00156 if (_click1.IsEmpty) {
00157 _click1.X = x;
00158 _click1.Y = y;
00159 } else {
00160 _click2.X = x;
00161 _click2.Y = y;
00162 }
00163 Graphics g = Graphics.FromImage(map);
00164 SolidBrush brush = new SolidBrush(Color.Green);
00165 g.FillEllipse(brush, x - 7, y - 7, 15, 15);
00166 }
00167
00168 public void setScale() {
00169 double deltaClickX = (double)Click2.X - (double)Click1.X;
00170 double deltaClickY = (double)Click2.Y - (double)Click1.Y;
00171 double deltaUtmX = (double)(Fiducial2.Point.X - Fiducial1.Point.X);
00172 double deltaUtmY = (double)(Fiducial2.Point.Y - Fiducial1.Point.Y);
00173 _scaleDeltaX = deltaClickX / deltaUtmX;
00174 _scaleDeltaY = deltaClickY / deltaUtmY;
00175 _scaleX = (double)Click1.X / (double)Fiducial1.Point.X;
00176 _scaleY = (double)Click1.Y / (double)Fiducial1.Point.Y;
00177 writeIniFile();
00178 drawFiducials();
00179 }
00180
00181 private void writeIniFile() {
00182 StreamWriter s = new StreamWriter(_racePath + "map.ini");
00183 s.Write(
00184 "scaleDeltaX=" + _scaleDeltaX + "\r\n"
00185 + "scaleDeltaY=" + _scaleDeltaY+ "\r\n"
00186 + "scaleX=" + _scaleX+ "\r\n"
00187 + "scaleY=" + _scaleY + "\r\n"
00188 );
00189 s.Close();
00190 }
00191
00192 }
00193 }