Esempio n. 1
0
        /**
         * <summary>Select the Leader of the agent</summary>
         */
        void selectLeader()
        {
            leader_ = null;
            for (int neighbor_id = 0; neighbor_id < agentNeighbors_.Count; ++neighbor_id)
            {
                Agent   neighbor           = agentNeighbors_[neighbor_id].Value;
                Vector2 relative_pos       = neighbor.position_ - position_;
                float   alpha              = Vector2.angle(velocity_);
                Vector2 local_relative_pos = Vector2.rotation(relative_pos, -alpha);
                Vector2 local_velocity     = Vector2.rotation(neighbor.velocity_, -alpha);
                float   alpha_v            = Vector2.angle(local_velocity);

                if (local_relative_pos.x() > 0 &&
                    local_relative_pos.x() < 1.5 &&
                    Math.Abs(local_relative_pos.y()) < this.radius_ + neighbor.radius_ &&
                    Math.Abs(alpha_v) < Math.PI / 6 &&
                    local_velocity.x() >= 0
                    )
                {
                    if (leader_ == null)
                    {
                        leader_ = neighbor;
                    }
                    else
                    {
                        Vector2 leader_relative_pos       = leader_.position_ - position_;
                        Vector2 leader_local_relative_pos = Vector2.rotation(leader_relative_pos, -alpha);
                        if (leader_local_relative_pos.x() > local_relative_pos.x())
                        {
                            leader_ = neighbor;
                        }
                    }
                }
            }
        }
Esempio n. 2
0
        private void setCorridor()
        {
            // Add (polygonal) obstacle(s), specifying vertices in counterclockwise order.
            // Add corridor right side

            IList <Vector2> right_side = new List <Vector2> {
                Vector2.rotation(new Vector2(corridor_length_ + 100, 0.0f), corridor_angle_),
                Vector2.rotation(new Vector2(-100.0f, 0.0f), corridor_angle_),
                Vector2.rotation(new Vector2(-100.0f, -50.0f), corridor_angle_),
                Vector2.rotation(new Vector2(corridor_length_ + 100, -50.0f), corridor_angle_)
            };

            sim_.addObstacle(right_side);
            //Add cooridor left side
            IList <Vector2> left_side = new List <Vector2> {
                Vector2.rotation(new Vector2(-100.0f, corridor_width_), corridor_angle_),
                Vector2.rotation(new Vector2(corridor_length_ + 100, corridor_width_), corridor_angle_),
                Vector2.rotation(new Vector2(corridor_length_ + 100, corridor_width_ + 50.0f), corridor_angle_),
                Vector2.rotation(new Vector2(-100.0f, corridor_width_ + 50.0f), corridor_angle_)
            };

            sim_.addObstacle(left_side);

            // Process obstacles so that they are accounted for in the simulation.
            sim_.processObstacles();
        }
Esempio n. 3
0
        /**
         * <summary>Following Behaviour of an agent</summary>
         */
        float followingBehavior()
        {
            // Compute pedestrian angle for local referential
            float alpha = Vector2.angle(velocity_);

            // If there is a leader to follow
            if (leader_ != null)
            {
                if (velocityBuffer_.Count > 0 && leader_.velocityBuffer_.Count > 0)
                {
                    // Compute relative position
                    Vector2 relative_pos       = leader_.position_ - position_;
                    Vector2 local_relative_pos = Vector2.rotation(relative_pos, -alpha);

                    // Compute related velocity with tau delay
                    Vector2 delayed_relative_vel;
                    int     element = (int)Math.Round((velocityBuffer_.Count - 1) - tau_ / sim_.getTimeStep());
                    element = Math.Max(0, element);
                    delayed_relative_vel = leader_.velocityBuffer_[element] - velocityBuffer_[element];
                    Vector2 delayed_local_relative_vel = Vector2.rotation(delayed_relative_vel, -alpha);

                    // Apply following model
                    return(lemercier(delayed_local_relative_vel.x(), local_relative_pos.x()));
                }
                else
                {
                    return(Vector2.rotation(acceleration_, -alpha).x());
                }
            }
            else
            {
                return(Vector2.rotation(acceleration_, -alpha).x());
            }
        }
