예제 #1
0
        public void addObstacle(KInt2 a, KInt2 b)
        {
            Obstacle first = new Obstacle();

            first.convex_ = true;
            Obstacle second = new Obstacle();

            second.convex_ = true;

            first.previous_  = second;
            second.previous_ = first;
            first.next_      = second;
            second.next_     = first;


            first.point_  = a;
            second.point_ = b;

            first.direction_  = RVOMath.normalize(KInt2.ToInt2(b.x - a.x, b.y - a.y));
            second.direction_ = -first.direction_;

            first.id_ = obstacles_.Count;
            obstacles_.Add(first);

            second.id_ = obstacles_.Count;
            obstacles_.Add(second);
        }
예제 #2
0
 public int queryNearAgent(KInt2 point, KInt radius)
 {
     if (getNumAgents() == 0)
     {
         return(-1);
     }
     return(kdTree_.queryNearAgent(point, radius));
 }
예제 #3
0
        /**
         * <summary>Solves a two-dimensional linear program subject to linear
         * constraints defined by lines and a circular constraint.</summary>
         *
         * <param name="lines">Lines defining the linear constraints.</param>
         * <param name="numObstLines">Count of obstacle lines.</param>
         * <param name="beginLine">The line on which the 2-d linear program
         * failed.</param>
         * <param name="radius">The radius of the circular constraint.</param>
         * <param name="result">A reference to the result of the linear program.
         * </param>
         */
        private void linearProgram3(IList <Line> lines, int numObstLines, int beginLine, KInt radius, ref KInt2 result)
        {
            KInt distance = 0;

            for (int i = beginLine; i < lines.Count; ++i)
            {
                if (RVOMath.det(lines[i].direction, lines[i].point - result) > distance)
                {
                    /* Result does not satisfy constraint of line i. */
                    IList <Line> projLines = new List <Line>();
                    for (int ii = 0; ii < numObstLines; ++ii)
                    {
                        projLines.Add(lines[ii]);
                    }

                    for (int j = numObstLines; j < i; ++j)
                    {
                        Line line = new Line();

                        KInt determinant = RVOMath.det(lines[i].direction, lines[j].direction);
                        if (RVOMath.fabs(determinant) <= 0)
                        {
                            /* Line i and line j are parallel. */
                            if (RVOMath.Dot(lines[i].direction, lines[j].direction) > 0)
                            {
                                /* Line i and line j point in the same direction. */

                                continue;
                            }
                            else
                            {
                                /* Line i and line j point in opposite direction. */
                                line.point = (lines[i].point + lines[j].point) / 2;
                            }
                        }
                        else
                        {
                            line.point = lines[i].point + (RVOMath.det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
                        }
                        line.direction = RVOMath.normalize((lines[j].direction - lines[i].direction));
                        projLines.Add(line);
                    }
                    KInt2 tempResult = result;
                    if (linearProgram2(projLines, radius, KInt2.ToInt2(-lines[i].direction.IntY, lines[i].direction.IntX), true, ref result) < projLines.Count)
                    {
                        /*
                         * This should in principle not happen. The result is by
                         * definition already in the feasible region of this
                         * linear program. If it fails, it is due to small
                         * floating point error, and the current result is kept.
                         */
                        result = tempResult;
                    }

                    distance = RVOMath.det(lines[i].direction, lines[i].point - result);
                }
            }
        }
예제 #4
0
        public bool ClosedObstaclePoint(int agentNo, ref KInt2 point)
        {
            Agent agent = agents_[agentNo2indexDict_[agentNo]];

            if (agent.obstacleNeighbors_.Count > 0)
            {
                point = agent.obstacleNeighbors_[0].Value.point_;
                return(true);
            }
            return(false);
        }
예제 #5
0
 /**
  * <summary>Updates the two-dimensional position and two-dimensional
  * velocity of this agent.</summary>
  */
 internal void update()
 {
     if (newVelocity_.x > 10 || newVelocity_.y > 10 || newVelocity_.x < -10 || newVelocity_.y < -10)
     {
         //newVelocity_ = newVelocity_ / 1000;
         UnityEngine.Debug.LogError("wrong velocity");
         //return;
     }
     velocity_  = newVelocity_;
     position_ += velocity_ * Simulator.Instance.timeStep_;
 }
예제 #6
0
        internal int queryNearAgent(KInt2 point, KInt radius)
        {
            KInt rangeSq = KInt.MaxValue;
            int  agentNo = -1;

            queryAgentTreeRecursive(point, ref rangeSq, ref agentNo, 0);
            if (rangeSq < radius * radius)
            {
                return(agentNo);
            }
            return(-1);
        }
예제 #7
0
        /**
         * <summary>Computes the squared distance from a line segment with the
         * specified endpoints to a specified point.</summary>
         *
         * <returns>The squared distance from the line segment to the point.
         * </returns>
         *
         * <param name="vector1">The first endpoint of the line segment.</param>
         * <param name="vector2">The second endpoint of the line segment.
         * </param>
         * <param name="vector3">The point to which the squared distance is to
         * be calculated.</param>
         */
        internal static KInt distSqPointLineSegment(KInt2 vector1, KInt2 vector2, KInt2 vector3)
        {
            KInt r = Dot(vector3 - vector1, vector2 - vector1) / absSq(vector2 - vector1);// (v31.IntX * v21.IntX  + v31.IntY * v21.IntY) * KInt.divscale / KInt2.div2scale;

            if (r < 0)
            {
                return(absSq(vector3 - vector1));
            }

            if (r > 1)
            {
                return(absSq(vector3 - vector2));
            }

            return(absSq(vector3 - (vector1 + r * (vector2 - vector1))));
        }
예제 #8
0
        private void queryAgentTreeRecursive(KInt2 position, ref KInt rangeSq, ref int agentNo, int node)
        {
            if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
            {
                for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
                {
                    KInt distSq = RVOMath.absSq(position - agents_[i].position_);
                    if (distSq < rangeSq)
                    {
                        rangeSq = distSq;
                        agentNo = agents_[i].id_;
                    }
                }
            }
            else
            {
                KInt distSqLeft = RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].left_].minx, position.IntX)) + RVOMath.sqr(ReduceMax(position.IntX, agentTree_[agentTree_[node].left_].maxx)) + RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].left_].miny, position.IntY)) + RVOMath.sqr(ReduceMax(position.IntY, agentTree_[agentTree_[node].left_].maxy));

                KInt distSqRight = RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].right_].minx, position.IntX)) + RVOMath.sqr(ReduceMax(position.IntX, agentTree_[agentTree_[node].right_].maxx)) + RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].right_].miny, position.IntY)) + RVOMath.sqr(ReduceMax(position.IntY, agentTree_[agentTree_[node].right_].maxy));

                if (distSqLeft < distSqRight)
                {
                    if (distSqLeft < rangeSq)
                    {
                        queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].left_);

                        if (distSqRight < rangeSq)
                        {
                            queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].right_);
                        }
                    }
                }
                else
                {
                    if (distSqRight < rangeSq)
                    {
                        queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].right_);

                        if (distSqLeft < rangeSq)
                        {
                            queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].left_);
                        }
                    }
                }
            }
        }
