Imports SteeringVB.ParamLoader

Public Class SteeringBehavior
    '//--------------------------- Constants ----------------------------------

    '//the radius of the constraining circle for the wander behavior
    Public Const WanderRad As Double = 1.2
    '//distance the wander circle is projected in front of the agent
    Public Const WanderDist As Double = 2.0
    '//the maximum amount of displacement along the circle each frame
    Public Const WanderJitterPerSec As Double = 80.0

    '//used in path following
    Public Const WaypointSeekDist As Double = 20

    '//------------------------------------------------------------------------

    Private Enum behavior_type
        none = &H0
        seek = &H2
        flee = &H4
        arrive = &H8
        wander = &H10
        cohesion = &H20
        separation = &H40
        allignment = &H80
        obstacle_avoidance = &H100
        wall_avoidance = &H200
        follow_path = &H400
        pursuit = &H800
        evade = &H1000
        interpose = &H2000
        hide = &H4000
        flock = &H8000
        offset_pursuit = &H10000
    End Enum

    '//Arrive makes use of these to determine how quickly a vehicle
    '//should decelerate to its target
    Private Enum Deceleration
        slow = 3
        normal = 2
        fast = 1
    End Enum


    '//a pointer to the owner of this instance
    Dim m_pVehicle As Vehicle

    '//the steering force created by the combined effect of all
    '//the selected behaviors
    Dim m_vSteeringForce As New Vector2D

    '//these can be used to keep track of friends, pursuers, or prey
    Dim m_pTargetAgent1 As Vehicle
    Dim m_pTargetAgent2 As Vehicle

    '//the current target
    Dim m_vTarget As Vector2D

    '//length of the 'detection box' utilized in obstacle avoidance
    Dim m_dDBoxLength As Double


    '//a vertex buffer to contain the feelers rqd for wall avoidance  
    Dim m_Feelers As ArrayList

    '//the length of the 'feeler/s' used in wall detection
    Dim m_dWallDetectionFeelerLength As Double



    '//the current position on the wander circle the agent is
    '//attempting to steer towards
    Dim m_vWanderTarget As Vector2D

    '//explained above
    Dim m_dWanderJitter As Double
    Dim m_dWanderRadius As Double
    Dim m_dWanderDistance As Double


    '//multipliers. These can be adjusted to effect strength of the  
    '//appropriate behavior. Useful to get flocking the way you require
    '//for example.
    Dim m_dWeightSeparation As Double
    Dim m_dWeightCohesion As Double
    Dim m_dWeightAlignment As Double
    Dim m_dWeightWander As Double
    Dim m_dWeightObstacleAvoidance As Double
    Dim m_dWeightWallAvoidance As Double
    Dim m_dWeightSeek As Double
    Dim m_dWeightFlee As Double
    Dim m_dWeightArrive As Double
    Dim m_dWeightPursuit As Double
    Dim m_dWeightOffsetPursuit As Double
    Dim m_dWeightInterpose As Double
    Dim m_dWeightHide As Double
    Dim m_dWeightEvade As Double
    Dim m_dWeightFollowPath As Double

    '//how far the agent can 'see'
    Dim m_dViewDistance As Double

    '//pointer to any current path
    Dim m_pPath As _Path

    '//the distance (squared) a vehicle has to be from a path waypoint before
    '//it starts seeking to the next waypoint
    Dim m_dWaypointSeekDistSq As Double


    '//any offset used for formations or offset pursuit
    Dim m_vOffset As Vector2D



    '//binary flags to indicate whether or not a behavior should be active
    Dim m_iFlags As behavior_type


    '//default
    Dim m_Deceleration As Deceleration

    '//is cell space partitioning to be used or not?
    Dim m_bCellSpaceOn As Boolean

    '//what type of method is used to sum any active behavior
    Dim m_SummingMethod As summing_method


    '//this function tests if a specific bit of m_iFlags is set
    Private Function IsOn(ByVal bt As behavior_type) As Boolean
        Return (m_iFlags And bt) = bt
    End Function

    '//--------------------- AccumulateForce ----------------------------------
    '//
    '//  This function calculates how much of its max steering force the 
    '//  vehicle has left to apply and then applies that amount of the
    '//  force to add.
    '//------------------------------------------------------------------------
    Private Function AccumulateForce(ByVal RunningTot As Vector2D, ByVal ForceToAdd As Vector2D) As Boolean
        '//calculate how much steering force the vehicle has used so far
        Dim MagnitudeSoFar As Double = RunningTot.Length()

        '//calculate how much steering force remains to be used by this vehicle
        Dim MagnitudeRemaining As Double = m_pVehicle.MaxForce() - MagnitudeSoFar

        '//return false if there is no more force left to use
        If MagnitudeRemaining <= 0.0 Then Return False

        '//calculate the magnitude of the force we want to add
        Dim MagnitudeToAdd As Double = ForceToAdd.Length()

        '//if the magnitude of the sum of ForceToAdd and the running total
        '//does not exceed the maximum force available to this vehicle, just
        '//add together. Otherwise add as much of the ForceToAdd vector is
        '//possible without going over the max.
        If (MagnitudeToAdd < MagnitudeRemaining) Then
            RunningTot.PlusEqual(ForceToAdd)

        Else
            '//add it to the steering force
            RunningTot.PlusEqual(Vector2D.Vec2DNormalize(ForceToAdd).Mutiply(MagnitudeRemaining))

        End If

        Return True

    End Function

    '//creates the antenna utilized by the wall avoidance behavior
    '//------------------------------- CreateFeelers --------------------------
    '//
    '//  Creates the antenna utilized by WallAvoidance
    '//------------------------------------------------------------------------
    Private Sub CreateFeelers()
        m_Feelers.Clear()
        '//feeler pointing straight in front
        m_Feelers.Add(m_pVehicle.Pos().Plus(m_pVehicle.Heading().Mutiply(m_dWallDetectionFeelerLength)))

        '//feeler to left
        Dim temp As Vector2D = m_pVehicle.Heading()
        Transformations.Vec2DRotateAroundOrigin(temp, Utils.HalfPi * 3.5F)
        m_Feelers.Add(m_pVehicle.Pos().Plus(temp.Mutiply(m_dWallDetectionFeelerLength / 2.0F)))

        '//feeler to right
        temp = m_pVehicle.Heading()
        Transformations.Vec2DRotateAroundOrigin(temp, Utils.HalfPi * 0.5F)
        m_Feelers.Add(m_pVehicle.Pos().Plus(temp.Mutiply(m_dWallDetectionFeelerLength / 2.0F)))

    End Sub

    '/* .......................................................

    '                    BEGIN BEHAVIOR DECLARATIONS

    '.......................................................*/


    '/////////////////////////////////////////////////////////////////////////////// START OF BEHAVIORS
    '//------------------------------- Seek -----------------------------------
    '//
    '//  Given a target, this behavior returns a steering force which will
    '//  direct the agent towards the target
    '//------------------------------------------------------------------------
    '//this behavior moves the agent towards a target position
    Private Function Seek(ByVal TargetPos As Vector2D) As Vector2D
        Dim DesiredVelocity As Vector2D = Vector2D.Vec2DNormalize(TargetPos.Minus(m_pVehicle.Pos())).Mutiply(m_pVehicle.MaxSpeed())
        Return DesiredVelocity.Minus(m_pVehicle.Velocity())
    End Function

    '//----------------------------- Flee -------------------------------------
    '//
    '//  Does the opposite of Seek
    '//------------------------------------------------------------------------
    '//this behavior returns a vector that moves the agent away
    '//from a target position
    Private Function Flee(ByVal TargetPos As Vector2D) As Vector2D

        '//only flee if the target is within 'panic distance'. Work in distance
        '//squared space.
        '/* const double PanicDistanceSq = 100.0f * 100.0;
        'if (Vec2DDistanceSq(m_pVehicle->Pos(), target) > PanicDistanceSq)
        '{
        'return Vector2D(0,0);
        '}
        '*/

        Dim DesiredVelocity As Vector2D = Vector2D.Vec2DNormalize(m_pVehicle.Pos().Minus(TargetPos)).Mutiply(m_pVehicle.MaxSpeed())
        Return DesiredVelocity.Minus(m_pVehicle.Velocity())
    End Function

    '//this behavior is similar to seek but it attempts to arrive 
    '//at the target position with a zero velocity
    '//--------------------------- Arrive -------------------------------------
    '//
    '//  This behavior is similar to seek but it attempts to arrive at the
    '//  target with a zero velocity
    '//------------------------------------------------------------------------
    Private Function Arrive(ByVal TargetPos As Vector2D, ByVal deceleration As Deceleration) As Vector2D
        Dim ToTarget As Vector2D = TargetPos.Minus(m_pVehicle.Pos())

        '//calculate the distance to the target
        Dim dist As Double = ToTarget.Length()

        If dist > 0 Then
            '//because Deceleration is enumerated as an int, this value is required
            '//to provide fine tweaking of the deceleration..
            Dim DecelerationTweaker As Double = 0.3

            '//calculate the speed required to reach the target given the desired
            '//deceleration
            Dim speed As Double = dist / (CDbl(deceleration * DecelerationTweaker))

            '//make sure the velocity does not exceed the max
            speed = Math.Min(speed, m_pVehicle.MaxSpeed())

            '//from here proceed just like Seek except we don't need to normalize 
            '//the ToTarget vector because we have already gone to the trouble
            '//of calculating its length: dist. 
            Dim DesiredVelocity As Vector2D = ToTarget.Mutiply(speed).Divided(dist)

            Return DesiredVelocity.Minus(m_pVehicle.Velocity())
        End If

        Return New Vector2D(0, 0)

    End Function

    '//this behavior predicts where an agent will be in time T and seeks
    '//towards that point to intercept it.
    '//------------------------------ Pursuit ---------------------------------
    '//
    '//  this behavior creates a force that steers the agent towards the 
    '//  evader
    '//------------------------------------------------------------------------
    Private Function Pursuit(ByVal evader As Vehicle) As Vector2D
        '//if the evader is ahead and facing the agent then we can just seek
        '//for the evader's current position.
        Dim ToEvader As Vector2D = evader.Pos().Minus(m_pVehicle.Pos())

        Dim RelativeHeading As Double = m_pVehicle.Heading().Dot(evader.Heading())

        If (ToEvader.Dot(m_pVehicle.Heading()) > 0) And (RelativeHeading < -0.95) Then   '  //acos(0.95)=18 degs
            Return Seek(evader.Pos())
        End If

        '//Not considered ahead so we predict where the evader will be.

        '//the lookahead time is propotional to the distance between the evader
        '//and the pursuer; and is inversely proportional to the sum of the
        '//agent's velocities
        Dim LookAheadTime As Double = ToEvader.Length() / (m_pVehicle.MaxSpeed() + evader.Speed())

        '//now seek to the predicted future position of the evader
        Return Seek(evader.Pos().Plus(evader.Velocity().Mutiply(LookAheadTime)))
    End Function

    '//this behavior maintains a position, in the direction of offset
    '//from the target vehicle
    '//------------------------- Offset Pursuit -------------------------------
    '//
    '//  Produces a steering force that keeps a vehicle at a specified offset
    '//  from a leader vehicle
    '//------------------------------------------------------------------------
    Private Function OffsetPursuit(ByVal leader As Vehicle, ByVal offset As Vector2D) As Vector2D
        '//calculate the offset's position in world space