Esempio n. 4
0
        internal Pair <Vector2, Vector2> computeTangentsPoints(Agent observer, Agent agent)
        {
            // First element of the pair = left tangent
            // Second element of the pair = right tangent
            Pair <Vector2, Vector2> toReturn = new Pair <Vector2, Vector2>();
            Vector2 centers = agent.position_ - observer.position_;
            Vector2 r1a     = Vector2.normalize(Vector2.rotation(centers, (float)-Math.PI / 2)) * observer.radius_;
            Vector2 r1b     = Vector2.normalize(Vector2.rotation(centers, (float)Math.PI / 2)) * observer.radius_;
            // Compute intersection points between radius and circle
            // Right one
            Vector2 h1a = observer.position_ + r1a;
            // Left one
            Vector2 h1b = observer.position_ + r1b;

            // If the radius is the same, tangents points are perpendicular to centers vector
            if (Math.Abs(observer.radius_ - agent.radius_) < RVO_EPSILON)
            {
                toReturn.First  = h1a;
                toReturn.Second = h1b;
            }
            else
            {
                Vector2 r2a = Vector2.normalize(Vector2.rotation(centers, (float)-Math.PI / 2)) * agent.radius_;
                Vector2 r2b = Vector2.normalize(Vector2.rotation(centers, (float)Math.PI / 2)) * agent.radius_;
                Vector2 h2a = agent.position_ + r2a;
                Vector2 h2b = agent.position_ + r2b;
                // If tangents are parallel, radius are the same, i.e. there is no intersection point.
                if (Math.Abs(Vector2.det(h1a - h2a, h1b - h2b)) < RVO_EPSILON)
                {
                    Console.Write("Problem while computing tangent points\n SHALL NOT HAPPEN !!! \n");
                    toReturn.First  = h1a;
                    toReturn.Second = h1b;
                }
                else
                {
                    Vector2 intersectionPoint = Vector2.intersectOf2Lines(h1a, h2a, h1b, h2b);
                    // Equivalent to :
                    Vector2 circleCenter = (intersectionPoint + observer.position_) / 2;
                    toReturn = Vector2.intersectOf2Circles(circleCenter, Vector2.abs(circleCenter - observer.position_), observer.position_, observer.radius_);
                    // Test angles to know which one is right &  which one is left
                    if (Vector2.isOnTheLeftSide(toReturn.First - observer.position_, centers))
                    {
                        Vector2 temp = toReturn.First;
                        toReturn.First  = toReturn.Second;
                        toReturn.Second = temp;
                    }
                }
            }
            return(toReturn);
        }
Esempio n. 5
0
        /**
         * <summary>Apply the behaviour of following to the agent</summary>
         */
        void applyFollowingBehavior(float following_acc)
        {
            float alpha = Vector2.angle(velocity_);
            // If the result lowers the tangential component of the acceleration,
            // apply this tangential acceleration to compute the new velocity
            Vector2 local_acceleration = Vector2.rotation(acceleration_, -alpha);

            if (following_acc < local_acceleration.x())
            {
                Vector2 local_new_acceleration = new Vector2(following_acc, local_acceleration.y());
                acceleration_ = Vector2.rotation(local_new_acceleration, alpha);
                newVelocity_  = velocity_ + acceleration_ * sim_.getTimeStep();
            }
        }
