예제 #1
0
        /// <summary>
        /// Generate Triangles from the navigation Datas
        /// Then link triangles when they share 2 vertices
        /// </summary>
        private void GenerateTriangles()
        {
            Triangles = new List <Triangle>();
            int _i, _j = 0;

            for (_i = 0; _i < NavigationDatas.Indices.Length; _i += 3)
            {
                Triangles.Add(new Triangle(_j, new int[3] {
                    NavigationDatas.Indices[_i], NavigationDatas.Indices[_i + 1], NavigationDatas.Indices[_i + 2]
                }));
                _j++;
            }
            for (_i = 0; _i < Triangles.Count; _i++)
            {
                for (_j = 0; _j < Triangles.Count; _j++)
                {
                    if (_i == _j)
                    {
                        continue;
                    }
                    if (GeometryHelper2D.VerticesInCommon(Triangles[_i], Triangles[_j]) == 2)
                    {
                        Triangles[_i].LinkedTriangles.Add(_j);
                    }
                }
            }
        }
예제 #2
0
        /// <summary>
        /// Build a path using Astar resources
        /// Get the last point and get all its parent to build the path
        /// </summary>
        /// <param name="_pathToBuild">Astar resources</param>
        static Vector2[] BuildPath(Dictionary <int, int> _pathToBuild, Triangle _firstTriangle, Triangle _lastTriangle, Vector2 _origin, Vector2 _destination, List <Triangle> _trianglesDatas)
        {
            if (_pathToBuild.Count <= 1)
            {
                List <Vector2> _pathPoints = new List <Vector2>();
                _pathPoints.Add(_origin);
                _pathPoints.Add(_destination);
                return(_pathPoints.ToArray());
            }

            #region BuildingAbsolutePath
            // Building absolute path -> Link all triangle's CenterPosition together
            // Adding _origin and destination to the path
            Triangle        _currentTriangle      = _trianglesDatas[_pathToBuild[_lastTriangle.Index]];
            List <Triangle> _absoluteTrianglePath = new List <Triangle>();
            while (_currentTriangle != _firstTriangle)
            {
                _absoluteTrianglePath.Add(_currentTriangle);
                _currentTriangle = _trianglesDatas[_pathToBuild[_currentTriangle.Index]];
            }
            _absoluteTrianglePath.Add(_currentTriangle);
            //Reverse the path to start at the origin
            _absoluteTrianglePath.Reverse();
            #endregion


            //Create the simplifiedPath
            List <Vector2> _simplifiedPath = new List <Vector2>()
            {
                _origin
            };

            if (_absoluteTrianglePath.Count <= 1)
            {
                _simplifiedPath.Add(_destination);
                return(_simplifiedPath.ToArray());
            }

            //Simplify the path with Funnel Algorithm

            //Create both portals vertices arrays
            Vector2[] _leftVertices  = new Vector2[_absoluteTrianglePath.Count - 1];
            Vector2[] _rightVertices = new Vector2[_absoluteTrianglePath.Count - 1];

            //Create the apex
            Vector2 _apex = _origin;

            //Initialize portal vertices
            Vector2 _startLinePoint = Vector2.zero;
            Vector2 _endLinePoint   = Vector2.zero;
            Vector2 _vertex1        = Vector2.zero;
            Vector2 _vertex2        = Vector2.zero;


            #region Initialise Portal Vertices

            //Initialize portal vertices between each triangles
            for (int i = 1; i < _absoluteTrianglePath.Count - 1; i++)
            {
                _currentTriangle = _absoluteTrianglePath[i];
                _startLinePoint  = NavMeshManager.Instance.GetCenterPosition(_currentTriangle);
                _endLinePoint    = NavMeshManager.Instance.GetCenterPosition(_absoluteTrianglePath[i + 1]);
                for (int j = 0; j < _currentTriangle.Vertices.Length; j++)
                {
                    int k = j + 1 >= _currentTriangle.Vertices.Length ? 0 : j + 1;
                    _vertex1 = NavMeshManager.Instance.NavigationDatas.Vertices[_currentTriangle.Vertices[j]];
                    _vertex2 = NavMeshManager.Instance.NavigationDatas.Vertices[_currentTriangle.Vertices[k]];

                    if (GeometryHelper2D.IsIntersecting(_startLinePoint, _endLinePoint, _vertex1, _vertex2))
                    {
                        //Debug.Log(_startLinePoint + "///" + _endLinePoint + " intersect with " + _vertex1 + "///" + _vertex2);
                        if (GeometryHelper2D.AngleSign(_startLinePoint, _endLinePoint, _vertex1) > 0)
                        {
                            _leftVertices[i]  = _vertex2;
                            _rightVertices[i] = _vertex1;
                        }
                        else
                        {
                            _leftVertices[i]  = _vertex1;
                            _rightVertices[i] = _vertex2;
                        }
                        break;
                    }
                }
            }

            //Initialize start portal vertices
            _startLinePoint  = _origin;
            _endLinePoint    = NavMeshManager.Instance.GetCenterPosition(_absoluteTrianglePath[1]);
            _currentTriangle = _absoluteTrianglePath[0];

            for (int j = 0; j < _currentTriangle.Vertices.Length; j++)
            {
                int k = j + 1 >= _currentTriangle.Vertices.Length ? 0 : j + 1;
                _vertex1 = NavMeshManager.Instance.NavigationDatas.Vertices[_currentTriangle.Vertices[j]];
                _vertex2 = NavMeshManager.Instance.NavigationDatas.Vertices[_currentTriangle.Vertices[k]];
                if (GeometryHelper2D.IsIntersecting(_startLinePoint, _endLinePoint, _vertex1, _vertex2))
                {
                    if (GeometryHelper2D.AngleSign(_startLinePoint, _endLinePoint, _vertex1) > 0)
                    {
                        _leftVertices[0]  = _vertex2;
                        _rightVertices[0] = _vertex1;
                    }
                    else
                    {
                        _leftVertices[0]  = _vertex1;
                        _rightVertices[0] = _vertex2;
                    }
                    break;
                }
            }

            // Initialise end portal vertices -> Close the funnel

            _leftVertices[_leftVertices.Length - 1]   = _destination;
            _rightVertices[_rightVertices.Length - 1] = _destination;
            #endregion

            //Step through the channel
            Vector2 _currentLeftVertex;
            Vector2 _nextLeftVertex;
            Vector2 _currentRightVertex;
            Vector2 _nextRightVertex;

            //Set left and right indexes
            int _leftIndex  = 0;
            int _rightIndex = 0;


            for (int i = 1; i < _absoluteTrianglePath.Count - 1; i++)
            {
                _currentLeftVertex = _leftVertices[_leftIndex];
                _nextLeftVertex    = _leftVertices[i];

                _currentRightVertex = _rightVertices[_rightIndex];
                _nextRightVertex    = _rightVertices[i];

                //If the new left vertex is different process
                if (_nextLeftVertex != _currentLeftVertex && i > _leftIndex)
                {
                    //If the next point does not widden funnel, update
                    if (GeometryHelper2D.AngleSign(_apex, _currentLeftVertex, _nextLeftVertex) >= 0)
                    {
                        //if next side cross the other side, place new apex
                        if (GeometryHelper2D.AngleSign(_apex, _currentRightVertex, _nextLeftVertex) > 0)
                        {
                            // Set the new Apex
                            _apex = _currentRightVertex;
                            _simplifiedPath.Add(_apex);

                            //Set i to the apex index to be at the good index on the next loop
                            i = _rightIndex;
                            // Find new right vertex.
                            for (int j = _rightIndex; j < _rightVertices.Length; j++)
                            {
                                if (_rightVertices[j] != _apex)
                                {
                                    _rightIndex = j;
                                    break;
                                }
                            }
                            _leftIndex = i;
                            i--;
                            continue;
                        }
                        // else skip to the next vertex
                        else
                        {
                            _leftIndex = i;
                        }
                    }
                    //else skip
                }
                //else skip


                // If the right vertex is different process
                if (_nextRightVertex != _currentRightVertex && i > _rightIndex)
                {
                    //If the next point does not widden funnel, update
                    if (GeometryHelper2D.AngleSign(_apex, _currentRightVertex, _nextRightVertex) <= 0)
                    {
                        //if next side cross the other side, place new apex
                        if (GeometryHelper2D.AngleSign(_apex, _currentLeftVertex, _nextRightVertex) < 0)
                        {
                            //Set the new Apex
                            _apex = _currentLeftVertex;
                            _simplifiedPath.Add(_apex);

                            //Set i to the apex index to be at the good index on the next loop
                            i = _leftIndex;
                            // Find next Left Index
                            for (int j = _leftIndex; j < _leftVertices.Length; j++)
                            {
                                if (_leftVertices[j] != _apex)
                                {
                                    _leftIndex = j;
                                    break;
                                }
                            }
                            _rightIndex = i;
                            i--;
                            continue;
                        }
                        //else skip to the next vertex
                        else
                        {
                            _rightIndex = i;
                        }
                    }
                    //else skip
                }
                //else skip
            }

            _simplifiedPath.Add(_destination);

            if (_simplifiedPath.Contains(Vector2.zero))
            {
                _simplifiedPath.Remove(Vector2.zero);
            }
            //Set the simplifiedPath
            return(_simplifiedPath.ToArray());
        }
