public static ProjectedPoint PolarToRectangular(ProjectedPoint origin, double theta, double r)
{
ProjectedPoint result = new ProjectedPoint();
result.Easting = r * Math.Cos(theta);
result.Northing = r * Math.Sin(theta);
result.Easting += origin.Easting;
result.Northing += origin.Northing;
return result;
}
public ProjectedPoint Project()
{
double easting;
double northing;
int zone;
ProjectToUTM(_latitude.Value, _longitude.Value, out easting, out northing, out zone);
ProjectedPoint pp = new ProjectedPoint();
pp.Easting = easting;
pp.Northing = northing;
pp.Zone = zone;
pp.Height = _heightAboveGeoID;
return(pp);
}
public bool IsRounding(ProjectedPoint a, ProjectedPoint b, Mark previousMark, Mark nextMark)
{
if (Bouys.Count == 1)
{
if (previousMark != null && nextMark != null)
{
//I know this is WAY more complicated than it needs to be, but it works
//I'll clean it later... maybe after I got back to college and take trig again.
Vector2 a2 = new Vector2((float)a.Easting, (float)a.Northing);
Vector2 b2 = new Vector2((float)b.Easting, (float)b.Northing);
ProjectedPoint myA = Bouys[0].CoordinatePoint.Project();
Vector2 myA2 = new Vector2((float)myA.Easting, (float)myA.Northing);
ProjectedPoint myB = FindMarkRoundPoint(previousMark, nextMark);
Vector2 myB2 = new Vector2((float)myB.Easting, (float)myB.Northing);;
Vector2 intersect;
return(GeometryHelper.TryFindIntersection(a2, b2, myA2, myB2, out intersect));
}
else
{
float markRoundDistance = 20f;
return(DistanceTo(a) <= markRoundDistance);
}
}
else if (Bouys.Count == 2)
{
ProjectedPoint myA = Bouys[0].CoordinatePoint.Project();
ProjectedPoint myB = Bouys[1].CoordinatePoint.Project();
//Vector3 aV = a.ToWorld();
//Vector3 bV = b.ToWorld();
//Vector3 myAV = myA.ToWorld();
//Vector3 myBV = myB.ToWorld();
Vector2 intersect;
return(GeometryHelper.TryFindIntersection(new Vector2((float)a.Easting, (float)a.Northing), new Vector2((float)b.Easting, (float)b.Northing), new Vector2((float)myA.Easting, (float)myA.Northing), new Vector2((float)myB.Easting, (float)myB.Northing), out intersect));
}
else
{
throw new Exception("Not Implemented");
}
}
public ProjectedPoint FindMarkRoundPoint(Mark previousMark, Mark nextMark)
{
ProjectedPoint myA = AveragedLocation.Project();
float previousAngle = (float)AngleHelper.FindAngleWTF(previousMark.AveragedLocation.Project(), myA);
previousAngle = AngleHelper.NormalizeAngle(-previousAngle);
float nextAngle = (float)AngleHelper.FindAngleWTF(nextMark.AveragedLocation.Project(), myA);
nextAngle = AngleHelper.NormalizeAngle(-nextAngle);
float roundingAngle = AngleHelper.FindHalfwayCounterClockwiseAngle(previousAngle, nextAngle);
ProjectedPoint pp = AngleHelper.PolarToRectangular(myA, (double)roundingAngle, 1000.0);
return(pp);
}
public double DistanceTo(ProjectedPoint point)
{
if (Bouys.Count == 1)
{
CoordinatePoint bp = new CoordinatePoint(Bouys[0].Latitude, Bouys[0].Longitude, 0);
return(CoordinatePoint.TwoDimensionalDistance(point.Easting, point.Northing, bp.Project().Easting, bp.Project().Northing));
}
else if (Bouys.Count == 2)
{
CoordinatePoint a = new CoordinatePoint(Bouys[0].Latitude, Bouys[0].Longitude, 0);
CoordinatePoint b = new CoordinatePoint(Bouys[1].Latitude, Bouys[1].Longitude, 0);
return(GeometryHelper.DistancePointToLineSegment(a.Project().Easting, a.Project().Northing, b.Project().Easting, b.Project().Northing, point.Easting, point.Northing));
}
else
{
throw new Exception("Not Implemented");
}
}
private Vector3 ProjectedPointToWorld(ProjectedPoint point, CameraMan cameraMan)
{
Vector3 v = new Vector3((float)point.Northing, (float)point.Height, (float)point.Easting);
return v;
}
public static double FindAngleWTF(ProjectedPoint a, ProjectedPoint b)
{
return -Math.Atan2( a.Northing - b.Northing,a.Easting - b.Easting);
}
private Vector3 ProjectedPointToWorld(ProjectedPoint point)
{
//this is passed via delagate to ProjectedPoint, and is used to convert projected points to 3d points we can use.
Vector3 v = new Vector3((float)point.Northing / _coordinateDivisor, (float)point.Height / _coordinateDivisor, (float)point.Easting / _coordinateDivisor);
v.X = v.X - _xOffset;
v.Z = v.Z - _zOffset;
return v;
}
public ProjectedPoint Project()
{
double easting;
double northing;
int zone;
ProjectToUTM(_latitude.Value, _longitude.Value, out easting, out northing, out zone);
ProjectedPoint pp = new ProjectedPoint();
pp.Easting = easting;
pp.Northing = northing;
pp.Zone = zone;
pp.Height = _heightAboveGeoID;
return pp;
}
public void Move(DateTime time,Course course,float windDirection,DateTime raceStart)
{
//cheat and jump to the time for the very first move.
//kindof hacky, but it keeps the user from having to wait for the boats to get
//intialized when the race starts. Also, we probably don't want stats for <race time
if (!_positionInitialized)
{
while (_boatDataRows[_currentBoatDataRow].datetime < raceStart && _currentBoatDataRow < _boatDataRows.Count - 2)
{
_currentBoatDataRow++;
}
//if (_currentBoatDataRow > 0)
//{
// _currentBoatDataRow = _currentBoatDataRow - 1;
//}
_positionInitialized = true;
}
windAngle = windDirection;
if (_lastUpdate == null)
{
_lastUpdate = time;
}
CoordinatePoint cp;
ProjectedPoint current=null;
CoordinatePoint np;
ProjectedPoint next=null;
long ticksBetween;
long actualTicks;
double percent=0.0;
//bool changedIndex = false;
bool doMove = false;
if (_lastUpdate.Ticks < time.Ticks && _currentBoatDataRow < _boatDataRows.Count - 2)
{
doMove = true;
direction = BoatDirection.Forward;
while (_boatDataRows[_currentBoatDataRow].datetime < time && _currentBoatDataRow < _boatDataRows.Count - 2)
{
_currentBoatDataRow++;
//changedIndex = true;
if ((_lowWaterMark == null || _highWaterMark == null) || (time < _lowWaterMark || time > _highWaterMark))
{
cp = new CoordinatePoint(new Coordinate(_boatDataRows[_currentBoatDataRow + 1].latitude), new Coordinate(_boatDataRows[_currentBoatDataRow + 1].longitude), 0);
current = cp.Project();
np = new CoordinatePoint(new Coordinate(_boatDataRows[_currentBoatDataRow].latitude), new Coordinate(_boatDataRows[_currentBoatDataRow].longitude), 0);
next = np.Project();
ProjectedPoint = current;
if (_currentMarkIndex.GetValue(_boatDataRows[_currentBoatDataRow].datetime) < course.Route.Count)
{
NavigateCourse(_boatDataRows[_currentBoatDataRow].datetime, course, cp, np);
}
double distance = CoordinatePoint.TwoDimensionalDistance(current.Easting, current.Northing, next.Easting, next.Northing);
TimeSpan ts = _boatDataRows[_currentBoatDataRow + 1].datetime - _boatDataRows[_currentBoatDataRow].datetime;
double kmh = ((distance / 1000.0) / ts.TotalHours);
speed = (float)kmh;
//float oldWindAngle = RelativeAngleToWind;
arrowAngle = (float)AngleHelper.FindAngle(current, next);
float newWindAngle = RelativeAngleToWind;
if (CurrentRacingStatus == RacingStatus.Racing)
{
float deadZone = MathHelper.PiOver4 / 2f;//use to be /4f;
Tack.TackDirection newTack = Tack.TackDirection.Undetermined;
if (newWindAngle < -deadZone && newWindAngle > -MathHelper.Pi + deadZone)
{
newTack = Tack.TackDirection.Starboard;
}
else if (newWindAngle > deadZone && newWindAngle < MathHelper.Pi - deadZone)
{
newTack = Tack.TackDirection.Port;
}
if (newTack != Tack.TackDirection.Undetermined && (_tacks.Count - 1 < _currentTackIndex.GetValue(_boatDataRows[_currentBoatDataRow].datetime) || _tacks[_currentTackIndex.GetValue(_boatDataRows[_currentBoatDataRow].datetime)].Direction != newTack))
{
PerformTack(_boatDataRows[_currentBoatDataRow].datetime);
}
}
//set the current and previous marks
if (_currentMarkIndex.GetValue(_boatDataRows[_currentBoatDataRow].datetime) < course.Route.Count)
{
_currentMark = course.Route[_currentMarkIndex.GetValue(_boatDataRows[_currentBoatDataRow].datetime)];
}
else
{
_currentMark = null;
}
if (_currentMarkIndex.GetValue(_boatDataRows[_currentBoatDataRow].datetime) > 0 && course.Route.Count > 0)
{
_previousMark = course.Route[_currentMarkIndex.GetValue(_boatDataRows[_currentBoatDataRow].datetime) - 1];
}
else
{
_previousMark = null;
}
UpdateStatistics(_boatDataRows[_currentBoatDataRow].datetime, distance / 1000.0);
Thread.Sleep(0);
}
}
}
else if (_lastUpdate.Ticks > time.Ticks && _currentBoatDataRow > 0)
{
doMove = true;
direction = BoatDirection.Backwards;
while (_boatDataRows[_currentBoatDataRow].datetime > time && _currentBoatDataRow > 1)
{
_currentBoatDataRow--;
//changedIndex = true;
if ((_lowWaterMark == null || _highWaterMark == null) || (time < _lowWaterMark || time > _highWaterMark))
{
cp = new CoordinatePoint(new Coordinate(_boatDataRows[_currentBoatDataRow - 1].latitude), new Coordinate(_boatDataRows[_currentBoatDataRow - 1].longitude), 0);
current = cp.Project();
np = new CoordinatePoint(new Coordinate(_boatDataRows[_currentBoatDataRow].latitude), new Coordinate(_boatDataRows[_currentBoatDataRow].longitude), 0);
next = np.Project();
//ProjectedPoint = cp.Project();
//double distance = CoordinatePoint.TwoDimensionalDistance(current.easting, current.northing, next.easting, next.northing);
//TimeSpan ts = _boatDataRows[_currentBoatDataRow].datetime - _boatDataRows[_currentBoatDataRow - 1].datetime;
//double kmh = ((distance / 1000.0) / ts.TotalHours);
//speed = (float)kmh;
arrowAngle = (float)AngleHelper.FindAngle(current, next);
//UpdateStatistics(_boatDataRows[_currentBoatDataRow].datetime, distance / 1000.0);
Thread.Sleep(0);
}
}
}
//set the current and previous marks
if (_currentMarkIndex.GetValue(_boatDataRows[_currentBoatDataRow].datetime) < course.Route.Count)
{
_currentMark = course.Route[_currentMarkIndex.GetValue(_boatDataRows[_currentBoatDataRow].datetime)];
}
else
{
_currentMark = null;
}
if (_currentMarkIndex.GetValue(_boatDataRows[_currentBoatDataRow].datetime) > 0 && course.Route.Count > 0)
{
_previousMark = course.Route[_currentMarkIndex.GetValue(_boatDataRows[_currentBoatDataRow].datetime) - 1];
}
else
{
_previousMark = null;
}
if (doMove && _currentBoatDataRow > 0 && _currentBoatDataRow < _boatDataRows.Count - 2)
{
int currentOffset;
if (direction == BoatDirection.Forward)
{
currentOffset = -1;
}
else
{
currentOffset = 1;
}
cp = new CoordinatePoint(new Coordinate(_boatDataRows[_currentBoatDataRow + currentOffset].latitude), new Coordinate(_boatDataRows[_currentBoatDataRow + currentOffset].longitude), 0);
current = cp.Project();
np = new CoordinatePoint(new Coordinate(_boatDataRows[_currentBoatDataRow].latitude), new Coordinate(_boatDataRows[_currentBoatDataRow].longitude), 0);
next = np.Project();
CoordinatePoint pp = new CoordinatePoint(new Coordinate(_boatDataRows[_currentBoatDataRow - currentOffset].latitude), new Coordinate(_boatDataRows[_currentBoatDataRow - currentOffset].longitude), 0);
ProjectedPoint previous = pp.Project();
ticksBetween = _boatDataRows[_currentBoatDataRow].datetime.Ticks - _boatDataRows[_currentBoatDataRow + currentOffset].datetime.Ticks;
actualTicks = time.Ticks - _boatDataRows[_currentBoatDataRow + currentOffset].datetime.Ticks;
percent = (double)actualTicks / (double)ticksBetween;
double northingDiff = next.Northing - current.Northing;
double eastingDiff = next.Easting - current.Easting;
double northing = current.Northing + (northingDiff * percent);
double easting = current.Easting + (eastingDiff * percent);
ProjectedPoint actual = new ProjectedPoint();
actual.Easting = easting;
actual.Northing = northing;
ProjectedPoint = actual;
double distance = CoordinatePoint.TwoDimensionalDistance(current.Easting, current.Northing, next.Easting, next.Northing);
TimeSpan ts = new TimeSpan(ticksBetween);
double kmh = ((distance / 1000.0) / ts.TotalHours);
speed = (float)kmh;
if (Math.Abs(kmh) > 2)
{
float startingAngle = (float)AngleHelper.FindAngle(previous, current);
float desiredAngle = (float)AngleHelper.FindAngle(next, current);
desiredAngle = AngleHelper.NormalizeAngle(desiredAngle);
float deltaAngle = AngleHelper.AngleDifference(startingAngle, desiredAngle);
arrowAngle = startingAngle + (deltaAngle * (float)percent);
arrowAngle = AngleHelper.NormalizeAngle(arrowAngle);
}
SetHeel(time,kmh);
SetSailCurve(time);
SetBoomAngle(time);
//if (changedIndex)
//{
// _updateStatistics();
//}
_lastUpdate = time;
}
else if(_currentBoatDataRow==0||_currentBoatDataRow==_boatDataRows.Count-1)
{
ProjectedPoint = new CoordinatePoint(new Coordinate(_boatDataRows[_currentBoatDataRow].latitude), new Coordinate(_boatDataRows[_currentBoatDataRow].longitude), 0).Project();
}
if (_lowWaterMark==null||time < _lowWaterMark)
{
_lowWaterMark = time;
}
if (_highWaterMark == null || time > _highWaterMark)
{
_highWaterMark = time;
}
}
private Vector3 ProjectedPointToWorld(ProjectedPoint point,CameraMan cameraMan)
{
GdiCameraMan gdiCM=(GdiCameraMan)cameraMan;
//note that the y/northing value is inverted to account for the fact that screen coordinates are inverted
Vector3 v = new Vector3((float)(point.Easting-_eastingOffset),-(float)(point.Northing-_northingOffset),0f);
//now zoom appropriatly
v.X = v.X * gdiCM.Zoom;
v.Y = v.Y * gdiCM.Zoom;
//now offset so that the point is aligned with the camera correctly
v.X = v.X - gdiCM.X;
v.Y = v.Y - gdiCM.Y;
return v;
}
public bool IsRounding(ProjectedPoint a, ProjectedPoint b, Mark previousMark,Mark nextMark)
{
if (Bouys.Count == 1)
{
if (previousMark != null && nextMark != null)
{
//I know this is WAY more complicated than it needs to be, but it works
//I'll clean it later... maybe after I got back to college and take trig again.
Vector2 a2 = new Vector2((float)a.Easting, (float)a.Northing);
Vector2 b2 = new Vector2((float)b.Easting, (float)b.Northing);
ProjectedPoint myA = Bouys[0].CoordinatePoint.Project();
Vector2 myA2 = new Vector2((float)myA.Easting, (float)myA.Northing);
ProjectedPoint myB=FindMarkRoundPoint(previousMark, nextMark);
Vector2 myB2 = new Vector2((float)myB.Easting, (float)myB.Northing); ;
Vector2 intersect;
return GeometryHelper.TryFindIntersection(a2,b2,myA2,myB2, out intersect);
}
else
{
float markRoundDistance = 20f;
return DistanceTo(a) <= markRoundDistance;
}
}
else if (Bouys.Count == 2)
{
ProjectedPoint myA = Bouys[0].CoordinatePoint.Project();
ProjectedPoint myB = Bouys[1].CoordinatePoint.Project();
//Vector3 aV = a.ToWorld();
//Vector3 bV = b.ToWorld();
//Vector3 myAV = myA.ToWorld();
//Vector3 myBV = myB.ToWorld();
Vector2 intersect;
return GeometryHelper.TryFindIntersection(new Vector2((float)a.Easting, (float)a.Northing), new Vector2((float)b.Easting, (float)b.Northing), new Vector2((float)myA.Easting, (float)myA.Northing), new Vector2((float)myB.Easting, (float)myB.Northing), out intersect);
}
else
{
throw new Exception("Not Implemented");
}
}
public double DistanceTo(ProjectedPoint point)
{
if (Bouys.Count == 1)
{
CoordinatePoint bp = new CoordinatePoint(Bouys[0].Latitude, Bouys[0].Longitude, 0);
return CoordinatePoint.TwoDimensionalDistance(point.Easting, point.Northing, bp.Project().Easting, bp.Project().Northing);
}
else if (Bouys.Count == 2)
{
CoordinatePoint a = new CoordinatePoint(Bouys[0].Latitude, Bouys[0].Longitude, 0);
CoordinatePoint b = new CoordinatePoint(Bouys[1].Latitude, Bouys[1].Longitude, 0);
return GeometryHelper.DistancePointToLineSegment(a.Project().Easting, a.Project().Northing, b.Project().Easting, b.Project().Northing, point.Easting, point.Northing);
}
else
{
throw new Exception("Not Implemented");
}
}
