예제 #1
0
        private bool CheckVisibilityRecursive(Vector3 q1, Vector3 q2, float radius, ObstacleTreeNode node)
        {
            if (node == null)
            {
                return(true);
            }
            else
            {
                Obstacle obstacle1 = node.m_Obstacle;
                Obstacle obstacle2 = obstacle1.m_NextObstacle;

                float q1LeftOfI  = VectorMath.leftOf(obstacle1.m_Point, obstacle2.m_Point, q1);
                float q2LeftOfI  = VectorMath.leftOf(obstacle1.m_Point, obstacle2.m_Point, q2);
                float invLengthI = 1.0f / VectorMath.absSq(obstacle2.m_Point - obstacle1.m_Point);

                if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f)
                {
                    return(CheckVisibilityRecursive(q1, q2, radius, node.m_Left) && ((VectorMath.sqr(q1LeftOfI) * invLengthI >= VectorMath.sqr(radius) && VectorMath.sqr(q2LeftOfI) * invLengthI >= VectorMath.sqr(radius)) || CheckVisibilityRecursive(q1, q2, radius, node.m_Right)));
                }
                else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f)
                {
                    return(CheckVisibilityRecursive(q1, q2, radius, node.m_Right) && ((VectorMath.sqr(q1LeftOfI) * invLengthI >= VectorMath.sqr(radius) && VectorMath.sqr(q2LeftOfI) * invLengthI >= VectorMath.sqr(radius)) || CheckVisibilityRecursive(q1, q2, radius, node.m_Left)));
                }
                else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f)
                {
                    /* One can see through obstacle from left to right. */
                    return(CheckVisibilityRecursive(q1, q2, radius, node.m_Left) && CheckVisibilityRecursive(q1, q2, radius, node.m_Right));
                }
                else
                {
                    float point1LeftOfQ = VectorMath.leftOf(q1, q2, obstacle1.m_Point);
                    float point2LeftOfQ = VectorMath.leftOf(q1, q2, obstacle2.m_Point);
                    float invLengthQ    = 1.0f / VectorMath.absSq(q2 - q1);

                    return(point1LeftOfQ * point2LeftOfQ >= 0.0f && VectorMath.sqr(point1LeftOfQ) * invLengthQ > VectorMath.sqr(radius) && VectorMath.sqr(point2LeftOfQ) * invLengthQ > VectorMath.sqr(radius) && CheckVisibilityRecursive(q1, q2, radius, node.m_Left) && CheckVisibilityRecursive(q1, q2, radius, node.m_Right));
                }
            }
        }
예제 #2
0
        private ObstacleTreeNode BuildRecursive(IList <Obstacle> obstacles)
        {
            if (obstacles.Count == 0)
            {
                return(null);
            }
            else
            {
                ObstacleTreeNode node = new ObstacleTreeNode();

                int optimalSplit = 0;
                int minLeft      = obstacles.Count;
                int minRight     = obstacles.Count;

                for (int i = 0; i < obstacles.Count; ++i)
                {
                    int leftSize  = 0;
                    int rightSize = 0;

                    Obstacle obstacleI1 = obstacles[i];
                    Obstacle obstacleI2 = obstacleI1.m_NextObstacle;

                    /* Compute optimal split node. */
                    for (int j = 0; j < obstacles.Count; ++j)
                    {
                        if (i == j)
                        {
                            continue;
                        }

                        Obstacle obstacleJ1 = obstacles[j];
                        Obstacle obstacleJ2 = obstacleJ1.m_NextObstacle;

                        float j1LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ1.m_Point);
                        float j2LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ2.m_Point);

                        if (j1LeftOfI >= -VectorMath.EPSILON && j2LeftOfI >= -VectorMath.EPSILON)
                        {
                            ++leftSize;
                        }
                        else if (j1LeftOfI <= VectorMath.EPSILON && j2LeftOfI <= VectorMath.EPSILON)
                        {
                            ++rightSize;
                        }
                        else
                        {
                            ++leftSize;
                            ++rightSize;
                        }

                        if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) >= new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
                        {
                            break;
                        }
                    }

                    if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) < new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
                    {
                        minLeft      = leftSize;
                        minRight     = rightSize;
                        optimalSplit = i;
                    }
                }

                {
                    /* Build split node. */
                    IList <Obstacle> leftObstacles = new List <Obstacle>(minLeft);
                    for (int n = 0; n < minLeft; ++n)
                    {
                        leftObstacles.Add(null);
                    }
                    IList <Obstacle> rightObstacles = new List <Obstacle>(minRight);
                    for (int n = 0; n < minRight; ++n)
                    {
                        rightObstacles.Add(null);
                    }

                    int leftCounter  = 0;
                    int rightCounter = 0;
                    int i            = optimalSplit;

                    Obstacle obstacleI1 = obstacles[i];
                    Obstacle obstacleI2 = obstacleI1.m_NextObstacle;

                    for (int j = 0; j < obstacles.Count; ++j)
                    {
                        if (i == j)
                        {
                            continue;
                        }

                        Obstacle obstacleJ1 = obstacles[j];
                        Obstacle obstacleJ2 = obstacleJ1.m_NextObstacle;

                        float j1LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ1.m_Point);
                        float j2LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ2.m_Point);

                        if (j1LeftOfI >= -VectorMath.EPSILON && j2LeftOfI >= -VectorMath.EPSILON)
                        {
                            leftObstacles[leftCounter++] = obstacles[j];
                        }
                        else if (j1LeftOfI <= VectorMath.EPSILON && j2LeftOfI <= VectorMath.EPSILON)
                        {
                            rightObstacles[rightCounter++] = obstacles[j];
                        }
                        else
                        {
                            /* Split obstacle j. */
                            float t = VectorMath.det(obstacleI2.m_Point - obstacleI1.m_Point, obstacleJ1.m_Point - obstacleI1.m_Point) / VectorMath.det(obstacleI2.m_Point - obstacleI1.m_Point, obstacleJ1.m_Point - obstacleJ2.m_Point);

                            Vector3 splitpoint = obstacleJ1.m_Point + t * (obstacleJ2.m_Point - obstacleJ1.m_Point);

                            Obstacle newObstacle = new Obstacle();
                            newObstacle.m_Point        = splitpoint;
                            newObstacle.m_PrevObstacle = obstacleJ1;
                            newObstacle.m_NextObstacle = obstacleJ2;
                            newObstacle.m_IsConvex     = true;
                            newObstacle.m_UnitDir      = obstacleJ1.m_UnitDir;

                            m_Obstacles.Add(newObstacle);

                            obstacleJ1.m_NextObstacle = newObstacle;
                            obstacleJ2.m_PrevObstacle = newObstacle;

                            if (j1LeftOfI > 0.0f)
                            {
                                leftObstacles[leftCounter++]   = obstacleJ1;
                                rightObstacles[rightCounter++] = newObstacle;
                            }
                            else
                            {
                                rightObstacles[rightCounter++] = obstacleJ1;
                                leftObstacles[leftCounter++]   = newObstacle;
                            }
                        }
                    }

                    node.m_Obstacle = obstacleI1;
                    node.m_Left     = BuildRecursive(leftObstacles);
                    node.m_Right    = BuildRecursive(rightObstacles);
                    return(node);
                }
            }
        }