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; }