예제 #9
0
        /**
         * <summary>Adds a new agent to the simulation.</summary>
         *
         * <returns>The number of the agent.</returns>
         *
         * <param name="position">The two-dimensional starting position of this
         * agent.</param>
         * <param name="neighborDist">The maximum distance (center point to
         * center point) to other agents this agent takes into account in the
         * navigation. The larger this number, the longer the running time of
         * the simulation. If the number is too low, the simulation will not be
         * safe. Must be non-negative.</param>
         * <param name="maxNeighbors">The maximum number of other agents this
         * agent takes into account in the navigation. The larger this number,
         * the longer the running time of the simulation. If the number is too
         * low, the simulation will not be safe.</param>
         * <param name="timeHorizon">The minimal amount of time for which this
         * agent's velocities that are computed by the simulation are safe with
         * respect to other agents. The larger this number, the sooner this
         * agent will respond to the presence of other agents, but the less
         * freedom this agent has in choosing its velocities. Must be positive.
         * </param>
         * <param name="timeHorizonObst">The minimal amount of time for which
         * this agent's velocities that are computed by the simulation are safe
         * with respect to obstacles. The larger this number, the sooner this
         * agent will respond to the presence of obstacles, but the less freedom
         * this agent has in choosing its velocities. Must be positive.</param>
         * <param name="radius">The radius of this agent. Must be non-negative.
         * </param>
         * <param name="maxSpeed">The maximum speed of this agent. Must be
         * non-negative.</param>
         * <param name="velocity">The initial two-dimensional linear velocity of
         * this agent.</param>
         */
        public int addAgent(KInt2 position, KInt neighborDist, int maxNeighbors, KInt timeHorizon, KInt timeHorizonObst, KInt radius, KInt maxSpeed, KInt2 velocity)
        {
            Agent agent = new Agent();

            agent.id_ = s_totalID;
            s_totalID++;
            agent.maxNeighbors_    = maxNeighbors;
            agent.maxSpeed_        = maxSpeed;
            agent.neighborDist_    = neighborDist;
            agent.position_        = position;
            agent.radius_          = radius;
            agent.timeHorizon_     = timeHorizon;
            agent.timeHorizonObst_ = timeHorizonObst;
            agent.velocity_        = velocity;
            agents_.Add(agent);
            onAddAgent();
            return(agent.id_);
        }
