gd_core/addons/core/ai/SpherePathFollower.cs

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//  D E R E L I C T
//
/// // (c) 2003..2025

using Godot;

/// <summary>
/// A path follower that treats the agent as a sphere. The target point is the
/// intersection of this sphere with the current path segment.
/// </summary>
public class SpherePathFollower
{
    private Vector3[] _path;
    private float _radius = 4.0f;
    private int _currentIndex = 0;

    public Vector3[] Path => _path;

    public int Index => _currentIndex;

    public int MaxIndex => _path.Length;

    /// <summary>
    /// Checks if the agent has reached the end of the path.
    /// </summary>
    public bool IsFinished => _currentIndex >= _path.Length;

    /// <summary>
    /// Checks if the agent has reached the end of the path.
    /// </summary>
    public bool IsNearlyFinished => _currentIndex >= ( _path.Length - 1 );

    /// <summary>
    /// Initializes a new path follower with a path and a sphere radius.
    /// </summary>
    /// <param name="path">The array of Vector3 points to follow.</param>
    /// <param name="sphereRadius">The radius of the agent's collision/interaction sphere.</param>
    public SpherePathFollower( float sphereRadius )
    {
        _radius = sphereRadius;
        SetPath( new Vector3[0], sphereRadius );
    }

    /// <summary>
    /// Initializes a new path follower with a path and a sphere radius.
    /// </summary>
    /// <param name="path">The array of Vector3 points to follow.</param>
    /// <param name="sphereRadius">The radius of the agent's collision/interaction sphere.</param>
    public SpherePathFollower( Vector3[] path, float sphereRadius )
    {
        SetPath( path, sphereRadius );
    }

    /// <summary>
    /// Assigns a new path and radius, resetting the follower's progress.
    /// </summary>
    public void SetPath( Vector3[] newPath )
    {
        SetPath( newPath, _radius );
    }

    /// <summary>
    /// Assigns a new path and radius, resetting the follower's progress.
    /// </summary>
    public void SetPath( Vector3[] newPath, float sphereRadius )
    {
        _path = newPath ?? new Vector3[0];
        _radius = Mathf.Max( 0.01f, sphereRadius ); // Ensure radius is positive
        _currentIndex = 0;
    }

    /// <summary>
    /// Calculates the next target point on the current path segment for the agent to move towards.
    /// </summary>
    /// <param name="agentPosition">The current world position of the agent.</param>
    /// <returns>The world-space target point on the path segment.</returns>
    public Vector3 GetTargetPoint( Vector3 agentPos, Vector3 goalPos, out bool advance )
    {
        advance = false;
        if( IsNearlyFinished )
        {
            return goalPos;
        }

        Vector3 p1 = _path[_currentIndex];
        Vector3 p2 = _path[_currentIndex + 1];

        //DebugDraw3D.DrawLine(p1, p2, Colors.Blue);

        Vector3 intersects = p2;

        var intersectLine = Geo.SphereSegment( agentPos, _radius, p1, p2, out intersects, out var intersectType );

        /*
        if (!intersectLine)
        {
            intersects = p1;
            intersectType = Geo.IntersectType.Before;
        }
        */

        //log.info($"{log.loc().Log}: Seg:({p1.Log})->({p2.Log}) On {intersectLine} Type {intersectType} To {intersects.Log}");

        switch( intersectType )
        {
            case Geo.IntersectType.After:
                advance = true;
                break;
        }

        return intersects;
    }


    /// <summary>
    /// Updates the follower's progress along the path. Call this after the agent moves.
    /// </summary>
    /// <param name="agentPosition">The agent's new world position after moving.</param>
    /// <returns>True if updated</returns>
    public int Update( Vector3 agentPos, Vector3 goalPos, bool advance )
    {
        if( IsNearlyFinished )
        {
            if( !IsFinished )
                return ++_currentIndex;

            return _currentIndex;
        }

        Vector3 endOfSegment = _path[_currentIndex + 1];

        // Check if the agent's sphere has reached the end of the current segment
        if( advance || agentPos.DistanceSquaredTo( endOfSegment ) <= _radius * _radius )
        {
            ++_currentIndex;
        }

        var targetPos = GetTargetPoint( agentPos, goalPos, out advance );

        return _currentIndex;
    }


    /// <summary>
    /// Updates the follower's progress along the path. Call this after the agent moves.
    /// </summary>
    /// <param name="agentPosition">The agent's new world position after moving.</param>
    /// <returns>True if updated</returns>
    public int Update_orig( Vector3 agentPosition, out bool advance )
    {
        advance = false;

        if( IsFinished )
            return _currentIndex;

        Vector3 endOfSegment = _path[_currentIndex + 1];

        // Check if the agent's sphere has reached the end of the current segment
        while( agentPosition.DistanceTo( endOfSegment ) <= _radius )
        {
            _currentIndex++;

            advance = true;

            if( IsFinished )
                return _currentIndex;

            endOfSegment = _path[_currentIndex + 1];
        }

        return _currentIndex;
    }
}