'//  Desc:   Functions for converting 2D vectors between World and Local space.
Public Class Transformations


    '//--------------------------- WorldTransform -----------------------------
    '//
    '//  given a std::vector of 2D vectors, a position, orientation and scale,
    '//  this function transforms the 2D vectors into the object's world space
    '//------------------------------------------------------------------------
    Public Shared Function WorldTransform(ByVal points As ArrayList, ByVal pos As Vector2D, ByVal forward As Vector2D, ByVal side As Vector2D, ByVal scale As Vector2D) As ArrayList
        '//copy the original vertices into the buffer about to be transformed
        Dim TranVector2Ds As New ArrayList
        Dim i As Integer
        For i = 0 To points.Count - 1
            TranVector2Ds.Add(CType(points(i), Vector2D).Copy)
        Next

        '//create a transformation matrix
        Dim matTransform As New C2DMatrix

        '//scale
        If (scale.x <> 1.0) Or (scale.y <> 1.0) Then matTransform.Scale(scale.x, scale.y)

        '//rotate
        matTransform.Rotate(forward, side)

        '//and translate
        matTransform.Translate(pos.x, pos.y)

        '//now transform the object's vertices
        matTransform.TransformVector2Ds(TranVector2Ds)

        Return TranVector2Ds
    End Function

    '//--------------------------- WorldTransform -----------------------------
    '//
    '//  given a std::vector of 2D vectors, a position and  orientation
    '//  this function transforms the 2D vectors into the object's world space
    '/------------------------------------------------------------------------
    Public Shared Function WorldTransform(ByVal points As ArrayList, ByVal pos As Vector2D, ByVal forward As Vector2D, ByVal side As Vector2D) As ArrayList
        '//copy the original vertices into the buffer about to be transformed
        Dim TranVector2Ds As ArrayList = points

        '//create a transformation matrix
        Dim matTransform As New C2DMatrix

        '//rotate
        matTransform.Rotate(forward, side)

        '//and translate
        matTransform.Translate(pos.x, pos.y)

        '//now transform the object's vertices
        matTransform.TransformVector2Ds(TranVector2Ds)

        Return TranVector2Ds '

    End Function

    '//--------------------- PointToWorldSpace --------------------------------
    '//
    '//  Transforms a point from the agent's local space into world space
    '//------------------------------------------------------------------------
    Public Shared Function PointToWorldSpace(ByVal point As Vector2D, ByVal AgentHeading As Vector2D, ByVal AgentSide As Vector2D, ByVal AgentPosition As Vector2D) As Vector2D
        '//make a copy of the point
        Dim TransPoint As Vector2D = point

        '//create a transformation matrix
        Dim matTransform As New C2DMatrix

        '//rotate
        matTransform.Rotate(AgentHeading, AgentSide)

        '//and translate
        matTransform.Translate(AgentPosition.x, AgentPosition.y)

        '//now transform the vertices
        matTransform.TransformVector2Ds(TransPoint)

        Return TransPoint
    End Function

    '//--------------------- VectorToWorldSpace --------------------------------
    '//
    '//  Transforms a vector from the agent's local space into world space
    '//------------------------------------------------------------------------
    Public Shared Function VectorToWorldSpace(ByVal vec As Vector2D, ByVal AgentHeading As Vector2D, ByVal AgentSide As Vector2D) As Vector2D
        '//make a copy of the point
        Dim TransVec As Vector2D = vec

        '//create a transformation matrix
        Dim matTransform As New C2DMatrix

        '//rotate
        matTransform.Rotate(AgentHeading, AgentSide)

        '//now transform the vertices
        matTransform.TransformVector2Ds(TransVec)

        Return TransVec
    End Function

    '//--------------------- PointToLocalSpace --------------------------------
    '//
    '//------------------------------------------------------------------------
    Public Shared Function PointToLocalSpace(ByVal point As Vector2D, ByVal AgentHeading As Vector2D, ByVal AgentSide As Vector2D, ByVal AgentPosition As Vector2D) As Vector2D
        '//make a copy of the point
        Dim TransPoint As Vector2D = point.Copy

        '//create a transformation matrix
        Dim matTransform As New C2DMatrix

        Dim Tx As Double = -AgentPosition.Dot(AgentHeading)
        Dim Ty As Double = -AgentPosition.Dot(AgentSide)

        '//create the transformation matrix
        matTransform._11(AgentHeading.x)
        matTransform._12(AgentSide.x)
        matTransform._21(AgentHeading.y)
        matTransform._22(AgentSide.y)
        matTransform._31(Tx)
        matTransform._32(Ty)

        '//now transform the vertices
        matTransform.TransformVector2Ds(TransPoint)

        Return TransPoint
    End Function

    '//--------------------- VectorToLocalSpace --------------------------------
    '//
    '//------------------------------------------------------------------------
    Public Shared Function VectorToLocalSpace(ByVal vec As Vector2D, ByVal AgentHeading As Vector2D, ByVal AgentSide As Vector2D) As Vector2D
        '//make a copy of the point
        Dim TransPoint As Vector2D = vec.Copy

        '//create a transformation matrix
        Dim matTransform As New C2DMatrix

        '//create the transformation matrix
        matTransform._11(AgentHeading.x)
        matTransform._12(AgentSide.x)
        matTransform._21(AgentHeading.y)
        matTransform._22(AgentSide.y)

        '//now transform the vertices
        matTransform.TransformVector2Ds(TransPoint)

        Return TransPoint
    End Function

    '//-------------------------- Vec2DRotateAroundOrigin --------------------------
    '//
    '//  rotates a vector ang rads around the origin
    '//-----------------------------------------------------------------------------
    Public Shared Sub Vec2DRotateAroundOrigin(ByVal v As Vector2D, ByVal ang As Double)
        '//create a transformation matrix
        Dim mat As New C2DMatrix

        '//rotate
        mat.Rotate(ang)

        '//now transform the object's vertices
        mat.TransformVector2Ds(v)
    End Sub

    '//------------------------ CreateWhiskers ------------------------------------
    '//
    '//  given an origin, a facing direction, a 'field of view' describing the 
    '//  limit of the outer whiskers, a whisker length and the number of whiskers
    '//  this method returns a vector containing the end positions of a series
    '//  of whiskers radiating away from the origin and with equal distance between
    '//  them. (like the spokes of a wheel clipped to a specific segment size)
    '//----------------------------------------------------------------------------
    Public Shared Function CreateWhiskers(ByVal NumWhiskers As Integer, ByVal WhiskerLength As Double, ByVal fov As Double, ByVal facing As Vector2D, ByVal origin As Vector2D) As ArrayList
        '//this is the magnitude of the angle separating each whisker
        Dim SectorSize As Double = fov / (NumWhiskers - 1)

        Dim whiskers As New ArrayList
        Dim temp As Vector2D
        Dim angle As Double = -fov * 0.5

        Dim w As Integer
        For w = 0 To NumWhiskers - 1
            '//create the whisker extending outwards at this angle
            temp = facing
            Vec2DRotateAroundOrigin(temp, angle)
            whiskers.Add(origin.Plus(temp.Mutiply(WhiskerLength)))

            angle += SectorSize
        Next
        Return whiskers
    End Function

End Class