예제 #10
0
        /**
         * <summary>Recursive method for querying the visibility between two
         * points within a specified radius.</summary>
         *
         * <returns>True if q1 and q2 are mutually visible within the radius;
         * false otherwise.</returns>
         *
         * <param name="q1">The first point between which visibility is to be
         * tested.</param>
         * <param name="q2">The second point between which visibility is to be
         * tested.</param>
         * <param name="radius">The radius within which visibility is to be
         * tested.</param>
         * <param name="node">The current obstacle k-D node.</param>
         */
        private bool queryVisibilityRecursive(KInt2 q1, KInt2 q2, KInt radius, ObstacleTreeNode node)
        {
            if (node == null)
            {
                return(true);
            }

            Obstacle obstacle1 = node.obstacle_;
            Obstacle obstacle2 = obstacle1.next_;

            KInt q1LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q1);
            KInt q2LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q2);
            KInt LengthI   = RVOMath.absSq(obstacle2.point_ - obstacle1.point_);

            // KInt invLengthI = 1.0f / RVOMath.absSq(obstacle2.point_ - obstacle1.point_);

            if (q1LeftOfI >= 0 && q2LeftOfI >= 0)
            {
                return(queryVisibilityRecursive(q1, q2, radius, node.left_) && ((RVOMath.sqr(q1LeftOfI) / LengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) / LengthI >= RVOMath.sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node.right_)));
            }

            if (q1LeftOfI <= 0 && q2LeftOfI <= 0)
            {
                return(queryVisibilityRecursive(q1, q2, radius, node.right_) && ((RVOMath.sqr(q1LeftOfI) / LengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) / LengthI >= RVOMath.sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node.left_)));
            }

            if (q1LeftOfI >= 0 && q2LeftOfI <= 0)
            {
                /* One can see through obstacle from left to right. */
                return(queryVisibilityRecursive(q1, q2, radius, node.left_) && queryVisibilityRecursive(q1, q2, radius, node.right_));
            }

            KInt point1LeftOfQ = RVOMath.leftOf(q1, q2, obstacle1.point_);
            KInt point2LeftOfQ = RVOMath.leftOf(q1, q2, obstacle2.point_);
            KInt LengthQ       = RVOMath.absSq(q2 - q1);

            // KInt invLengthQ = 1.0f / RVOMath.absSq(q2 - q1);

            return(point1LeftOfQ * point2LeftOfQ >= 0 && RVOMath.sqr(point1LeftOfQ) / LengthQ > RVOMath.sqr(radius) && RVOMath.sqr(point2LeftOfQ) / LengthQ > RVOMath.sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node.left_) && queryVisibilityRecursive(q1, q2, radius, node.right_));
        }
예제 #11
0
        /**
         * <summary>Adds a new agent with default properties to the simulation.
         * </summary>
         *
         * <returns>The number of the agent, or -1 when the agent defaults have
         * not been set.</returns>
         *
         * <param name="position">The two-dimensional starting position of this
         * agent.</param>
         */
        public int addAgent(KInt2 position)
        {
            if (defaultAgent_ == null)
            {
                return(-1);
            }

            Agent agent = new Agent();

            agent.id_ = s_totalID;
            s_totalID++;
            agent.maxNeighbors_    = defaultAgent_.maxNeighbors_;
            agent.maxSpeed_        = defaultAgent_.maxSpeed_;
            agent.neighborDist_    = defaultAgent_.neighborDist_;
            agent.position_        = position;
            agent.radius_          = defaultAgent_.radius_;
            agent.timeHorizon_     = defaultAgent_.timeHorizon_;
            agent.timeHorizonObst_ = defaultAgent_.timeHorizonObst_;
            agent.velocity_        = defaultAgent_.velocity_;
            agents_.Add(agent);
            onAddAgent();
            return(agent.id_);
        }
