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 void LoadBoatTracks()
{
_boatTracks = new Dictionary<ReplayBoat, SortedList<DateTime, ProjectedPoint>>();
foreach (ReplayBoat boat in Replay.Boats)
{
var points = from r in boat.SensorReadings
orderby r.datetime ascending
select r;
SortedList<DateTime, ProjectedPoint> linePoints = new SortedList<DateTime, ProjectedPoint>();
foreach (var point in points)
{
CoordinatePoint cp = new CoordinatePoint(new Coordinate(point.latitude), new Coordinate(point.longitude), 0);
ProjectedPoint pp = cp.Project();
if (!linePoints.ContainsKey(point.datetime))
{
linePoints.Add(point.datetime, pp);
}
}
_boatTracks.Add(boat, linePoints);
}
}
public void BuildBoatPathCurve(ReplayBoat boat)
{
SortedList<int, int> tempBoatDataRowsCurveMap = new SortedList<int, int>();
List<Vector3> tempControlPoints = new List<Vector3>();
List<float> tempDistances = new List<float>();
List<Vector3> controlPoints = new List<Vector3>();
for (int i = 0; i < boat.SensorReadings.Count; i++)
{
CoordinatePoint cp = new CoordinatePoint(new Coordinate(boat.SensorReadings[i].latitude), new Coordinate(boat.SensorReadings[i].longitude), 0);
controlPoints.Add(ProjectedPointToWorld(cp.Project()));
}
List<Vector3> curvePoints = BezierHelper.CreateSmoothedLine(controlPoints, out tempBoatDataRowsCurveMap, out tempControlPoints, out tempDistances);
_boatDataRowsCurveMap[boat] = tempBoatDataRowsCurveMap;
_boatPathCurve[boat] = new VertexPositionColor[(curvePoints.Count * 2) - 2];
_boatPathControlPoints[boat] = tempControlPoints;
_boatDataRowsDistances[boat] = tempDistances;
float maxWidth = 0.2f;//meters/coord divisor
float minWidth = 0.01f;//meters/coord divisor
float maxSpeed = 20f;//kmh
//float minSpeed = 0f;//kmh
int vertexIndex = 0;
for (int i = 0; i < boat.SensorReadings.Count - 2; i++)
{
int curveStart = _boatDataRowsCurveMap[boat][i];
int curveEnd = _boatDataRowsCurveMap[boat][i + 1];
float startDistance = _boatDataRowsDistances[boat][i];
float endDistance = _boatDataRowsDistances[boat][i + 1];
float startSpeed = (startDistance / 1000) / (float)(boat.SensorReadings[i + 1].datetime - boat.SensorReadings[i].datetime).TotalHours;
float endSpeed = (endDistance / 1000) / (float)(boat.SensorReadings[i + 2].datetime - boat.SensorReadings[i + 1].datetime).TotalHours;
float speedDelta = endSpeed - startSpeed;
int curvePointCount = curveEnd - curveStart;
for (int c = curveStart; c < curveEnd; c++)
{
float percentThrough = ((float)c - (float)curveStart) / (float)curvePointCount;
float speed = startSpeed + (percentThrough * speedDelta);
float width = minWidth + ((speed / maxSpeed) * (maxWidth - minWidth));
width = maxWidth - width;
float angleToNext = -(float)Math.Atan2(curvePoints[c + 1].Z - curvePoints[c].Z, curvePoints[c + 1].X - curvePoints[c].X);
float angleRight = angleToNext + MathHelper.PiOver2;
float angleLeft = angleToNext - MathHelper.PiOver2;
float rightX = curvePoints[c].X + (float)Math.Cos(angleRight) * (width / 2f);
float rightZ = curvePoints[c].Z - (float)Math.Sin(angleRight) * (width / 2f);
VertexPositionColor right = new VertexPositionColor();
right.Position = new Vector3(rightX, 0, rightZ);
_boatPathCurve[boat][vertexIndex] = right;
vertexIndex++;
float leftX = curvePoints[c].X + (float)Math.Cos(angleLeft) * (width / 2f);
float leftZ = curvePoints[c].Z - (float)Math.Sin(angleLeft) * (width / 2f);
VertexPositionColor left = new VertexPositionColor();
left.Position = new Vector3(leftX, 0, leftZ);
_boatPathCurve[boat][vertexIndex] = left;
vertexIndex++;
}
}
}
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 void NavigateCourse(DateTime time, Course course,CoordinatePoint a,CoordinatePoint b)
{
//if (course.Route[CurrentMarkIndex.GetValue(time)].DistanceTo(location.Project()) < _markRoundDistance)
//{
// CurrentMarkIndex.AddValue(time, CurrentMarkIndex.GetValue(time) + 1);
// #warning possible bad increment of tack count
// //not sure this should increment tack count, technically rounding a mark is not a tack.
// if (CurrentMarkIndex.GetValue(time) < course.Route.Count)
// {
// PerformTack(time);
// }
//}
Mark previous = null;
Mark next = null;
if (CurrentMarkIndex.GetValue(time) > 0)
{
previous = course.Route[CurrentMarkIndex.GetValue(time) - 1];
}
if (CurrentMarkIndex.GetValue(time) < course.Route.Count - 1)
{
next = course.Route[CurrentMarkIndex.GetValue(time) + 1];
}
if (course.Route[CurrentMarkIndex.GetValue(time)].IsRounding(a.Project(),b.Project(),previous,next))
{
CurrentMarkIndex.AddValue(time, CurrentMarkIndex.GetValue(time) + 1);
if (CurrentMarkIndex.GetValue(time) < course.Route.Count)
{
PerformTack(time);
}
}
}
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");
}
}