Esempio n. 6
0
        // Update is called once per frame
        void Update()
        {
            if (!reachedGoal())
            {
                setPreferredVelocities();
                doStep(false);

                for (int i = 0; i < getNumAgents(); ++i)
                {
                    Vector2 position = getPosition(i);
                    agents[i].transform.position = new Vector3(position.x(), 0.5f, position.y());

                    /*  RVO.Vector2 vector2 = sim_.getAgentVelocity(i);
                     * agents[i].rotation = Quaternion.LookRotation(new Vector3(vector2.x_, 0, vector2.y_));*/
                    /*  RVO.Vector2 vector2 = sim_.getAgentVelocity(i);
                     * agents[i].rotation = Quaternion.LookRotation(new Vector3(vector2.x_, 0, vector2.y_));*/
                    setColor(i);
                    float key = -1f;
                    if (sim_.getAgent(i).agentNeighbors_.Count > 0)
                    {
                        key = sim_.getAgent(i).agentNeighbors_[0].Key;
                    }



                    pas = pas + i;
                }
                //Debug.Log("Distance " + Math.Sqrt(Math.Pow(agents[0].position.x - agents[1].position.x,2) + Math.Pow(agents[0].position.y - agents[1].position.y,2)));
                //Debug.Log("Vitesse agent 1 " + Math.Sqrt(Math.Pow(sim_.getAgentVelocity(1).x_,2) + Math.Pow(sim_.getAgentVelocity(1).y_,2)));
                using (TextWriter tw = new StreamWriter(name2, true))
                {
                    tw.WriteLine(sim_.getAgent(0).position_.x_ + "\t" + sim_.getAgent(0).position_.y_ + "\t" + sim_.getAgent(0).agentNeighbors_.Count + "\t" + sim_.getAgent(1).position_.x_ + "\t" + sim_.getAgent(1).position_.y_ + "\t" + sim_.getAgent(1).agentNeighbors_.Count);
                }


                Agent   neighbor = sim_.getAgent(1);
                Vector2 relative_pos;
                float   alpha;
                Vector2 local_relative_pos = new Vector2();
                Agent   neighbor2          = sim_.getAgent(0);;
                Vector2 relative_pos2;
                float   alpha2;
                Vector2 local_relative_pos2 = new Vector2();
                relative_pos        = neighbor.position_ - sim_.getAgent(0).position_;
                alpha               = Vector2.angle(sim_.getAgent(0).velocity_);
                local_relative_pos  = Vector2.rotation(relative_pos, -alpha);
                relative_pos2       = neighbor2.position_ - sim_.getAgent(1).position_;
                alpha2              = Vector2.angle(sim_.getAgent(1).velocity_);
                local_relative_pos2 = Vector2.rotation(relative_pos2, -alpha2);


                using (TextWriter tw = new StreamWriter(name, true))
                {
                    tw.WriteLine(pas + "\t" + sim_.getAgentPosition(0).x() + "\t" + sim_.getAgentPosition(0).y() + "\t" + sim_.getAgentPosition(1).x() + "\t" + sim_.getAgentPosition(1).y()
                                 + "\t" + sim_.getAgentVelocity(0).x() + "\t" + sim_.getAgentVelocity(0).y() + "\t" + sim_.getAgentVelocity(1).x() + "\t" + sim_.getAgentVelocity(1).y() +
                                 "\t" + sim_.getAgentAcceleration(0).x() + "\t" + sim_.getAgentAcceleration(0).y() + "\t" + sim_.getAgentAcceleration(1).x() + "\t" + sim_.getAgentAcceleration(1).y() + "\t"
                                 + local_relative_pos.x() + "\t" + local_relative_pos.y() + "\t" + local_relative_pos2.x() + "\t" + local_relative_pos2.y() + "\t"
                                 + Vector2.angle(sim_.agents_[0].velocity_) * (180 / Math.PI) + "\t"
                                 + Vector2.angle(sim_.agents_[1].velocity_) * (180 / Math.PI) + "\t" + sim_.getAgentLeaderNo(0) + "\t" + sim_.getAgentLeaderNo(1));
                }
            }
            else
            {
                for (int i = 0; i < getNumAgents(); ++i)
                {
                    agents[i].GetComponent <Rigidbody>().isKinematic = true;
                }
            }
        }
