Exemplo n.º 1
0
        private void insertObstacleNeighbor(Vector3 pos, IList <KeyValuePair <float, Obstacle> > list, Obstacle obstacle, float rangeSq)
        {
            Obstacle nextObstacle = obstacle.m_NextObstacle;

            float distSq = VectorMath.distSqPointLineSegment(obstacle.m_Point, nextObstacle.m_Point, pos);

            if (distSq < rangeSq)
            {
                list.Add(new KeyValuePair <float, Obstacle>(distSq, obstacle));

                int i = list.Count - 1;
                while (i != 0 && distSq < list[i - 1].Key)
                {
                    list[i] = list[i - 1];
                    --i;
                }
                list[i] = new KeyValuePair <float, Obstacle>(distSq, obstacle);
            }
        }
Exemplo n.º 2
0
        /* Search for the best new velocity. */
        public Vector3 ComputeNewVelocity(ISpaceObject obj, Vector3 prefDir, float timeStep, KdTree kdTree, KdObstacleTree kdObstacleTree, float maxSpeed, float neighborDist, bool isUsingAvoidanceVelocity)
        {
            ComputeNeighbors(obj, kdTree, kdObstacleTree, maxSpeed, neighborDist);
            ClearOrcaLine();
            Vector3 position    = obj.GetPosition();
            float   radius      = (float)obj.GetRadius();
            Vector3 velocity    = obj.GetVelocity();
            Vector3 newVelocity = new Vector3();

            float invTimeHorizonObst = 1.0f / m_TimeHorizonObst;

            /* Create obstacle ORCA lines. */
            for (int i = 0; i < m_ObstacleNeighbors.Count; ++i)
            {
                Obstacle obstacle1 = m_ObstacleNeighbors[i].Value;
                Obstacle obstacle2 = obstacle1.m_NextObstacle;

                Vector3 relativePosition1 = obstacle1.m_Point - position;
                Vector3 relativePosition2 = obstacle2.m_Point - position;

                /*
                 * Check if velocity obstacle of obstacle is already taken care of by
                 * previously constructed obstacle ORCA lines.
                 */
                bool alreadyCovered = false;

                for (int j = 0; j < m_OrcaLines.Count; ++j)
                {
                    if (VectorMath.det(invTimeHorizonObst * relativePosition1 - m_OrcaLines[j].point, m_OrcaLines[j].direction) - invTimeHorizonObst * radius >= -VectorMath.EPSILON && VectorMath.det(invTimeHorizonObst * relativePosition2 - m_OrcaLines[j].point, m_OrcaLines[j].direction) - invTimeHorizonObst * radius >= -VectorMath.EPSILON)
                    {
                        alreadyCovered = true;
                        break;
                    }
                }

                if (alreadyCovered)
                {
                    continue;
                }

                /* Not yet covered. Check for collisions. */

                float distSq1 = VectorMath.absSq(relativePosition1);
                float distSq2 = VectorMath.absSq(relativePosition2);

                float radiusSq = VectorMath.sqr(radius);

                Vector3 obstacleVector = obstacle2.m_Point - obstacle1.m_Point;
                float   s          = -VectorMath.dot(relativePosition1, obstacleVector) / VectorMath.absSq(obstacleVector);
                float   distSqLine = VectorMath.absSq(-relativePosition1 - s * obstacleVector);

                OrcaLine line;

                if (s < 0 && distSq1 <= radiusSq)
                {
                    /* Collision with left vertex. Ignore if non-convex. */
                    if (obstacle1.m_IsConvex)
                    {
                        line.point     = new Vector3();
                        line.direction = VectorMath.normalize(new Vector3(-relativePosition1.Z, 0, relativePosition1.X));
                        AddOrcaLine(line);
                    }
                    continue;
                }
                else if (s > 1 && distSq2 <= radiusSq)
                {
                    /* Collision with right vertex. Ignore if non-convex
                     * or if it will be taken care of by neighoring obstace */
                    if (obstacle2.m_IsConvex && VectorMath.det(relativePosition2, obstacle2.m_UnitDir) >= 0)
                    {
                        line.point     = new Vector3();
                        line.direction = VectorMath.normalize(new Vector3(-relativePosition2.Z, 0, relativePosition2.X));
                        AddOrcaLine(line);
                    }
                    continue;
                }
                else if (s >= 0 && s < 1 && distSqLine <= radiusSq)
                {
                    /* Collision with obstacle segment. */
                    line.point     = new Vector3();
                    line.direction = -obstacle1.m_UnitDir;
                    AddOrcaLine(line);
                    continue;
                }

                /*
                 * No collision.
                 * Compute legs. When obliquely viewed, both legs can come from a single
                 * vertex. Legs extend cut-off line when nonconvex vertex.
                 */

                Vector3 leftLegDirection, rightLegDirection;

                if (s < 0 && distSqLine <= radiusSq)
                {
                    /*
                     * Obstacle viewed obliquely so that left vertex
                     * defines velocity obstacle.
                     */
                    if (!obstacle1.m_IsConvex)
                    {
                        /* Ignore obstacle. */
                        continue;
                    }

                    obstacle2 = obstacle1;

                    float leg1 = VectorMath.sqrt(distSq1 - radiusSq);
                    leftLegDirection  = new Vector3(relativePosition1.X * leg1 - relativePosition1.Z * radius, 0, relativePosition1.X * radius + relativePosition1.Z * leg1) / distSq1;
                    rightLegDirection = new Vector3(relativePosition1.X * leg1 + relativePosition1.Z * radius, 0, -relativePosition1.X * radius + relativePosition1.Z * leg1) / distSq1;
                }
                else if (s > 1 && distSqLine <= radiusSq)
                {
                    /*
                     * Obstacle viewed obliquely so that
                     * right vertex defines velocity obstacle.
                     */
                    if (!obstacle2.m_IsConvex)
                    {
                        /* Ignore obstacle. */
                        continue;
                    }

                    obstacle1 = obstacle2;

                    float leg2 = VectorMath.sqrt(distSq2 - radiusSq);
                    leftLegDirection  = new Vector3(relativePosition2.X * leg2 - relativePosition2.Z * radius, 0, relativePosition2.X * radius + relativePosition2.Z * leg2) / distSq2;
                    rightLegDirection = new Vector3(relativePosition2.X * leg2 + relativePosition2.Z * radius, 0, -relativePosition2.X * radius + relativePosition2.Z * leg2) / distSq2;
                }
                else
                {
                    /* Usual situation. */
                    if (obstacle1.m_IsConvex)
                    {
                        float leg1 = VectorMath.sqrt(distSq1 - radiusSq);
                        leftLegDirection = new Vector3(relativePosition1.X * leg1 - relativePosition1.Z * radius, 0, relativePosition1.X * radius + relativePosition1.Z * leg1) / distSq1;
                    }
                    else
                    {
                        /* Left vertex non-convex; left leg extends cut-off line. */
                        leftLegDirection = -obstacle1.m_UnitDir;
                    }

                    if (obstacle2.m_IsConvex)
                    {
                        float leg2 = VectorMath.sqrt(distSq2 - radiusSq);
                        rightLegDirection = new Vector3(relativePosition2.X * leg2 + relativePosition2.Z * radius, 0, -relativePosition2.X * radius + relativePosition2.Z * leg2) / distSq2;
                    }
                    else
                    {
                        /* Right vertex non-convex; right leg extends cut-off line. */
                        rightLegDirection = obstacle1.m_UnitDir;
                    }
                }

                /*
                 * Legs can never point into neighboring edge when convex vertex,
                 * take cutoff-line of neighboring edge instead. If velocity projected on
                 * "foreign" leg, no constraint is added.
                 */

                Obstacle leftNeighbor = obstacle1.m_PrevObstacle;

                bool isLeftLegForeign  = false;
                bool isRightLegForeign = false;

                if (obstacle1.m_IsConvex && VectorMath.det(leftLegDirection, -leftNeighbor.m_UnitDir) >= 0.0f)
                {
                    /* Left leg points into obstacle. */
                    leftLegDirection = -leftNeighbor.m_UnitDir;
                    isLeftLegForeign = true;
                }

                if (obstacle2.m_IsConvex && VectorMath.det(rightLegDirection, obstacle2.m_UnitDir) <= 0.0f)
                {
                    /* Right leg points into obstacle. */
                    rightLegDirection = obstacle2.m_UnitDir;
                    isRightLegForeign = true;
                }

                /* Compute cut-off centers. */
                Vector3 leftCutoff  = invTimeHorizonObst * (obstacle1.m_Point - position);
                Vector3 rightCutoff = invTimeHorizonObst * (obstacle2.m_Point - position);
                Vector3 cutoffVec   = rightCutoff - leftCutoff;

                /* Project current velocity on velocity obstacle. */

                /* Check if current velocity is projected on cutoff circles. */
                float t      = (obstacle1 == obstacle2 ? 0.5f : VectorMath.dot((velocity - leftCutoff), cutoffVec) / VectorMath.absSq(cutoffVec));
                float tLeft  = VectorMath.dot((velocity - leftCutoff), leftLegDirection);
                float tRight = VectorMath.dot((velocity - rightCutoff), rightLegDirection);

                if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f))
                {
                    /* Project on left cut-off circle. */
                    Vector3 unitW = VectorMath.normalize(velocity - leftCutoff);

                    line.direction = new Vector3(unitW.Z, 0, -unitW.X);
                    line.point     = leftCutoff + radius * invTimeHorizonObst * unitW;
                    AddOrcaLine(line);
                    continue;
                }
                else if (t > 1.0f && tRight < 0.0f)
                {
                    /* Project on right cut-off circle. */
                    Vector3 unitW = VectorMath.normalize(velocity - rightCutoff);

                    line.direction = new Vector3(unitW.Z, 0, -unitW.X);
                    line.point     = rightCutoff + radius * invTimeHorizonObst * unitW;
                    AddOrcaLine(line);
                    continue;
                }

                /*
                 * Project on left leg, right leg, or cut-off line, whichever is closest
                 * to velocity.
                 */
                float distSqCutoff = ((t <0.0f || t> 1.0f || obstacle1 == obstacle2) ? float.PositiveInfinity : VectorMath.absSq(velocity - (leftCutoff + t * cutoffVec)));
                float distSqLeft   = ((tLeft < 0.0f) ? float.PositiveInfinity : VectorMath.absSq(velocity - (leftCutoff + tLeft * leftLegDirection)));
                float distSqRight  = ((tRight < 0.0f) ? float.PositiveInfinity : VectorMath.absSq(velocity - (rightCutoff + tRight * rightLegDirection)));

                if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
                {
                    /* Project on cut-off line. */
                    line.direction = -obstacle1.m_UnitDir;
                    line.point     = leftCutoff + radius * invTimeHorizonObst * new Vector3(-line.direction.Z, 0, line.direction.X);
                    AddOrcaLine(line);
                    continue;
                }
                else if (distSqLeft <= distSqRight)
                {
                    /* Project on left leg. */
                    if (isLeftLegForeign)
                    {
                        continue;
                    }

                    line.direction = leftLegDirection;
                    line.point     = leftCutoff + radius * invTimeHorizonObst * new Vector3(-line.direction.Z, 0, line.direction.X);
                    AddOrcaLine(line);
                    continue;
                }
                else
                {
                    /* Project on right leg. */
                    if (isRightLegForeign)
                    {
                        continue;
                    }

                    line.direction = -rightLegDirection;
                    line.point     = rightCutoff + radius * invTimeHorizonObst * new Vector3(-line.direction.Z, 0, line.direction.X);
                    AddOrcaLine(line);
                    continue;
                }
            }

            int numObstLines = m_OrcaLines.Count;

            float invTimeHorizon = 1.0f / m_TimeHorizon;

            /* Create obj ORCA lines. */
            for (int i = 0; i < m_ObjNeighbors.Count; ++i)
            {
                KdTreeObject other = m_ObjNeighbors[i].Value;
                if (obj == other.SpaceObject)
                {
                    continue;
                }

                Vector3 relativePosition = other.Position - position;
                Vector3 relativeVelocity = velocity - other.Velocity;
                float   distSq           = VectorMath.absSq(relativePosition);
                float   combinedRadius   = radius + other.Radius;
                float   combinedRadiusSq = VectorMath.sqr(combinedRadius);

                OrcaLine line;
                Vector3  u;

                if (distSq > combinedRadiusSq)
                {
                    /* No collision. */
                    Vector3 w = relativeVelocity - invTimeHorizon * relativePosition;
                    /* Vector from cutoff center to relative velocity. */
                    float wLengthSq = VectorMath.absSq(w);

                    float dotProduct1 = VectorMath.dot(w, relativePosition);

                    if (dotProduct1 < 0.0f && VectorMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
                    {
                        /* Project on cut-off circle. */
                        float   wLength = VectorMath.sqrt(wLengthSq);
                        Vector3 unitW   = w / wLength;

                        line.direction = new Vector3(unitW.Z, 0, -unitW.X);
                        u = (combinedRadius * invTimeHorizon - wLength) * unitW;
                    }
                    else
                    {
                        /* Project on legs. */
                        float leg = VectorMath.sqrt(distSq - combinedRadiusSq);

                        if (VectorMath.det(relativePosition, w) > 0.0f)
                        {
                            /* Project on left leg. */
                            line.direction = new Vector3(relativePosition.X * leg - relativePosition.Z * combinedRadius, 0, relativePosition.X * combinedRadius + relativePosition.Z * leg) / distSq;
                        }
                        else
                        {
                            /* Project on right leg. */
                            line.direction = -new Vector3(relativePosition.X * leg + relativePosition.Z * combinedRadius, 0, -relativePosition.X * combinedRadius + relativePosition.Z * leg) / distSq;
                        }

                        float dotProduct2 = VectorMath.dot(relativeVelocity, line.direction);

                        u = dotProduct2 * line.direction - relativeVelocity;
                    }
                }
                else
                {
                    /* Collision. Project on cut-off circle of time timeStep. */
                    float invTimeStep = 1.0f / timeStep;

                    /* Vector from cutoff center to relative velocity. */
                    Vector3 w = relativeVelocity - invTimeStep * relativePosition;

                    float wLength = VectorMath.abs(w);
                    if (wLength < VectorMath.EPSILON)
                    {
                        //LogSystem.Error("RvoAlgorithm error: Objects already overlap ! {0}({1}):{2}-{3}({4}):{5}", obj.GetID(), obj.GetObjType(), obj.GetPosition().ToString(), other.SpaceObject.GetID(), other.SpaceObject.GetObjType(), other.Position.ToString());
                        continue;
                    }
                    Vector3 unitW = w / wLength;

                    line.direction = new Vector3(unitW.Z, 0, -unitW.X);
                    u = (combinedRadius * invTimeStep - wLength) * unitW;
                }

                line.point = velocity + 0.5f * u;
                AddOrcaLine(line);
            }

            Vector3 avoidanceVelocity;

            if (isUsingAvoidanceVelocity)
            {
                avoidanceVelocity = obj.GetVelocity();
            }
            else
            {
                avoidanceVelocity = new Vector3();
            }

            int lineFail = LinearProgram2(m_OrcaLines, maxSpeed, prefDir, avoidanceVelocity, false, ref newVelocity);

            if (lineFail < m_OrcaLines.Count)
            {
                LinearProgram3(m_OrcaLines, numObstLines, lineFail, maxSpeed, avoidanceVelocity, ref newVelocity);
            }
            return(newVelocity);
        }
Exemplo n.º 3
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);
                }
            }
        }