Add a point to a polyline

Module Geometry
    ''' <summary>
    ''' Finds which side of a line the point is
    ''' </summary>
    ''' <param name="PointToBeEvaluated">Evaluation point</param>
    ''' <param name="StartPointOnLine">Startpoint of line</param>
    ''' <param name="EndPointOnLine">Endpoint on line</param>
    ''' <returns>-1 for a point to the right, 0 for a point on the line, +1 for a point to the left</returns>
    ''' <remarks></remarks>
    Private Function WhichSide(ByVal PointToBeEvaluated As Point, ByVal StartPointOnLine As Point, ByVal EndPointOnLine As Point) As Integer
        Dim ReturnvalueEquation As Double
        ReturnvalueEquation = ((PointToBeEvaluated.Y - StartPointOnLine.Y) _
                               * (EndPointOnLine.X - StartPointOnLine.X)) - ((EndPointOnLine.Y - StartPointOnLine.Y) _
                               * (PointToBeEvaluated.X - StartPointOnLine.X))

        If ReturnvalueEquation > 0 Then
            Return -1
        ElseIf ReturnvalueEquation = 0 Then
            Return 0
        Else
            Return 1
        End If
    End Function

    ''' <summary>
    ''' Returns the distance from a point to a line
    ''' </summary>
    ''' <param name="LinePoint1"></param>
    ''' <param name="LinePoint2"></param>
    ''' <param name="TestPoint"></param>
    ''' <returns></returns>
    ''' <remarks></remarks>
    Private Function DistanceFromLine(ByVal LinePoint1 As Point, ByVal LinePoint2 As Point, TestPoint As Point) As Double
        Dim d As Double
        d = Math.Abs((LinePoint2.X - LinePoint1.X) * (LinePoint1.Y - TestPoint.Y) - (LinePoint1.X - TestPoint.X) * (LinePoint2.Y - LinePoint1.Y))
        d = d / Math.Sqrt((LinePoint2.X - LinePoint1.X) ^ 2 + (LinePoint2.Y - LinePoint1.Y) ^ 2)
        Return d
    End Function

    Private Function DistanceBetweenPoints(ByVal Point1 As Point, Point2 As Point) As Double
        Return Math.Sqrt((Point1.X - Point2.X) ^ 2 + (Point1.Y - Point2.Y) ^ 2)
    End Function

    ''' <summary>
    ''' Calculate the angel in degrees between point 1 and 3 at point 2
    ''' </summary>
    ''' <param name="Point1"></param>
    ''' <param name="Point2"></param>
    ''' <param name="Point3"></param>
    ''' <returns></returns>
    ''' <remarks></remarks>
    Private Function Angles(ByVal Point1 As Point, ByVal Point2 As Point, ByVal Point3 As Point) As Double
        Dim result As Double
        Dim a, b, c As Double
        c = DistanceBetweenPoints(Point1, Point3)
        b = DistanceBetweenPoints(Point1, Point2)
        a = DistanceBetweenPoints(Point2, Point3)
        result = Math.Acos((a ^ 2 + b ^ 2 - c ^ 2) / (2 * b * a))
        Return result
    End Function

    ''' <summary>
    ''' Calculates the Normal vector at point 1
    ''' </summary>
    ''' <param name="Point1"></param>
    ''' <param name="point2"></param>
    ''' <returns></returns>
    ''' <remarks></remarks>
    Private Function Normal2D(ByVal Point1 As Point, ByVal point2 As Point, Optional ByVal theta As Double = Math.PI / 2) As Point
        Dim p As New Point

        p.X = Math.Cos(theta) * (point2.X - Point1.X) - Math.Sin(theta) * (point2.Y - Point1.Y) + Point1.X
        p.Y = Math.Sin(theta) * (point2.X - Point1.X) + Math.Cos(theta) * (point2.Y - Point1.Y) + Point1.Y
        Return p
    End Function


    ''' <summary>
    ''' Insert a new point to the line
    ''' </summary>
    ''' <param name="OriginalPointColletion"></param>
    ''' <param name="NewPoint"></param>
    ''' <returns></returns>
    ''' <remarks></remarks>
    Public Function InsertPoint(ByVal OriginalPointColletion As PointCollection, ByVal NewPoint As Point) As PointCollection
        Dim result As New PointCollection
        result = OriginalPointColletion.Clone
        Dim min_distance As Double = Double.MaxValue
        'For Each p As Point In result
        '    min_distance += DistanceBetweenPoints(NewPoint, p)
        'Next
        Dim temp_distance As Double
        Dim index As Integer

        Dim VectorLinesCalc As New List(Of VectorLine)
        VectorLinesCalc = CalculateAllAngles(OriginalPointColletion)

        For i As Integer = 0 To OriginalPointColletion.Count - 2
            temp_distance = DistanceFromLine2(NewPoint, VectorLinesCalc(i), VectorLinesCalc(i + 1))
            If temp_distance < min_distance Then
                min_distance = temp_distance
                index = i + 1
            End If
        Next

        If DistanceBetweenPoints(OriginalPointColletion(0), NewPoint) < min_distance Then
            min_distance = DistanceBetweenPoints(OriginalPointColletion(0), NewPoint)
            index = 0
        End If

        If DistanceBetweenPoints(OriginalPointColletion(OriginalPointColletion.Count - 1), NewPoint) < min_distance Then
            index = -1
            min_distance = DistanceBetweenPoints(OriginalPointColletion(OriginalPointColletion.Count - 1), NewPoint)
        End If

        If Not index = -1 Then
            result.Insert(index, NewPoint)
        Else
            result.Add(NewPoint)
        End If
        Return result
    End Function

    Private Function DistanceFromLine2(TestPoint As Point, ByVal vector1 As VectorLine, ByVal vector2 As VectorLine) As Double

        Dim FirstTest, SecondTest As Integer

        FirstTest = WhichSide(TestPoint, vector1.Point1, vector1.Point2)
        SecondTest = WhichSide(TestPoint, vector2.Point1, vector2.Point2)

        If FirstTest <> SecondTest Then
            Return DistanceFromLine(vector1.Point1, vector2.Point1, TestPoint)
        Else
            Return Double.MaxValue
        End If

    End Function


    Public Function CalculateAllAngles(ByVal OriginalPointCollection As PointCollection) As List(Of VectorLine)
        Dim result As New List(Of VectorLine)
        For i As Integer = 0 To OriginalPointCollection.Count - 1
            Dim NewVectorLine As New VectorLine
            If i = 0 Then
                NewVectorLine.Point2 = Normal2D(OriginalPointCollection(i), OriginalPointCollection(i + 1))
                NewVectorLine.Point1 = OriginalPointCollection(i)
                result.Add(NewVectorLine)
            ElseIf i = OriginalPointCollection.Count - 1 Then
                NewVectorLine.Point2 = Normal2D(OriginalPointCollection(i), OriginalPointCollection(i - 1), 3 * Math.PI / 2)
                NewVectorLine.Point1 = OriginalPointCollection(i)
                result.Add(NewVectorLine)
            Else
                NewVectorLine.Point1 = OriginalPointCollection(i)
                Dim angl As Double = Angles(OriginalPointCollection(i - 1), OriginalPointCollection(i), OriginalPointCollection(i + 1))

                Dim PreviousAngle, NextAngle As Integer
                PreviousAngle = WhichSide(result(i - 1).Point2, OriginalPointCollection(i - 1), OriginalPointCollection(i))
                NextAngle = WhichSide(OriginalPointCollection(i + 1), OriginalPointCollection(i - 1), OriginalPointCollection(i))
                If PreviousAngle = NextAngle Then
                    NewVectorLine.Point2 = Normal2D(OriginalPointCollection(i), OriginalPointCollection(i + 1), angl / 2)
                Else
                    NewVectorLine.Point2 = Normal2D(OriginalPointCollection(i), OriginalPointCollection(i + 1), (2 * Math.PI - angl) / 2)
                End If
                result.Add(NewVectorLine)
            End If
        Next
        Return result
    End Function


    Public Class VectorLine
        Public Point1 As New Point
        Public Point2 As New Point
    End Class
End Module