Esempio n. 1
0
        /**
         * <summary>Computes the new velocity of this agent.</summary>
         */
        internal void computeNewVelocity()
        {
            orcaLines_.Clear();

            float invTimeHorizonObst = 1.0f / timeHorizonObst_;

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

                Vector2 relativePosition1 = obstacle1.point_ - position_;
                Vector2 relativePosition2 = obstacle2.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 < orcaLines_.Count; ++j)
                {
                    if (RVOMath.det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON && RVOMath.det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON)
                    {
                        alreadyCovered = true;

                        break;
                    }
                }

                if (alreadyCovered)
                {
                    continue;
                }

                /* Not yet covered. Check for collisions. */
                float distSq1 = RVOMath.absSq(relativePosition1);
                float distSq2 = RVOMath.absSq(relativePosition2);

                float radiusSq = RVOMath.sqr(radius_);

                Vector2 obstacleVector = obstacle2.point_ - obstacle1.point_;
                float   s          = (-relativePosition1 * obstacleVector) / RVOMath.absSq(obstacleVector);
                float   distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector);

                Line line;

                if (s < 0.0f && distSq1 <= radiusSq)
                {
                    /* Collision with left vertex. Ignore if non-convex. */
                    if (obstacle1.convex_)
                    {
                        line.point     = new Vector2(0.0f, 0.0f);
                        line.direction = RVOMath.normalize(new Vector2(-relativePosition1.y(), relativePosition1.x()));
                        orcaLines_.Add(line);
                    }

                    continue;
                }
                else if (s > 1.0f && distSq2 <= radiusSq)
                {
                    /*
                     * Collision with right vertex. Ignore if non-convex or if
                     * it will be taken care of by neighboring obstacle.
                     */
                    if (obstacle2.convex_ && RVOMath.det(relativePosition2, obstacle2.direction_) >= 0.0f)
                    {
                        line.point     = new Vector2(0.0f, 0.0f);
                        line.direction = RVOMath.normalize(new Vector2(-relativePosition2.y(), relativePosition2.x()));
                        orcaLines_.Add(line);
                    }

                    continue;
                }
                else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq)
                {
                    /* Collision with obstacle segment. */
                    line.point     = new Vector2(0.0f, 0.0f);
                    line.direction = -obstacle1.direction_;
                    orcaLines_.Add(line);

                    continue;
                }

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

                Vector2 leftLegDirection, rightLegDirection;

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

                    obstacle2 = obstacle1;

                    float leg1 = RVOMath.sqrt(distSq1 - radiusSq);
                    leftLegDirection  = new Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
                    rightLegDirection = new Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
                }
                else if (s > 1.0f && distSqLine <= radiusSq)
                {
                    /*
                     * Obstacle viewed obliquely so that
                     * right vertex defines velocity obstacle.
                     */
                    if (!obstacle2.convex_)
                    {
                        /* Ignore obstacle. */
                        continue;
                    }

                    obstacle1 = obstacle2;

                    float leg2 = RVOMath.sqrt(distSq2 - radiusSq);
                    leftLegDirection  = new Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
                    rightLegDirection = new Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
                }
                else
                {
                    /* Usual situation. */
                    if (obstacle1.convex_)
                    {
                        float leg1 = RVOMath.sqrt(distSq1 - radiusSq);
                        leftLegDirection = new Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
                    }
                    else
                    {
                        /* Left vertex non-convex; left leg extends cut-off line. */
                        leftLegDirection = -obstacle1.direction_;
                    }

                    if (obstacle2.convex_)
                    {
                        float leg2 = RVOMath.sqrt(distSq2 - radiusSq);
                        rightLegDirection = new Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
                    }
                    else
                    {
                        /* Right vertex non-convex; right leg extends cut-off line. */
                        rightLegDirection = obstacle1.direction_;
                    }
                }

                /*
                 * 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.previous_;

                bool isLeftLegForeign  = false;
                bool isRightLegForeign = false;

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

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

                /* Compute cut-off centers. */
                Vector2 leftCutOff   = invTimeHorizonObst * (obstacle1.point_ - position_);
                Vector2 rightCutOff  = invTimeHorizonObst * (obstacle2.point_ - position_);
                Vector2 cutOffVector = rightCutOff - leftCutOff;

                /* Project current velocity on velocity obstacle. */

                /* Check if current velocity is projected on cutoff circles. */
                float t      = obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutOff) * cutOffVector) / RVOMath.absSq(cutOffVector);
                float tLeft  = (velocity_ - leftCutOff) * leftLegDirection;
                float tRight = (velocity_ - rightCutOff) * rightLegDirection;

                if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f))
                {
                    /* Project on left cut-off circle. */
                    Vector2 unitW = RVOMath.normalize(velocity_ - leftCutOff);

                    line.direction = new Vector2(unitW.y(), -unitW.x());
                    line.point     = leftCutOff + radius_ * invTimeHorizonObst * unitW;
                    orcaLines_.Add(line);

                    continue;
                }
                else if (t > 1.0f && tRight < 0.0f)
                {
                    /* Project on right cut-off circle. */
                    Vector2 unitW = RVOMath.normalize(velocity_ - rightCutOff);

                    line.direction = new Vector2(unitW.y(), -unitW.x());
                    line.point     = rightCutOff + radius_ * invTimeHorizonObst * unitW;
                    orcaLines_.Add(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 : RVOMath.absSq(velocity_ - (leftCutOff + t * cutOffVector));
                float distSqLeft   = tLeft < 0.0f ? float.PositiveInfinity : RVOMath.absSq(velocity_ - (leftCutOff + tLeft * leftLegDirection));
                float distSqRight  = tRight < 0.0f ? float.PositiveInfinity : RVOMath.absSq(velocity_ - (rightCutOff + tRight * rightLegDirection));

                if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
                {
                    /* Project on cut-off line. */
                    line.direction = -obstacle1.direction_;
                    line.point     = leftCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y(), line.direction.x());
                    orcaLines_.Add(line);

                    continue;
                }

                if (distSqLeft <= distSqRight)
                {
                    /* Project on left leg. */
                    if (isLeftLegForeign)
                    {
                        continue;
                    }

                    line.direction = leftLegDirection;
                    line.point     = leftCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y(), line.direction.x());
                    orcaLines_.Add(line);

                    continue;
                }

                /* Project on right leg. */
                if (isRightLegForeign)
                {
                    continue;
                }

                line.direction = -rightLegDirection;
                line.point     = rightCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y(), line.direction.x());
                orcaLines_.Add(line);
            }

            int numObstLines = orcaLines_.Count;

            float invTimeHorizon = 1.0f / timeHorizon_;

            /* Create agent ORCA lines. */
            for (int i = 0; i < agentNeighbors_.Count; ++i)
            {
                Agent other = agentNeighbors_[i].Value;

                Vector2 relativePosition = other.position_ - position_;
                Vector2 relativeVelocity = velocity_ - other.velocity_;
                float   distSq           = RVOMath.absSq(relativePosition);
                float   combinedRadius   = radius_ + other.radius_;
                float   combinedRadiusSq = RVOMath.sqr(combinedRadius);

                Line    line;
                Vector2 u;

                if (distSq > combinedRadiusSq)
                {
                    /* No collision. */
                    Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;

                    /* Vector from cutoff center to relative velocity. */
                    float wLengthSq   = RVOMath.absSq(w);
                    float dotProduct1 = w * relativePosition;

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

                        line.direction = new Vector2(unitW.y(), -unitW.x());
                        u = (combinedRadius * invTimeHorizon - wLength) * unitW;
                    }
                    else
                    {
                        /* Project on legs. */
                        float leg = RVOMath.sqrt(distSq - combinedRadiusSq);

                        if (RVOMath.det(relativePosition, w) > 0.0f)
                        {
                            /* Project on left leg. */
                            line.direction = new Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
                        }
                        else
                        {
                            /* Project on right leg. */
                            line.direction = -new Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
                        }

                        float dotProduct2 = 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. */
                    Vector2 w = relativeVelocity - invTimeStep * relativePosition;

                    float   wLength = RVOMath.abs(w);
                    Vector2 unitW   = w / wLength;

                    line.direction = new Vector2(unitW.y(), -unitW.x());
                    u = (combinedRadius * invTimeStep - wLength) * unitW;
                }

                line.point = velocity_ + 0.5f * u;
                orcaLines_.Add(line);
            }

            int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);

            if (lineFail < orcaLines_.Count)
            {
                linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
            }
        }
