Как я могу установить расстояние newPositions, чтобы оно было короче ближе к исходным позициям squadMembers?

Question

у меня 3 формирования: В каждом формировании я вычисляю новые позиции для перемещения и добавляю их в Список новых позиций.

private void FormationTriangle()
    {
        newpositions = new List<Vector3>();

        int height = Mathf.CeilToInt((Mathf.Sqrt(8 * squadMembers.Count + 1f) - 1f) / 2);
        int slots = (int)(height * (height + 1f) / 2f);

        float verticalModifier = 1.25f;  // * 1.25f to increase horizontal space
        float horizontalModifier = 0.8f; // * 0.8f to decrease "vertical" space

        float width = 0.5f * (height - 1f);
        Vector3 startPos = new Vector3(width * horizontalModifier, 0f, (float)(height - 1f) * verticalModifier);

        int finalRowCount = height - slots + squadMembers.Count;
        for (int rowNum = 0; rowNum < height && newpositions.Count < squadMembers.Count; rowNum++)
        {
            for (int i = 0; i < rowNum + 1 && newpositions.Count < squadMembers.Count; i++)
            {
                float xOffset = 0f;

                if (rowNum + 1 == height)
                {
                    // If we're in the last row, stretch it ...
                    if (finalRowCount != 1)
                    {
                        // Unless there's only one item in the last row. 
                        // If that's the case, leave it centered.

                        xOffset = Mathf.Lerp(
                                rowNum / 2f,
                                -rowNum / 2f,
                                i / (finalRowCount - 1f)
                                ) * horizontalModifier;
                    }
                }
                else
                {
                    xOffset = (i - rowNum / 2f) * horizontalModifier;
                }

                float yOffset = (float)rowNum * verticalModifier;

                Vector3 position = new Vector3(
                        startPos.x + xOffset, 0f, startPos.y - yOffset);
                newpositions.Add(position);

            }
        }

        move = true;
        formation = Formation.Square;
    }

    private Vector3 FormationSquarePositionCalculation(int index) // call this func for all your objects
    {
        float posX = (index % columns) * gaps;
        float posY = (index / columns) * gaps;
        return new Vector3(posX, posY);
    }

    private void FormationSquare()
    {
        newpositions = new List<Vector3>();
        quaternions = new List<Quaternion>();

        for (int i = 0; i < squadMembers.Count; i++)
        {
            Vector3 pos = FormationSquarePositionCalculation(i);

            //squadMembers[i].transform.position = new Vector3(transform.position.x + pos.x, 0, transform.position.y + pos.y);
            squadMembers[i].transform.Rotate(new Vector3(0, -90, 0));
            newpositions.Add(new Vector3(transform.position.x + pos.x, 0, transform.position.y + pos.y));
        }
        move = true;
        squareFormation = true;
        formation = Formation.Circle;
    }

    private Vector3 FormationCirclePositionCalculation(Vector3 center, float radius, int index, float angleIncrement)
    {
        float ang = index * angleIncrement;
        Vector3 pos;
        pos.x = center.x + radius * Mathf.Sin(ang * Mathf.Deg2Rad);
        pos.z = center.z + radius * Mathf.Cos(ang * Mathf.Deg2Rad);
        pos.y = center.y;
        return pos;
    }

    private void FormationCircle()
    {
        newpositions = new List<Vector3>();
        quaternions = new List<Quaternion>();

        Vector3 center = transform.position;
        float radius = (float)circleRadius / 2;
        float angleIncrement = 360 / (float)numberOfSquadMembers;
        for (int i = 0; i < numberOfSquadMembers; i++)
        {
            Vector3 pos = FormationCirclePositionCalculation(center, radius, i, angleIncrement);

            var rot = Quaternion.LookRotation(center - pos);

            if (Terrain.activeTerrain == true)
                pos.y = Terrain.activeTerrain.SampleHeight(pos);
            pos.y = pos.y + yOffset;
            newpositions.Add(pos);
            quaternions.Add(rot);
        }
        move = true;
        squareFormation = false;
        formation = Formation.Triangle;
    }

Затем я перевожу членов отряда в новые позиции:

private void MoveToNextFormation()
    {
        if (randomSpeed == false)
        {
            if (step.Length > 0)
                step[0] = moveSpeed * Time.deltaTime;
        }

        for (int i = 0; i < squadMembers.Count; i++)
        {
            squadMembers[i].transform.LookAt(newpositions[i]);

            if (Vector3.Distance(squadMembers[i].transform.position, newpositions[i]) > 30f)
            {
                if (randomSpeed == true)
                {
                    squadMembers[i].transform.position =
                Vector3.MoveTowards(squadMembers[i].transform.position, newpositions[i], step[i]);
                }
                else
                {
                    squadMembers[i].transform.position =
                    Vector3.MoveTowards(squadMembers[i].transform.position, newpositions[i], step[0]);
                }
            }

            if (Vector3.Distance(squadMembers[i].transform.position, newpositions[i]) < threshold)
            {
                if (squareFormation == true)
                {
                    Vector3 degrees = new Vector3(0, 0, 0);
                    Quaternion quaternion = Quaternion.Euler(degrees);
                    squadMembers[i].transform.rotation = Quaternion.Slerp(squadMembers[i].transform.rotation, quaternion, rotateSpeed * Time.deltaTime);
                }
                else
                {
                    squadMembers[i].transform.rotation = Quaternion.Slerp(squadMembers[i].transform.rotation, quaternions[i], rotateSpeed * Time.deltaTime);
                }
            }
        }
   }

Но если я хочу, чтобы члены отряда вообще не двигались, а переходили в следующую группировку на той же позиции, что и они? Или, может быть, немного подняться, например, на расстояние 20 или 5, 30 или 300?

Я пытался добавить проверку расстояния для хода:

if (Vector3.Distance(squadMembers[i].transform.position, newpositions[i]) > 30f)

Таким образом, они переходят к следующей формации, когда расстояние больше 30. Но тогда формация выглядит не очень хорошо. Если, например, это квадратная формация, формация после того, как они закончили движение, выглядит как волна.

Пример снимка экрана:

Формирование волны