/** * <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; } } } } }
/** * <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()); } }
/** * <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(); } }
public void detectGroups(bool looped) { // Deal with looped scenarios IList <Agent> agents; if (looped) { agents = virtual_and_agents_; } else { agents = agents_; } // Remove groups from last step for (int i = 0; i < agents_.Count; i++) { agents_[i].groupBelongingTo_ = null; } groupAgents_.Clear(); for (int i = 0; i < agents.Count - 1; i++) { Agent agent = agents[i]; for (int neighbor_id = 0; neighbor_id < agent.agentNeighbors_.Count; ++neighbor_id) { Agent toCompare = agents[agent.agentNeighbors_[neighbor_id].Value.id_]; float distance = Vector2.abs(agent.position_ - toCompare.position_); float delta_angle = Math.Abs(Vector2.angle(agent.velocity_) - Vector2.angle(toCompare.velocity_)); float delta_speed = Math.Max(Vector2.abs(agent.velocity_), Vector2.abs(toCompare.velocity_)) - Math.Min(Vector2.abs(agent.velocity_), Vector2.abs(toCompare.velocity_)); // Check distance and velocity raints if (distance < defaultAgent_.radius_ * 5 && (delta_angle < Math.PI / 6 || delta_angle > 11 * Math.PI / 6) && delta_speed < 0.2 * Math.Max(agent.maxSpeed_, toCompare.maxSpeed_) ) { mergeGroups(agent, toCompare); } } } }
// 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; } } }
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); }