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);
}
private void Move()
{
//find angle to current waypoint.
float wayX = _wayPoints[0, _currentWayPoint];
float wayZ = _wayPoints[1, _currentWayPoint];
float curX = _currentPosition.X;
float curZ = _currentPosition.Z;
float dirX = wayX - curX;
float dirZ = wayZ - curZ;
float wayAngle = (-(float)Math.Atan2((float)dirZ, (float)dirX)); //angle to the waypoint
wayAngle = AngleHelper.NormalizeAngle(wayAngle);
float diff = Math.Abs(wayAngle - _currentDirection);
if (_currentDirection < wayAngle || (_currentDirection > wayAngle + MathHelper.Pi))
{
//add
if (diff < _turnRate)
{
_currentDirection = _currentDirection + diff;
}
else
{
_currentDirection = _currentDirection + _turnRate;
}
}
else if (_currentDirection > wayAngle || (_currentDirection < wayAngle + MathHelper.Pi))
{
//subtract
if (diff < _turnRate)
{
_currentDirection = _currentDirection - diff;
}
else
{
_currentDirection = _currentDirection - _turnRate;
}
}
_currentDirection = AngleHelper.NormalizeAngle(_currentDirection);
//move to the waypoint
//_currentPosition.X = wayX;
//_currentPosition.Z = wayZ;
//now that we have the new angle, move the boat;
float dx = 1.0f;
float dz = 0f;
curX = curX + (float)Math.Cos(_currentDirection) * dx - (float)Math.Sin(_currentDirection) * dz;
curZ = curZ - (float)Math.Sin(_currentDirection) * dx - (float)Math.Cos(_currentDirection) * dz;
_currentPosition.X = curX;
_currentPosition.Z = curZ;
}