예제 #1
0
        /// <summary>
        /// Recursive method for building an agent k-D tree.
        /// </summary>
        /// <param name="begin">The beginning agent k-D tree node node index.</param>
        /// <param name="end">The ending agent k-D tree node index.</param>
        /// <param name="node">The current agent k-D tree node index.</param>
        private void BuildAgentTreeRecursive(int begin, int end, int node)
        {
            ObstacleTreeNode   treeNode = m_outputTree[node];
            ObstacleVertexData obstacle = m_inputObstacles[begin];
            float2             pos;
            float minX, minY, maxX, maxY;

            treeNode.begin = begin;
            treeNode.end   = end;
            minX           = maxX = obstacle.pos.x;
            minY           = maxY = obstacle.pos.y;

            for (int i = begin + 1; i < end; ++i)
            {
                pos  = m_inputObstacles[i].pos;
                maxX = max(maxX, pos.x);
                minX = min(minX, pos.x);
                maxY = max(maxY, pos.y);
                minY = min(minY, pos.y);
            }

            treeNode.minX = minX;
            treeNode.maxX = maxX;
            treeNode.minY = minY;
            treeNode.maxY = maxY;

            m_outputTree[node] = treeNode;

            if (end - begin > ObstacleTreeNode.MAX_LEAF_SIZE)
            {
                // No leaf node.
                bool  isVertical = treeNode.maxX - treeNode.minX > treeNode.maxY - treeNode.minY;
                float splitValue = 0.5f * (isVertical ? treeNode.maxX + treeNode.minX : treeNode.maxY + treeNode.minY);

                int left  = begin;
                int right = end;

                while (left < right)
                {
                    while (left < right && (isVertical ? m_inputObstacles[left].pos.x : m_inputObstacles[left].pos.y) < splitValue)
                    {
                        ++left;
                    }

                    while (right > left && (isVertical ? m_inputObstacles[right - 1].pos.x : m_inputObstacles[right - 1].pos.y) >= splitValue)
                    {
                        --right;
                    }

                    if (left < right)
                    {
                        ObstacleVertexData tempAgent = m_inputObstacles[left];
                        m_inputObstacles[left]      = m_inputObstacles[right - 1];
                        m_inputObstacles[right - 1] = tempAgent;
                        ++left;
                        --right;
                    }
                }

                int leftSize = left - begin;

                if (leftSize == 0)
                {
                    ++leftSize;
                    ++left;
                    ++right;
                }

                treeNode.left      = node + 1;
                treeNode.right     = node + 2 * leftSize;
                m_outputTree[node] = treeNode;

                BuildAgentTreeRecursive(begin, left, treeNode.left);
                BuildAgentTreeRecursive(left, end, treeNode.right);
            }
        }
예제 #2
0
        private void QueryObstacleTreeRecursive(
            ref RaycastData raycast,
            ref float rangeSq,
            int node,
            ref NativeList <int> obstacleNeighbors,
            ref NativeArray <ObstacleVertexData> obstacles,
            ref NativeArray <ObstacleVertexData> refObstacles,
            ref NativeArray <ObstacleInfos> obstaclesInfos,
            ref NativeArray <ObstacleTreeNode> kdTree)
        {
            float2           center   = raycast.position;
            ObstacleTreeNode treeNode = kdTree[node];

            if (treeNode.end - treeNode.begin <= ObstacleTreeNode.MAX_LEAF_SIZE)
            {
                ObstacleVertexData o;
                ObstacleInfos      infos;
                float  top = raycast.baseline, bottom = raycast.baseline;
                float2 oPos, nPos;
                for (int i = treeNode.begin; i < treeNode.end; ++i)
                {
                    o     = obstacles[i];
                    infos = obstaclesInfos[o.infos];

                    if (!infos.collisionEnabled || (infos.layerOccupation & ~raycast.layerIgnore) == 0)
                    {
                        continue;
                    }

                    if (top < infos.baseline || bottom > infos.baseline + infos.height)
                    {
                        continue;
                    }

                    oPos = o.pos; nPos = refObstacles[o.next].pos;
                    float distSq = DistSqPointLineSegment(oPos, nPos, center);
                    if (distSq < rangeSq)
                    {
                        float raycastLeftOfLine = LeftOf(oPos, nPos, center);
                        if ((lengthsq(raycastLeftOfLine) / lengthsq(nPos - oPos)) < rangeSq && raycastLeftOfLine < 0.0f)
                        {
                            obstacleNeighbors.Add(i);
                        }
                    }
                }
            }
            else
            {
                ObstacleTreeNode leftNode  = kdTree[treeNode.left],
                                 rightNode = kdTree[treeNode.right];

                float distSqLeft = lengthsq(max(0.0f, leftNode.minX - center.x))
                                   + lengthsq(max(0.0f, center.x - leftNode.maxX))
                                   + lengthsq(max(0.0f, leftNode.minY - center.y))
                                   + lengthsq(max(0.0f, center.y - leftNode.maxY));
                float distSqRight = lengthsq(max(0.0f, rightNode.minX - center.x))
                                    + lengthsq(max(0.0f, center.x - rightNode.maxX))
                                    + lengthsq(max(0.0f, rightNode.minY - center.y))
                                    + lengthsq(max(0.0f, center.y - rightNode.maxY));

                if (distSqLeft < distSqRight)
                {
                    if (distSqLeft < rangeSq)
                    {
                        QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.left, ref obstacleNeighbors,
                                                   ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);

                        if (distSqRight < rangeSq)
                        {
                            QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.right, ref obstacleNeighbors,
                                                       ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);
                        }
                    }
                }
                else
                {
                    if (distSqRight < rangeSq)
                    {
                        QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.right, ref obstacleNeighbors,
                                                   ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);

                        if (distSqLeft < rangeSq)
                        {
                            QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.left, ref obstacleNeighbors,
                                                       ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);
                        }
                    }
                }
            }
        }