예제 #12
0
        /**
         * <summary>Solves a two-dimensional linear program subject to linear
         * constraints defined by lines and a circular constraint.</summary>
         *
         * <returns>The number of the line it fails on, and the number of lines
         * if successful.</returns>
         *
         * <param name="lines">Lines defining the linear constraints.</param>
         * <param name="radius">The radius of the circular constraint.</param>
         * <param name="optVelocity">The optimization velocity.</param>
         * <param name="directionOpt">True if the direction should be optimized.
         * </param>
         * <param name="result">A reference to the result of the linear program.
         * </param>
         */
        private int linearProgram2(IList <Line> lines, KInt radius, KInt2 optVelocity, bool directionOpt, ref KInt2 result)
        {
            if (directionOpt)
            {
                /*
                 * Optimize direction. Note that the optimization velocity is of
                 * unit length in this case.
                 */
                result = optVelocity * radius;
            }
            else if (RVOMath.absSq(optVelocity) > RVOMath.sqr(radius))
            {
                /* Optimize closest point and outside circle. */
                result = RVOMath.normalize(optVelocity) * radius;
            }
            else
            {
                /* Optimize closest point and inside circle. */
                result = optVelocity;
            }
            for (int i = 0; i < lines.Count; ++i)
            {
                if (RVOMath.det(lines[i].direction, lines[i].point - result) > 0)
                {
                    /* Result does not satisfy constraint i. Compute new optimal result. */
                    KInt2 tempResult = result;
                    if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result))
                    {
                        result = tempResult;
                        return(i);
                    }
                }
            }

            return(lines.Count);
        }
예제 #13
0
 /**
  * <summary>Queries the visibility between two points within a specified
  * radius.</summary>
  *
  * <returns>True if q1 and q2 are mutually visible within the radius;
  * false otherwise.</returns>
  *
  * <param name="q1">The first point between which visibility is to be
  * tested.</param>
  * <param name="q2">The second point between which visibility is to be
  * tested.</param>
  * <param name="radius">The radius within which visibility is to be
  * tested.</param>
  */
 internal bool queryVisibility(KInt2 q1, KInt2 q2, KInt radius)
 {
     return(queryVisibilityRecursive(q1, q2, radius, obstacleTree_));
 }
예제 #14
0
 /**
  * <summary>Computes the squared length of a specified two-dimensional
  * vector.</summary>
  *
  * <returns>The squared length of the two-dimensional vector.</returns>
  *
  * <param name="vector">The two-dimensional vector whose squared length
  * is to be computed.</param>
  */
 public static KInt absSq(KInt2 vector)
 {
     return(KInt.ToInt((vector.IntX * vector.IntX + vector.IntY * vector.IntY) * KInt.divscale / KInt2.div2scale));
 }
예제 #15
0
 /**
  * <summary>Computes the normalization of the specified two-dimensional
  * vector.</summary>
  *
  * <returns>The normalization of the two-dimensional vector.</returns>
  *
  * <param name="vector">The two-dimensional vector whose normalization
  * is to be computed.</param>
  */
 public static KInt2 normalize(KInt2 vector)
 {
     return(vector.normalized);
 }
예제 #16
0
 /**
  * <summary>Computes the length of a specified two-dimensional vector.
  * </summary>
  *
  * <param name="vector">The two-dimensional vector whose length is to be
  * computed.</param>
  * <returns>The length of the two-dimensional vector.</returns>
  */
 public static KInt abs(KInt2 vector)
 {
     return(vector.IntMagnitude);
 }
예제 #17
0
 /**
  * <summary>Computes the signed distance from a line connecting the
  * specified points to a specified point.</summary>
  *
  * <returns>Positive when the point c lies to the left of the line ab.
  * </returns>
  *
  * <param name="a">The first point on the line.</param>
  * <param name="b">The second point on the line.</param>
  * <param name="c">The point to which the signed distance is to be
  * calculated.</param>
  */
 internal static KInt leftOf(KInt2 a, KInt2 b, KInt2 c)
 {
     return(det(a - c, b - a));
 }
