Comparing two indetical Vector3 in ArrayList

Hi. I am writing a C# script on Unity 3D.
I’m working on pathfinding now.
The path has been found correctly in the Array List and the Object can move according to the path with a constant speed.
Then, I want to improvise, where when the path found is a straight line, the object will increase its speed. Meanwhile, when the path turns the object will reduce its speed again.
Here, I don’t know how to group between straight and turning paths in an arrayList.
Thankyou for helping me :slight_smile:
This is the code :

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;

public abstract class Unit : MonoBehaviour
{
    #region public variables
    public GameObject Astar;
    public bool drawGizmos = false;
    protected float gravity = 9.8f;
    public Transform target;
    public Transform NPC;
    public Vector3 lastTargetPosition;
    public float movementSpeed;
    public float rotationSpeed = 85;
    protected float distanceToPath = 1;
    public Vector2 currentPosition = new Vector2(0, 0);
    protected int spacesMoved = 0;
    // Default action times to 5 second interval
    protected float period = 1f;
    protected float nextActionTime = 1f;
    protected bool isSafeToUpdatePath = false;
    protected int pathFoundCount = 0;
    protected bool isMoving = false;
    protected bool isTargetReached = false;
    #endregion

    #region member variables
    public Vector3[] m_path;
    protected int m_targetIndex;
    protected CharacterController m_characterController;
    private Node lastNodePosition;
    private List<Node> lastPositionNeighbors;
    private Vector3 m_lastKnownPosition;
    private Quaternion m_lookAtRotation;
    private GridSystem m_grid;
    private Coroutine lastRoutine = null;
    private bool preventExtraNodeUpdate = false;
    public Stopwatch timer;
    #endregion

    public virtual void Awake()
    {
        timer = new Stopwatch();
        if (Astar != null)
            m_grid = Astar.GetComponent<GridSystem>();
    }

    public virtual void Start()
    {
        m_characterController = GetComponent<CharacterController>();
        timer.Reset();
        timer.Start();
        PathRequestManager.RequestPath(transform.position, target.position, OnPathFound);
        lastTargetPosition = target.position;
        UnityEngine.Debug.Log("NPC Position : " + transform.position);
        UnityEngine.Debug.Log("Target Position : " + target.position);
    }

    public virtual void Update()
    {

        if (Time.time > nextActionTime) //update path setiap 1f
        {
            nextActionTime += period;
            isSafeToUpdatePath = true;
        }
        else
        {
            isSafeToUpdatePath = false;
        }

        //If we don't check !isMoving the AI may get stuck waiting to update the grid for nextActionTime.

        if (target.position != lastTargetPosition)
        {
            isMoving = true;
            UpdateNodePosition();
            UpdatePath();
            UpdateRotation();
        }

        lastTargetPosition = target.position;

    }

    public void UpdatePath()
    {
        lastNodePosition.walkable = Walkable.Passable;
        PathRequestManager.RequestPath(transform.position, target.position, OnPathFound);
    }

    public virtual void OnPathFound(Vector3[] newPath, bool pathSuccessful)
    {
        if (pathSuccessful)
        {
            pathFoundCount++;
            m_path = newPath;
            m_targetIndex = 0;

            // Stop coroutine if it is already running.
            if (lastRoutine != null)
                StopCoroutine(lastRoutine);

            lastRoutine = StartCoroutine(FollowPath());
        }
    }

    public float time;
    public virtual IEnumerator FollowPath()
    {
        Vector3 currentPath = m_path[0];

        while (true)
        {

            if (Vector3.Distance(transform.position, currentPath) < distanceToPath)
            {
                m_targetIndex++;

                // If we are done with path.
                if (m_targetIndex >= m_path.Length)
                {
                    timer.Stop();
                    UnityEngine.Debug.Log("Time Move : " + timer.ElapsedMilliseconds);
                    isMoving = false;
                    yield break;
                }


                currentPath = m_path[m_targetIndex];
            }
            float Distance = Vector3.Distance(NPC.transform.position, target.transform.position);
            UnityEngine.Debug.Log("Distance : " + Distance);
            // Occurs each frame
            //move follow path
            UpdatePosition(currentPath);

            yield return null;

        }
    }


    public virtual void UpdatePosition(Vector3 destination)
    {
        Node node = m_grid.NodeFromWorldPoint(transform.position);

        Vector3 direction = destination - transform.position;
        movementSpeed = 1;
        transform.Translate(direction.normalized * movementSpeed * Time.deltaTime, Space.World);
        UnityEngine.Debug.Log("Speed NPC : " + movementSpeed);

    }

    public virtual void UpdateRotation()
    {
        m_lastKnownPosition = target.transform.position;
        m_lookAtRotation = Quaternion.LookRotation(m_lastKnownPosition - transform.position);
      
        if (transform.rotation != m_lookAtRotation)
            transform.rotation = Quaternion.RotateTowards(transform.rotation, m_lookAtRotation, rotationSpeed * Time.deltaTime);
    }


    public void UpdateNodePosition()
    {
        Node node = m_grid.NodeFromWorldPoint(transform.position);

        if (isMoving == false)
        {
            lastPositionNeighbors = m_grid.GetNeighbours(node);
            foreach (Node n in lastPositionNeighbors)
            {
                if (n.walkable != Walkable.Impassable)
                    n.walkable = Walkable.Blocked;
            }
            node.walkable = Walkable.Blocked;
            lastNodePosition = node;
            currentPosition = new Vector2(node.gridX, node.gridY);
            return;
        }

        if (lastNodePosition != null && isMoving)
        {
            preventExtraNodeUpdate = false;
            lastPositionNeighbors = m_grid.GetNeighbours(node);
            lastNodePosition.walkable = Walkable.Passable;
            if (lastPositionNeighbors != null)
                foreach (Node n in lastPositionNeighbors)
                {
                    if (n.walkable != Walkable.Impassable)
                        n.walkable = Walkable.Passable;
                }
            if (!node.Equals(lastNodePosition))
                spacesMoved++;
        }
        else
        {
            node.walkable = Walkable.Blocked;
            lastNodePosition = node;
            currentPosition = new Vector2(node.gridX, node.gridY);
        }



    }

    public void OnDrawGizmos()
    {
        if (!drawGizmos)
            return;

        if (m_path != null)
        {
            for (int i = m_targetIndex; i < m_path.Length; i++)
            {
                Gizmos.color = Color.black;
                Gizmos.DrawCube(m_path[i], Vector3.one);

            }
        }
    }

}

this is screenshot :
https://drive.google.com/file/d/12CHc2044xElUSJn-_9oZJfRD-CVsCBjZ/view?usp=sharing

Usually path won’t have similar directions, so you may assume straight intervals already grouped. You might try to check angle between current and next intervals like this

var angle = Vector3.Angle(path[current waypoint]  - path[prev waypoint], path[next waypoint] - path[current waypoint]);

But this angle won’t ever be zero, it always will be some noticeable value. The solution for your problem is to check distance to next waypoint. If it is big enogh, for instance 20+ meters, then you have enough space to accelerate, move fast for some noticeable time, and decelerate back before next turn. You might go further and do not decelerate if angle is small, like 15 degrees or less, so fast motion looks smooth and natural.