Esempio n. 7
0
        public void unitTests()
        {
            // Test Compute tangents Points
            setAgentDefaults(15.0f, 10, 1.0f, 10.0f, 0.5f, 2.0f, new Vector2());
            addAgent(new Vector2(-1, 1), 0, true, true);
            addAgent(new Vector2(0, 1), 0, true, true);
            setAgentRadius(1, 1.0f);
            addAgent(new Vector2(0, 0), 0, true, true);
            addAgent(new Vector2(1, 2), 0, true, true);
            addAgent(new Vector2(1, 0), 0, true, true);

            GroupAgent group           = new GroupAgent(this);
            Pair <Vector2, Vector2> p1 = group.computeTangentsPoints(getAgent(0), getAgent(1));
            Pair <Vector2, Vector2> p2 = group.computeTangentsPoints(getAgent(2), getAgent(1));
            Pair <Vector2, Vector2> p3 = group.computeTangentsPoints(getAgent(3), getAgent(1));
            Pair <Vector2, Vector2> p4 = group.computeTangentsPoints(getAgent(4), getAgent(1));

            Pair <Vector2, Vector2> p5 = group.computeTangentsPoints(getAgent(1), getAgent(0));
            Pair <Vector2, Vector2> p6 = group.computeTangentsPoints(getAgent(1), getAgent(2));
            Pair <Vector2, Vector2> p7 = group.computeTangentsPoints(getAgent(1), getAgent(3));
            Pair <Vector2, Vector2> p8 = group.computeTangentsPoints(getAgent(1), getAgent(4));

            Console.Write("Compute Tangents Points\n");
            Console.Write("T1" + p1.First + "Expected ~(-1,0.5) - T2" + p1.Second + "Expected ~(-1,1.5)\n");
            Console.Write("T1" + p2.First + "Expected ~(0.5,0) - T2" + p2.Second + "Expected ~(-0.5,0)\n");
            Console.Write("T1" + p3.First + "Expected ~(0.64,2.35) - T2" + p3.Second + "Expected ~(1.35,1.64)\n");
            Console.Write("T1" + p4.First + "Expected ~(1.35,0.35) - T2" + p4.Second + "Expected ~(0.64,-0.35)\n\n");

            Console.Write("T1" + p5.First + "Expected ~(0,1.5) - T2" + p5.Second + "Expected ~(0,0.5)\n");
            Console.Write("T1" + p6.First + "Expected ~(-0.5,1) - T2" + p6.Second + "Expected ~(0.5,1)\n");
            Console.Write("T1" + p7.First + "Expected ~(0.35,0.64) - T2" + p7.Second + "Expected ~(-0.35,1.35)\n");
            Console.Write("T1" + p8.First + "Expected ~(-0.35,0.64) - T2" + p8.Second + "Expected ~(0.35,1.35)\n\n");

            // Test detect groups
            setAgentRadius(1, 0.5f);
            addAgent(new Vector2(1, 0), 0, true, true);

            setAgentPosition(0, new Vector2(0.0f, 3 * getDefaultRadius()));
            setAgentPosition(1, new Vector2(0.0f, 0.0f));
            setAgentPosition(2, new Vector2(-5 * getDefaultRadius(), 0.0f));
            setAgentPosition(3, new Vector2(-5 * getDefaultRadius(), -3 * getDefaultRadius()));
            setAgentPosition(4, new Vector2(3 * getDefaultRadius(), 0.0f));
            setAgentPosition(5, new Vector2(6 * getDefaultRadius(), 0.0f));

            setAgentVelocity(0, new Vector2(0.87f, 0.0f));
            setAgentVelocity(1, new Vector2(1.0f, 0.0f));
            setAgentVelocity(2, new Vector2(0.93f, 0.0f));
            setAgentVelocity(3, Vector2.rotation(new Vector2(0.93f, 0.0f), (float)Math.PI / 6 + 0.1f));
            setAgentVelocity(4, Vector2.rotation(new Vector2(0.93f, 0.0f), (float)Math.PI / 6 - 0.1f));
            setAgentVelocity(5, Vector2.rotation(new Vector2(0.87f, 0.0f), 2 * ((float)Math.PI / 6 - 0.1f)));

            detectGroups(false);
            Console.Write("Detect Groups\n");
            for (int i = 0; i < agents_.Count; i++)
            {
                Console.Write("Agent:" + i + " Address:" + agents_[i] + " group:" + agents_[i].groupBelongingTo_ + "\n");
            }
            GroupAgent group1 = agents_[1].groupBelongingTo_;

            Console.Write("\n Group1 : Agents = ");
            for (int i = 0; i < group1.agents_.Count; i++)
            {
                Console.Write(group1.agents_[i] + " ");
            }
            Console.Write("\n\n");


            // Test Represent group
            setAgentVelocity(0, new Vector2(1, 0));
            detectGroups(false);
            Console.Write("Detect Groups\n");
            for (int i = 0; i < agents_.Count; i++)
            {
                Console.Write("Agent:" + i + " Address:" + agents_[i] + " group:" + agents_[i].groupBelongingTo_ + "\n");
            }
            group1 = agents_[1].groupBelongingTo_;
            Console.Write("\n Group1 : Agents = ");
            for (int i = 0; i < group1.agents_.Count; i++)
            {
                Console.Write(group1.agents_[i] + " ");
            }
            Console.Write("\n\n");

            addAgent(new Vector2(3, 3), 0, true, true);
            SuperAgent sa = group1.RepresentGroup(agents_.Last());

            Console.Write("Represent group\n");
            Console.Write("Position: " + sa.position_ + ".  Velocity: " + sa.velocity_ + ". Radius: " + sa.radius_ + "\n");
            setAgentPosition(agents_.Count - 1, new Vector2(2, 1.5f));
            sa = group1.RepresentGroup(agents_.Last());
            Console.Write("Position: " + sa.position_ + ".  Velocity: " + sa.velocity_ + ". Radius: " + sa.radius_ + "\n");
            setAgentPosition(agents_.Count - 1, new Vector2(0.75f, 0.75f));
            sa = group1.RepresentGroup(agents_.Last());
            Console.Write("Position: " + sa.position_ + ".  Velocity: " + sa.velocity_ + ". Radius: " + sa.radius_ + "\n");
        }