Esempio n. 2
0
        /**
         * <summary>Recursive method for building an obstacle k-D tree.
         * </summary>
         *
         * <returns>An obstacle k-D tree node.</returns>
         *
         * <param name="obstacles">A list of obstacles.</param>
         */
        private ObstacleTreeNode buildObstacleTreeRecursive(IList <Obstacle> obstacles)
        {
            if (obstacles.Count == 0)
            {
                return(null);
            }

            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.next_;

                /* Compute optimal split node. */

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

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

                    KInt j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
                    KInt j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);

                    if (j1LeftOfI >= 0 && j2LeftOfI >= 0)
                    {
                        ++leftSize;
                    }
                    else if (j1LeftOfI <= 0 && j2LeftOfI <= 0)
                    {
                        ++rightSize;
                    }
                    else
                    {
                        ++leftSize;
                        ++rightSize;
                    }
                    if (!Less(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize), Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
                    {
                        break;
                    }
                }

                if (Less(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize), 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.next_;

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

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

                    KInt j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
                    KInt j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);

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

                        KInt2 splitPoint = obstacleJ1.point_ + RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleI1.point_) * (obstacleJ2.point_ - obstacleJ1.point_) / RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleJ2.point_);

                        Obstacle newObstacle = new Obstacle();
                        newObstacle.point_     = splitPoint;
                        newObstacle.previous_  = obstacleJ1;
                        newObstacle.next_      = obstacleJ2;
                        newObstacle.convex_    = true;
                        newObstacle.direction_ = obstacleJ1.direction_;

                        newObstacle.id_ = Simulator.Instance.obstacles_.Count;

                        Simulator.Instance.obstacles_.Add(newObstacle);

                        obstacleJ1.next_     = newObstacle;
                        obstacleJ2.previous_ = newObstacle;

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

                node.obstacle_ = obstacleI1;
                node.left_     = buildObstacleTreeRecursive(leftObstacles);
                node.right_    = buildObstacleTreeRecursive(rightObstacles);

                return(node);
            }
        }