예제 #18
0
        /**
         * <summary>Sets the default properties for any new agent that is added.
         * </summary>
         *
         * <param name="neighborDist">The default maximum distance (center point
         * to center point) to other agents a new agent takes into account in
         * the navigation. The larger this number, the longer he running time of
         * the simulation. If the number is too low, the simulation will not be
         * safe. Must be non-negative.</param>
         * <param name="maxNeighbors">The default maximum number of other agents
         * a new agent takes into account in the navigation. The larger this
         * number, the longer the running time of the simulation. If the number
         * is too low, the simulation will not be safe.</param>
         * <param name="timeHorizon">The default minimal amount of time for
         * which a new agent's velocities that are computed by the simulation
         * are safe with respect to other agents. The larger this number, the
         * sooner an agent will respond to the presence of other agents, but the
         * less freedom the agent has in choosing its velocities. Must be
         * positive.</param>
         * <param name="timeHorizonObst">The default minimal amount of time for
         * which a new agent's velocities that are computed by the simulation
         * are safe with respect to obstacles. The larger this number, the
         * sooner an agent will respond to the presence of obstacles, but the
         * less freedom the agent has in choosing its velocities. Must be
         * positive.</param>
         * <param name="radius">The default radius of a new agent. Must be
         * non-negative.</param>
         * <param name="maxSpeed">The default maximum speed of a new agent. Must
         * be non-negative.</param>
         * <param name="velocity">The default initial two-dimensional linear
         * velocity of a new agent.</param>
         */
        public void setAgentDefaults(KInt neighborDist, int maxNeighbors, KInt timeHorizon, KInt timeHorizonObst, KInt radius, KInt maxSpeed, KInt2 velocity)
        {
            if (defaultAgent_ == null)
            {
                defaultAgent_ = new Agent();
            }

            defaultAgent_.maxNeighbors_    = maxNeighbors;
            defaultAgent_.maxSpeed_        = maxSpeed;
            defaultAgent_.neighborDist_    = neighborDist;
            defaultAgent_.radius_          = radius;
            defaultAgent_.timeHorizon_     = timeHorizon;
            defaultAgent_.timeHorizonObst_ = timeHorizonObst;
            defaultAgent_.velocity_        = velocity;
        }
예제 #19
0
 public void addObstacle(KInt3 a, KInt3 b)
 {
     addObstacle(KInt2.ToInt2(a.x, a.z), KInt2.ToInt2(b.x, b.z));
 }
예제 #20
0
 public static KInt Dot(KInt2 left, KInt2 right)
 {
     return(KInt.ToInt((left.IntX * right.IntX + left.IntY * right.IntY) * KInt.divscale / KInt2.div2scale));
 }
예제 #21
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_);
            }
        }
예제 #22
0
 bool isover(KInt2 dir)
 {
     return(dir.x > 100 || dir.y > 100);
 }
