// Returns the grid points in XY occupied by the given obstacle public List <mPoint> LocalizeObstacle(Obstacle obstacle) { double currentDirection = AngleUtils.DegreesToRadians(CurrentPose.CurrentDirection); // Convert the left and right points from mm to cm Vector leftObstaclePoint = obstacle.leftPoint / 10; Vector rightObstaclePoint = obstacle.rightPoint / 10; // Find the offset of the left and right of the obstacle from the current position Vector leftOffsetVector = CurrentPose.CurrentDirectionVector + VectorUtils.RotateVector(leftObstaclePoint, currentDirection); Vector rightOffsetVector = CurrentPose.CurrentDirectionVector + VectorUtils.RotateVector(rightObstaclePoint, currentDirection); // Estimate the locations of the actual grid points mPoint leftMapPoint = new mPoint(CurrentPose.CurrentPositionInXY.X + leftOffsetVector.X, CurrentPose.CurrentPositionInXY.Y + leftOffsetVector.Y, CurrentPose.CurrentRoom); mPoint rightMapPoint = new mPoint(CurrentPose.CurrentPositionInXY.X + leftOffsetVector.X, CurrentPose.CurrentPositionInXY.Y + leftOffsetVector.Y, CurrentPose.CurrentRoom); // Find the actual grid points from the estimated grid points leftMapPoint = CurrentPose.CurrentRoom.GetClosestGridPoint(leftMapPoint.GetUVFromXY(CurrentPose.CurrentRoom)); rightMapPoint = CurrentPose.CurrentRoom.GetClosestGridPoint(rightMapPoint.GetUVFromXY(CurrentPose.CurrentRoom)); List <mPoint> points = new List <mPoint>(); points.Add(leftMapPoint); points.Add(rightMapPoint); return(points); }
public double DifInRaidans(Vector startVector, Vector compassDirection) { double value = Vector.AngleBetween(startVector, compassDirection); return(AngleUtils.DegreesToRadians(value)); }