Esempio n. 8
0
        private void placeAgents()
        {
            NormalDistribution normal = new NormalDistribution();

            normal.Mu    = 1.2;
            normal.Sigma = Math.Sqrt(0.3);

            MT19937Generator  generator = new MT19937Generator();
            StandardGenerator sg        = new StandardGenerator();

            ContinuousUniformDistribution x_distribution = new ContinuousUniformDistribution(generator);
            NormalDistribution            y_distribution = new NormalDistribution(generator);

            y_distribution.Mu    = corridor_width_ / 2;
            y_distribution.Sigma = sim_.getDefaultRadius();
            for (int ped = 0; ped < ped_num_; ped++)
            {  // Place Agent
                float x = (float)x_distribution.NextDouble() * corridor_length_ % corridor_length_;
                float y = (float)((y_distribution.NextDouble() * corridor_width_) - 9) % corridor_width_;

                Vector2 position = new Vector2(x, Math.Abs(y));
                position = Vector2.rotation(position, corridor_angle_);

                sim_.addAgent(position, model_type_, follow_, group_);

                addAgent(prefab, new Vector3(position.x(), 0, position.y()), sim_.getDefaultRadius());


                step_stop.Add(0);

                // Set agent max speeds
                sim_.setAgentMaxSpeed(ped, (float)normal.NextDouble());
                if (sim_.getAgentMaxSpeed(ped) > 2.0f)
                {
                    sim_.setAgentMaxSpeed(ped, 2.0f);
                }
                if (sim_.getAgentMaxSpeed(ped) < 0.3f)
                {
                    sim_.setAgentMaxSpeed(ped, 0.3f);
                }

                // Set agent's goal

                /* Change the switch in case 2 like :
                 * Instantiate 2 Agents (one at each side) in a geometric way
                 * Add color and goals.
                 */
                switch (fluxes_)
                {
                case 1:
                {
                    Vector2 corridor_end = new Vector2(corridor_length_ + 100, y);
                    //Vector2 corridor_end = new Vector2(corridor_length_ + 12, y);
                    corridor_end = Vector2.rotation(corridor_end, corridor_angle_);
                    sim_.setAgentGoal(ped, corridor_end);
                    break;
                }

                case 2:
                {
                    if (ped < ped_num_ / 2)
                    {
                        Vector2 corridor_end = new Vector2(corridor_length_ + 1, y);
                        corridor_end = Vector2.rotation(corridor_end, corridor_angle_);
                        agents[ped].transform.GetComponent <MeshRenderer>().material.color = new Color(1, 0, 0);
                        sim_.setAgentGoal(ped, corridor_end);
                    }
                    else
                    {
                        Vector2 corridor_start = new Vector2(-100, y);
                        corridor_start = Vector2.rotation(corridor_start, corridor_angle_);
                        agents[ped].transform.GetComponent <MeshRenderer>().material.color = new Color(0, 0, 1);
                        sim_.setAgentGoal(ped, corridor_start);
                    }
                    break;
                }

                default:
                    break;
                }
            }
        }