예제 #23
0
        /**
         * <summary>Solves a one-dimensional linear program on a specified line
         * subject to linear constraints defined by lines and a circular
         * constraint.</summary>
         *
         * <returns>True if successful.</returns>
         *
         * <param name="lines">Lines defining the linear constraints.</param>
         * <param name="lineNo">The specified line constraint.</param>
         * <param name="radius">The radius of the circular constraint.</param>
         * <param name="optVelocity">The optimization velocity.</param>
         * <param name="directionOpt">True if the direction should be optimized.
         * </param>
         * <param name="result">A reference to the result of the linear program.
         * </param>
         */
        private bool linearProgram1(IList <Line> lines, int lineNo, KInt radius, KInt2 optVelocity, bool directionOpt, ref KInt2 result)
        {
            KInt dotProduct   = RVOMath.Dot(lines[lineNo].point, lines[lineNo].direction);
            KInt discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - RVOMath.absSq(lines[lineNo].point);

            if (discriminant < 0)
            {
                /* Max speed circle fully invalidates line lineNo. */
                return(false);
            }

            KInt sqrtDiscriminant = RVOMath.sqrt(discriminant);
            KInt tLeft            = -dotProduct - sqrtDiscriminant;
            KInt tRight           = -dotProduct + sqrtDiscriminant;

            for (int i = 0; i < lineNo; ++i)
            {
                KInt denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction);
                KInt numerator   = RVOMath.det(lines[i].direction, lines[lineNo].point - lines[i].point);

                if (RVOMath.fabs(denominator) <= 0)
                {
                    /* Lines lineNo and i are (almost) parallel. */
                    if (numerator < 0)
                    {
                        return(false);
                    }

                    continue;
                }

                KInt t = numerator / denominator;

                if (denominator >= 0)
                {
                    /* Line i bounds line lineNo on the right. */
                    tRight = KInt.Min(tRight, t);
                }
                else
                {
                    /* Line i bounds line lineNo on the left. */
                    tLeft = KInt.Max(tLeft, t);
                }

                if (tLeft > tRight)
                {
                    return(false);
                }
            }

            if (directionOpt)
            {
                /* Optimize direction. */
                if (RVOMath.Dot(optVelocity, lines[lineNo].direction) > 0)
                {
                    /* Take right extreme. */
                    result = lines[lineNo].point + tRight * lines[lineNo].direction;
                }
                else
                {
                    /* Take left extreme. */
                    result = lines[lineNo].point + tLeft * lines[lineNo].direction;
                }
            }
            else
            {
                /* Optimize closest point. */
                KInt t = RVOMath.Dot(lines[lineNo].direction, (optVelocity - lines[lineNo].point));

                if (t < tLeft)
                {
                    result = lines[lineNo].point + tLeft * lines[lineNo].direction;
                }
                else if (t > tRight)
                {
                    result = lines[lineNo].point + tRight * lines[lineNo].direction;
                }
                else
                {
                    result = lines[lineNo].point + t * lines[lineNo].direction;
                }
            }

            return(true);
        }
예제 #24
0
 UnityEngine.Vector3 getUnityVector3(KInt2 v)
 {
     return(new UnityEngine.Vector3(v.x, 0, v.y));
 }
예제 #25
0
 /**
  * <summary>Computes the determinant of a two-dimensional square matrix
  * with rows consisting of the specified two-dimensional vectors.
  * </summary>
  *
  * <returns>The determinant of the two-dimensional square matrix.
  * </returns>
  *
  * <param name="vector1">The top row of the two-dimensional square
  * matrix.</param>
  * <param name="vector2">The bottom row of the two-dimensional square
  * matrix.</param>
  */
 internal static KInt det(KInt2 vector1, KInt2 vector2)
 {
     return(KInt.ToInt((vector1.IntX * vector2.IntY - vector1.IntY * vector2.IntX) * KInt.divscale / KInt2.div2scale));
 }
예제 #26
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);
            }
        }
예제 #27
0
 /**
  * <summary>Performs a visibility query between the two specified points
  * with respect to the obstacles.</summary>
  *
  * <returns>A boolean specifying whether the two points are mutually
  * visible. Returns true when the obstacles have not been processed.
  * </returns>
  *
  * <param name="point1">The first point of the query.</param>
  * <param name="point2">The second point of the query.</param>
  * <param name="radius">The minimal distance between the line connecting
  * the two points and the obstacles in order for the points to be
  * mutually visible (optional). Must be non-negative.</param>
  */
 public bool queryVisibility(KInt2 point1, KInt2 point2, KInt radius)
 {
     return(kdTree_.queryVisibility(point1, point2, radius));
 }
예제 #28
0
 /**
  * <summary>Sets the two-dimensional linear velocity of a specified
  * agent.</summary>
  *
  * <param name="agentNo">The number of the agent whose two-dimensional
  * linear velocity is to be modified.</param>
  * <param name="velocity">The replacement two-dimensional linear
  * velocity.</param>
  */
 public void setAgentVelocity(int agentNo, KInt2 velocity)
 {
     agents_[agentNo2indexDict_[agentNo]].velocity_ = velocity;
 }
예제 #29
0
 /**
  * <summary>Sets the two-dimensional position of a specified agent.
  * </summary>
  *
  * <param name="agentNo">The number of the agent whose two-dimensional
  * position is to be modified.</param>
  * <param name="position">The replacement of the two-dimensional
  * position.</param>
  */
 public void setAgentPosition(int agentNo, KInt2 position)
 {
     agents_[agentNo2indexDict_[agentNo]].position_ = position;
 }