Esempio n. 3
0
        /**
         * <summary>Computes the new velocity of this agent.</summary>
         */
        internal void computeNewVelocity()
        {
            orcaLines_.Clear();

            //KInt invTimeHorizonObst = 1 / timeHorizonObst_;

            KInt tempradius = radius_ / timeHorizonObst_;

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

                KInt2 relativePosition1 = obstacle1.point_ - position_;
                KInt2 relativePosition2 = obstacle2.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 < orcaLines_.Count; ++j)
                {
                    if (RVOMath.det(relativePosition1 / timeHorizonObst_ - orcaLines_[j].point, orcaLines_[j].direction) - tempradius >= 0 && RVOMath.det(relativePosition2 / timeHorizonObst_ - orcaLines_[j].point, orcaLines_[j].direction) - tempradius >= 0)
                    {
                        alreadyCovered = true;

                        break;
                    }
                }

                if (alreadyCovered)
                {
                    continue;
                }

                /* Not yet covered. Check for collisions. */
                KInt distSq1 = RVOMath.absSq(relativePosition1);
                KInt distSq2 = RVOMath.absSq(relativePosition2);

                KInt radiusSq = RVOMath.sqr(radius_);

                KInt2 obstacleVector = obstacle2.point_ - obstacle1.point_;
                KInt  s          = (-RVOMath.Dot(relativePosition1, obstacleVector)) / RVOMath.absSq(obstacleVector);
                KInt  distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector);

                Line line = new Line();

                if (s < 0 && distSq1 <= radiusSq)
                {
                    /* Collision with left vertex. Ignore if non-convex. */
                    if (obstacle1.convex_)
                    {
                        line.point     = KInt2.zero;
                        line.direction = RVOMath.normalize(KInt2.ToInt2(-relativePosition1.IntY, relativePosition1.IntX));
                        orcaLines_.Add(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 neighboring obstacle.
                     */
                    if (obstacle2.convex_ && RVOMath.det(relativePosition2, obstacle2.direction_) >= 0)
                    {
                        line.point     = KInt2.zero;
                        line.direction = RVOMath.normalize(KInt2.ToInt2(-relativePosition2.IntY, relativePosition2.IntX));
                        orcaLines_.Add(line);
                    }

                    continue;
                }
                else if (s >= 0 && s < 1 && distSqLine <= radiusSq)
                {
                    /* Collision with obstacle segment. */
                    line.point     = KInt2.zero;
                    line.direction = -obstacle1.direction_;
                    orcaLines_.Add(line);

                    continue;
                }

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

                KInt2 leftLegDirection, rightLegDirection;

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

                    obstacle2 = obstacle1;

                    KInt leg1 = RVOMath.sqrt(distSq1 - radiusSq);
                    leftLegDirection  = KInt2.ToInt2(relativePosition1.IntX * leg1 - relativePosition1.IntY * radius_, relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
                    rightLegDirection = KInt2.ToInt2(relativePosition1.IntX * leg1 + relativePosition1.IntY * radius_, -relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
                    if (isover(leftLegDirection) || isover(rightLegDirection))
                    {
                        UnityEngine.Debug.LogError("!!!");
                    }
                }
                else if (s > 1 && distSqLine <= radiusSq)
                {
                    /*
                     * Obstacle viewed obliquely so that
                     * right vertex defines velocity obstacle.
                     */
                    if (!obstacle2.convex_)
                    {
                        /* Ignore obstacle. */
                        continue;
                    }

                    obstacle1 = obstacle2;

                    KInt leg2 = RVOMath.sqrt(distSq2 - radiusSq);
                    leftLegDirection  = KInt2.ToInt2(relativePosition2.IntX * leg2 - relativePosition2.IntY * radius_, relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
                    rightLegDirection = KInt2.ToInt2(relativePosition2.IntX * leg2 + relativePosition2.IntY * radius_, -relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
                    if (isover(leftLegDirection) || isover(rightLegDirection))
                    {
                        Debug.LogError("!!!");
                    }
                }
                else
                {
                    /* Usual situation. */
                    if (obstacle1.convex_)
                    {
                        KInt leg1 = RVOMath.sqrt(distSq1 - radiusSq);
                        leftLegDirection = KInt2.ToInt2(relativePosition1.IntX * leg1 - relativePosition1.IntY * radius_, relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
                        if (isover(leftLegDirection))
                        {
                            Debug.LogError("!!!");
                        }
                    }
                    else
                    {
                        /* Left vertex non-convex; left leg extends cut-off line. */
                        leftLegDirection = -obstacle1.direction_;
                        if (isover(leftLegDirection))
                        {
                            Debug.LogError("!!!");
                        }
                    }

                    if (obstacle2.convex_)
                    {
                        KInt leg2 = RVOMath.sqrt(distSq2 - radiusSq);

                        rightLegDirection = KInt2.ToInt2(relativePosition2.IntX * leg2 + relativePosition2.IntY * radius_, -relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
                        if (isover(rightLegDirection))
                        {
                            Debug.LogError("!!!");
                        }
                    }
                    else
                    {
                        /* Right vertex non-convex; right leg extends cut-off line. */
                        rightLegDirection = obstacle1.direction_;
                        if (isover(rightLegDirection))
                        {
                            Debug.LogError("!!!");
                        }
                    }
                }

                /*
                 * 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.previous_;

                bool isLeftLegForeign  = false;
                bool isRightLegForeign = false;

                if (obstacle1.convex_ && RVOMath.det(leftLegDirection, -leftNeighbor.direction_) >= 0)
                {
                    /* Left leg points into obstacle. */
                    leftLegDirection = -leftNeighbor.direction_;
                    if (isover(leftLegDirection))
                    {
                        Debug.LogError("!!!");
                    }
                    isLeftLegForeign = true;
                }

                if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= 0)
                {
                    /* Right leg points into obstacle. */
                    rightLegDirection = obstacle2.direction_;
                    isRightLegForeign = true;
                    if (isover(rightLegDirection))
                    {
                        Debug.LogError("!!!");
                    }
                }

                /* Compute cut-off centers. */
                KInt2 leftCutOff   = (obstacle1.point_ - position_) / timeHorizonObst_;
                KInt2 rightCutOff  = (obstacle2.point_ - position_) / timeHorizonObst_;
                KInt2 cutOffVector = rightCutOff - leftCutOff;

                /* Project current velocity on velocity obstacle. */

                /* Check if current velocity is projected on cutoff circles. */
                KInt sqvalue = RVOMath.absSq(cutOffVector);
                KInt t       = KInt.ToInt(KInt.divscale / 2);
                if (obstacle1 != obstacle2)
                {
                    if (sqvalue == 0)
                    {
                        t = KInt.MaxValue;
                    }
                    else
                    {
                        t = RVOMath.Dot((velocity_ - leftCutOff), cutOffVector) / sqvalue;
                    }
                }

                KInt tLeft  = RVOMath.Dot((velocity_ - leftCutOff), leftLegDirection);
                KInt tRight = RVOMath.Dot((velocity_ - rightCutOff), rightLegDirection);

                if ((t < 0 && tLeft < 0) || (obstacle1 == obstacle2 && tLeft < 0 && tRight < 0))
                {
                    /* Project on left cut-off circle. */
                    KInt2 unitW = RVOMath.normalize((velocity_ - leftCutOff));

                    line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
                    line.point     = leftCutOff + radius_ * unitW / timeHorizonObst_;
                    orcaLines_.Add(line);

                    continue;
                }
                else if (t > 1 && tRight < 0)
                {
                    /* Project on right cut-off circle. */
                    KInt2 unitW = RVOMath.normalize((velocity_ - rightCutOff));

                    line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
                    line.point     = rightCutOff + radius_ * unitW / timeHorizonObst_;
                    orcaLines_.Add(line);

                    continue;
                }

                /*
                 * Project on left leg, right leg, or cut-off line, whichever is
                 * closest to velocity.
                 */
                KInt distSqCutoff = (t < 0 || t > 1 || obstacle1 == obstacle2) ? KInt.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + t * cutOffVector));
                KInt distSqLeft   = tLeft < 0 ? KInt.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + tLeft * leftLegDirection));
                KInt distSqRight  = tRight < 0 ? KInt.MaxValue : RVOMath.absSq(velocity_ - (rightCutOff + tRight * rightLegDirection));

                if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
                {
                    /* Project on cut-off line. */
                    line.direction = -obstacle1.direction_;
                    line.point     = leftCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
                    orcaLines_.Add(line);

                    continue;
                }

                if (distSqLeft <= distSqRight)
                {
                    /* Project on left leg. */
                    if (isLeftLegForeign)
                    {
                        continue;
                    }

                    line.direction = leftLegDirection;
                    line.point     = leftCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
                    orcaLines_.Add(line);

                    continue;
                }

                /* Project on right leg. */
                if (isRightLegForeign)
                {
                    continue;
                }

                line.direction = -rightLegDirection;
                line.point     = rightCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
                orcaLines_.Add(line);
            }

            int numObstLines = orcaLines_.Count;

            //KInt invTimeHorizon = 1 / timeHorizon_;

            /* Create agent ORCA lines. */
            for (int i = 0; i < agentNeighbors_.Count; ++i)
            {
                Agent other = agentNeighbors_[i].Value;

                KInt2 relativePosition = other.position_ - position_;
                KInt2 relativeVelocity = velocity_ - other.velocity_;
                KInt  distSq           = RVOMath.absSq(relativePosition);
                KInt  combinedRadius   = radius_ + other.radius_;
                KInt  combinedRadiusSq = RVOMath.sqr(combinedRadius);

                Line  line = new Line();
                KInt2 u;

                if (distSq > combinedRadiusSq)
                {
                    /* No collision. */
                    KInt2 w = relativeVelocity - relativePosition / timeHorizon_;
                    /* Vector from cutoff center to relative velocity. */
                    KInt wLengthSq = RVOMath.absSq(w);

                    KInt dotProduct1 = RVOMath.Dot(w, relativePosition);

                    if (dotProduct1 < 0 && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
                    {
                        /* Project on cut-off circle. */
                        KInt wLength = RVOMath.sqrt(wLengthSq);
                        if (wLength == 0)
                        {
                            continue;
                        }
                        KInt2 unitW = w / wLength;

                        line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
                        u = (combinedRadius / timeHorizon_ - wLength) * unitW;
                    }
                    else
                    {
                        /* Project on legs. */
                        KInt leg = RVOMath.sqrt(distSq - combinedRadiusSq);

                        if (RVOMath.det(relativePosition, w) > 0)
                        {
                            /* Project on left leg. */
                            line.direction = KInt2.ToInt2(relativePosition.IntX * leg - relativePosition.IntY * combinedRadius, relativePosition.IntX * combinedRadius + relativePosition.IntY * leg) / distSq;
                        }
                        else
                        {
                            /* Project on right leg. */
                            line.direction = -KInt2.ToInt2(relativePosition.IntX * leg + relativePosition.IntY * combinedRadius, -relativePosition.IntX * combinedRadius + relativePosition.IntY * leg) / distSq;
                        }

                        KInt dotProduct2 = RVOMath.Dot(relativeVelocity, line.direction);
                        u = dotProduct2 * line.direction - relativeVelocity;
                    }
                }
                else
                {
                    /* Collision. Project on cut-off circle of time timeStep. */
                    //KInt invTimeStep = 1 / Simulator.Instance.timeStep_;
                    /* Vector from cutoff center to relative velocity. */
                    KInt2 w       = relativeVelocity - relativePosition / Simulator.Instance.timeStep_;
                    KInt  wLength = RVOMath.abs(w);
                    if (wLength == 0)
                    {
                        continue;
                    }
                    KInt2 unitW = w / wLength;

                    line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);

                    u = (combinedRadius / Simulator.Instance.timeStep_ - wLength) * unitW;
                }

                line.point = velocity_ + u / 2;
                orcaLines_.Add(line);
            }

            int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);

            if (lineFail < orcaLines_.Count)
            {
                linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
            }
        }