Esempio n. 9
0
        void setAgentsProperties()
        {
            for (int i = 0; i < sim_.getNumAgents(); ++i)
            {  // Set Agent Goal
                Vector2 pos  = sim_.getAgentPosition(i);
                Vector2 goal = sim_.getAgentGoal(i);
                // Position in the corridor referential
                Vector2 local_pos  = Vector2.rotation(pos, -corridor_angle_);
                Vector2 local_goal = Vector2.rotation(goal, -corridor_angle_);
                // Set agent goal
                Vector2 new_goal = new Vector2(local_goal.x(), local_pos.y());
                // Back to world's referential
                new_goal = Vector2.rotation(new_goal, corridor_angle_);
                // Set goal
                sim_.setAgentGoal(i, new_goal);

                // Set Agent Position (looped corridor)
                // If the agent as reached the end of the corridor (case 1)
                if (local_pos.x() >= corridor_length_ && local_goal.x() > corridor_length_)
                {
                    // Put at the start of the corridor
                    Vector2 new_pos = new Vector2(local_pos.x() - (corridor_length_), local_pos.y());
                    // Back to world's referential
                    new_pos = Vector2.rotation(new_pos, corridor_angle_);
                    // Add agent
                    sim_.setAgentPosition(i, new_pos);
                    // Save agent's data
                    //DataSaving::saveAgentData(sim_, i, follow_);
                    // Reinitialize data
                    sim_.reinitializeOutputVariables(i);
                }
                if (pos.y() > corridor_width_ || pos.y() < 0)
                {
                    System.Random rand    = new System.Random();
                    Vector2       new_pos = new Vector2(pos.x_, rand.Next((int)corridor_width_ + 1));
                    // Back to world's referential
                    new_pos = Vector2.rotation(new_pos, corridor_angle_);
                    // Add agent
                    sim_.setAgentPosition(i, new_pos);
                    // Save agent's data
                    //DataSaving::saveAgentData(sim_, i, follow_);
                    // Reinitialize data
                    sim_.reinitializeOutputVariables(i);
                }

                // If the agent as reached the end of the corridor (case 2)
                if (local_pos.x() < 0 && local_goal.x() < 0)
                {
                    // Put at the start of the corridor
                    Vector2 new_pos = new Vector2(local_pos.x() + corridor_length_, local_pos.y());
                    // Back to world's referential
                    new_pos = Vector2.rotation(new_pos, corridor_angle_);
                    // Add agent
                    sim_.setAgentPosition(i, new_pos);
                    // Save agent's data
                    //DataSaving::saveAgentData(sim_, i, following_behavior_);
                    // Reinitialize data
                    sim_.reinitializeOutputVariables(i);
                }
            }
        }