예제 #3
0
        /// <summary>
        /// Make the agent move along the path and avoid the obstacles
        /// </summary>
        protected override void MovableUpdate()
        {
            if (currentPath == null || currentPath.Length == 0)
            {
                StopAgent();
                return;
            }
            if (isMoving && (currentIndex > 0))
            {
                Vector2 _previousPosition = currentPath[currentIndex - 1];
                Vector2 _nextPosition     = currentPath[currentIndex];

                if (Vector2.Distance(transform.position, LastPosition) < collider.bounds.extents.x)
                {
                    StopAgent();
                }
                else
                {
                    if (Vector2.Distance(transform.position, _nextPosition) <= collider.bounds.extents.x)
                    {
                        //Increasing path index
                        currentIndex++;
                        _previousPosition = currentPath[currentIndex - 1];
                        _nextPosition     = currentPath[currentIndex];
                    }
                    /* Get the predicted Velocity and the Predicted position*/
                    Vector2      _predictedPosition = (Vector2)transform.position + velocity.normalized;
                    RaycastHit2D _obstacle;
                    if (CastCollider(_predictedPosition, out _obstacle))
                    {
                        Vector2 _dir = (_obstacle.point - (Vector2)_obstacle.transform.position).normalized;
                        Avoid(_dir);
                    }
                    else
                    {
                        /*Get the transposed Position of the predicted position on the segment between the previous and the next point
                         * The agent has to get closer while it's to far away from the path
                         */
                        Vector2 _normalPoint = GeometryHelper2D.GetNormalPoint(_predictedPosition, _previousPosition, _nextPosition);

                        /* Direction of the segment between the previous and the next position normalized in order to go further on the path
                         * Targeted position is the normal point + an offset defined by the direction of the segment to go a little further on the path
                         * If the target is out of the segment between the previous and the next position, the target position is the next position
                         */
                        Vector2 _dir            = (_nextPosition - _previousPosition).normalized;
                        Vector2 _targetPosition = _normalPoint + _dir;
                        if (!GeometryHelper2D.PointContainedInSegment(_previousPosition, _nextPosition, _targetPosition))
                        {
                            _targetPosition = _nextPosition;
                        }

                        /* Distance between the predicted position and the normal point on the segment
                         * If the distance is greater than the radius, it has to steer to get closer
                         */
                        float _distance      = Vector2.Distance(_predictedPosition, _normalPoint);
                        float _scalarProduct = Vector2.Dot(velocity, _dir);
                        if (_distance > collider.bounds.extents.x || _scalarProduct == -1 || velocity == Vector2.zero)
                        {
                            Seek(_targetPosition);
                        }
                    }
                    velocity.Normalize();
                    Move(velocity);
                }
            }
            base.MovableUpdate();
        }