Esempio n. 10
0
        internal SuperAgent representGroup(Agent observer)
        {
            // Compute the left & right tangent points obtained for each agent of the group
            // First element of the pair = right tangent
            // Second element of the pair = left tangent
            IList <Pair <Vector2, Vector2> > tangents = new List <Pair <Vector2, Vector2> >();
            IList <Pair <Vector2, Vector2> > radii    = new List <Pair <Vector2, Vector2> >();

            for (int i = 0; i < agents_.Count; i++)
            {
                tangents.Add(computeTangentsPoints(observer, agents_[i]));
                Pair <Vector2, Vector2> rads = new Pair <Vector2, Vector2>();
                rads.First  = tangents[i].First - observer.position_;
                rads.Second = tangents[i].Second - observer.position_;
                radii.Add(rads);
            }
            // Compute the group tangent points (extrema)
            int rightExtremumId = 0;
            int leftExtremumId  = 0;

            for (int i = 1; i < tangents.Count; i++)
            {
                // Comparison
                if (Vector2.isOnTheLeftSide(radii[rightExtremumId].First, radii[i].First))
                {
                    rightExtremumId = i;
                }
                if (Vector2.isOnTheLeftSide(radii[i].Second, radii[leftExtremumId].Second))
                {
                    leftExtremumId = i;
                }
            }
            // If the tangent are taking more than 180°, cannot be considered as a group
            for (int i = 0; i < agents_.Count; i++)
            {
                if (Vector2.isOnTheLeftSide(radii[rightExtremumId].First, radii[i].First))
                {
                    //std::cout << "Problem representing groups : tangent angle > 180°\n";
                    return(new SuperAgent(null));
                }
                if (Vector2.isOnTheLeftSide(radii[i].Second, radii[leftExtremumId].Second))
                {
                    //std::cout << "Problem representing groups : tangent angle > 180°\n";
                    return(new SuperAgent(null));
                }
            }
            // Compute bisector
            Vector2 rightTangent      = Vector2.rotation(radii[rightExtremumId].First, (float)Math.PI / 2);
            Vector2 leftTangent       = Vector2.rotation(radii[leftExtremumId].Second, -(float)Math.PI / 2);
            Vector2 intersectionPoint = Vector2.intersectOf2Lines(tangents[rightExtremumId].First, tangents[rightExtremumId].First + rightTangent,
                                                                  tangents[leftExtremumId].Second, tangents[leftExtremumId].Second + leftTangent);
            // alpha/2 is more usefull than alpha
            float alpha2 = Vector2.angle(Vector2.rotation(tangents[leftExtremumId].Second - intersectionPoint, -Vector2.angle(tangents[rightExtremumId].First - intersectionPoint))) / 2;

            if (alpha2 < 0)
            {
                //std::cout << "Problem representing groups : angle computation\n SHALL NOT HAPPEN !!! \n";
                // But if radii are different or if
                return(new SuperAgent(null));
            }
            Vector2 bisector_normalize_vector = Vector2.normalize(observer.position_ - intersectionPoint);
            // Compute circle
            // The distance between the observer and the circle (along the bisector axis)
            float d = Single.PositiveInfinity;
            float a, b, c, delta, x;
            int   constrainingAgent = 0;

            for (int i = 0; i < agents_.Count; i++)
            {
                Vector2 ic1 = agents_[i].position_ - intersectionPoint;
                a     = 1 - Vector2.sqr((float)Math.Sin(alpha2));
                b     = 2 * (agents_[i].radius_ * (float)Math.Sin(alpha2) - ic1 * bisector_normalize_vector);
                c     = Vector2.absSq(ic1) - Vector2.sqr(agents_[i].radius_);
                delta = Vector2.sqr(b) - 4 * a * c;
                if (delta <= 0)
                {
                    if (delta < -4 * RVO_EPSILON * c)
                    {
                        return(new SuperAgent(null));
                    }
                    else
                    {
                        delta = 0;
                    }
                }
                x = (-b + (float)Math.Sqrt(delta)) / (2 * a);
                if (x < d)
                {
                    d = x;
                    constrainingAgent = i;
                }
            }
            if (d < Vector2.abs(observer.position_ - intersectionPoint) + observer.radius_ + d * Math.Sin(alpha2))
            {
                return(new SuperAgent(null));
            }
            SuperAgent toReturn     = new SuperAgent(sim_);

            toReturn.position_ = intersectionPoint + bisector_normalize_vector * d;
            toReturn.radius_   = d * (float)Math.Sin(alpha2);
            toReturn.velocity_ = agents_[constrainingAgent].velocity_;
            return(toReturn);
        }