public float getAgentLocalDensity(int agentNo, bool looped, float obs_distance) { // Deal with looped scenarios IList <Agent> agents; if (looped) { agents = virtual_and_agents_; } else { agents = agents_; } int cpt = 0; Vector2 position = getAgentPosition(agentNo); for (int i = 0; i < agents.Count; i++) { if (Vector2.abs(position - agents[i].position_) < obs_distance) { cpt++; } } return(cpt / ((float)Math.PI * obs_distance * obs_distance)); }
public float getAgentDistanceWithCloserAgent(int agentNo, bool looped) { // Deal with looped scenarios IList <Agent> agents; if (looped) { agents = virtual_and_agents_; } else { agents = agents_; } float closer_distance = Single.PositiveInfinity; float distance; Vector2 agent_position = agents_[agentNo].position_; for (int i = 0; i < agents.Count; i++) { distance = Vector2.abs(agent_position - agents[i].position_); if (distance > RVO_EPSILON && distance < closer_distance) { closer_distance = distance; } } return(closer_distance); }
public override void interactWith(SuperAgent agent) { Vector2 relative_position = agent.position_ - agent.position_; // w_phi enables to deal with elliptical forces float w_phi = 1; float test = ((HelbingAgent)(agent)).motion_dir_ * Vector2.normalize(-relative_position); float test2 = ((1 + motion_dir_ * Vector2.normalize(-relative_position)) / 2); float A = A_; if (id_ == 0) { A = A_ * 5; } if (type_ == 1) { //w_phi = lambda_ + (1 - lambda_)*((1+motion_dir_*normalize(-relative_position))/2); w_phi = (float)Math.Pow(test + 1, lambda_) / (float)Math.Pow(2f, lambda_); } // Compute force float radius_m_dist = radius_ + agent.radius_ - Vector2.abs(relative_position); float exp_term = (radius_m_dist) / B_; Vector2 force = w_phi * A * (float)Math.Exp(exp_term) * Vector2.normalize(relative_position); // Body force if (radius_m_dist > 0) { force += k_ * radius_m_dist * Vector2.normalize(relative_position); } acceleration_ += force; }
public static void saveSimulatorData(RVOSimulator sim, bool follow) { string name = "Agents_moy_data.csv"; if (!File.Exists(name)) { string tmp = " "; foreach (Agent a in sim.agents_) { tmp += a.id_ + " : vitesse_moy \t"; } File.Create(name).Dispose(); using (TextWriter tw = new StreamWriter(name)) { tw.WriteLine(tmp); } } using (TextWriter tw = new StreamWriter(name, true)) { string tmp = " "; foreach (Agent a in sim.agents_) { tmp += " " + Vector2.abs(a.velocity_) + "\t"; } tw.WriteLine(tmp); } }
public override void interactWith(Obstacle obstacle) { // Get vertex Obstacle obstacle2 = obstacle.next_; // Distance to the vertex Vector2 vector2Vertex = Vector2.vectorToSegment(obstacle.point_, obstacle2.point_, position_); // w_phi enables to deal with elliptical forces float w_phi = 1; float test = motion_dir_ * Vector2.normalize(vector2Vertex); if (type_ == 1) { //w_phi = lambda_ + (1 - lambda_)*(1+motion_dir_*normalize(vector2Vertex)/2); w_phi = (float)(Math.Pow(test + 1, lambda_) / Math.Pow(2, lambda_)); } // Compute force float exp_term = (radius_ - Vector2.abs(vector2Vertex)) / B_; Vector2 force = w_phi * A_ * (float)Math.Exp(exp_term) * -1 * Vector2.normalize(vector2Vertex) / m_; // Body force if (Vector2.abs(vector2Vertex) < radius_) { force += k_ * (radius_ - Vector2.abs(vector2Vertex)) * -1 * Vector2.normalize(vector2Vertex) / m_; } acceleration_ += force; }
/** * <summary>Updates the two-dimensional position and two-dimensional * velocity of this agent.</summary> */ internal void update() { if (sim_.pas < 1) { velocityMed = Vector2.abs(velocity_); } else { velocityMed += Vector2.abs(velocity_); velocityMed /= 2; } velocityBuffer_.Add(velocity_); if (!Double.IsNaN(newVelocity_.x_) && !Double.IsNaN(newVelocity_.y_)) { velocity_ = newVelocity_; position_ += velocity_ * sim_.timeStep_; cpt_dist += Vector2.abs(velocity_) * sim_.getTimeStep(); cpt_time++; } else { newVelocity_ = new Vector2(0, 0); velocity_ = newVelocity_; position_ += velocity_ * sim_.timeStep_; cpt_dist += Vector2.abs(velocity_) * sim_.getTimeStep(); cpt_time++; } }
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); }
public override void computeNewVelocity() { acceleration_ = new Vector2(0, 0); // Compute goal's force Vector2 toto = Vector2.normalize(goal_ - position_); Vector2 toto2 = Vector2.normalize(goal_ - position_) - velocity_; Vector2 goal_force = (maxSpeed_ * Vector2.normalize(goal_ - position_) - velocity_) / tau_; acceleration_ = goal_force; // Direction of motion if (velocity_ != new Vector2(0, 0)) { motion_dir_ = Vector2.normalize(velocity_); } else { motion_dir_ = Vector2.normalize(goal_ - position_); } // Compute obstacles' forces for (int i = 0; i < obstacleNeighbors_.Count; ++i) { interactWith(obstacleNeighbors_[i].Value); } // Compute neighbors' forces interactWithAgents(); // Limit acceleration if (Vector2.abs(acceleration_) > 5) { acceleration_ = 5 * Vector2.normalize(acceleration_); } newVelocity_ = velocity_ + acceleration_ * sim_.getTimeStep(); if (Vector2.abs(newVelocity_) > maxSpeed_) { newVelocity_ = Vector2.normalize(newVelocity_) * maxSpeed_; } }
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); } } } }
public override void interactWith(SuperAgent agent) { SuperAgent other = agent; Vector2 relativePosition = other.position_ - position_; Vector2 relativeVelocity = velocity_ - other.velocity_; float distSq = (float)Math.Round(Vector2.absSq(relativePosition), 3); float combinedRadius = radius_ + other.radius_; float combinedRadiusSq = (float)Math.Round(Vector2.sqr(combinedRadius), 3); Vector2 w = new Vector2(); Line line = new Line(); Vector2 u; if (distSq > combinedRadiusSq) { /* No collision. */ w = relativeVelocity - (1.0f / timeHorizon_) * relativePosition; /* Vector from cutoff center to relative velocity. */ float wLengthSq = Vector2.absSq(w); Vector2 unitW = new Vector2(); float dotProduct1 = w * relativePosition; if (dotProduct1 < 0.0f && Vector2.sqr(dotProduct1) > combinedRadiusSq * wLengthSq) { /* Project on cut-off circle. */ float wLength = (float)Math.Round(Math.Sqrt(wLengthSq), 3); unitW = w / wLength; line.direction = new Vector2(unitW.y(), -unitW.x()); u = (combinedRadius * (1.0f / timeHorizon_) - wLength) * unitW; } else { /* Project on legs. */ float leg = (float)Math.Round(Math.Sqrt(distSq - combinedRadiusSq), 3); if (Vector2.det(relativePosition, w) > 0.0f) { /* Project on left leg. */ line.direction = new Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; } else { /* Project on right leg. */ line.direction = -new Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; } float dotProduct2 = (float)Math.Round(relativeVelocity * line.direction, 3); u = dotProduct2 * line.direction - relativeVelocity; } } else { /* Collision. Project on cut-off circle of time timeStep. */ float invTimeStep = (float)Math.Round(1.0f / sim_.getTimeStep(), 3); /* Vector from cutoff center to relative velocity. */ w = relativeVelocity - invTimeStep * relativePosition; float wLength = (float)Math.Round(Vector2.abs(w), 3); Vector2 unitW = w / wLength; line.direction = new Vector2(unitW.y(), -unitW.x()); u = (combinedRadius * invTimeStep - wLength) * unitW; } // This is where you can choose the proportion of responsabilities that each agents takes in avoiding collision line.point = velocity_ + 0.5f * u; orcaLines_.Add(line); }
// Update is called once per frame void Update() { for (int block = 0; block < workers.Count; ++block) { doneEvents_[block].Reset(); ThreadPool.QueueUserWorkItem(workers[block].clear); } WaitHandle.WaitAll(doneEvents_); updateWorker(workers.Count); if (!reachedGoal()) { if (hum_but_.isOn && !human_prefab) { int range = agents.Count; for (int i = 0; i < range; i++) { Destroy(agents[i].gameObject); addAgent(human, agents[i].position, sim_.getDefaultRadius(), i); } human_prefab = true; } else if (!hum_but_.isOn && human_prefab) { int range = agents.Count; for (int i = 0; i < range; i++) { Destroy(agents[i].gameObject); addAgent(prefab, agents[i].position, sim_.getDefaultRadius(), i); } human_prefab = false; } setAgentsProperties(); setPreferredVelocities(); sim_.initialize_virtual_and_agents(); for (int i = 0; i < getNumAgents(); i++) { Vector2 agent_position = sim_.getAgentPosition(i); Vector2 p1 = agent_position + new Vector2(corridor_length_, 0); sim_.addVirtualAgent(0, p1); } doStep(true); /* Output the current global time. */ //print(Simulator.Instance.getGlobalTime()); if (follow_but_.isOn != follow_) { follow_ = follow_but_.isOn; for (int i = 0; i < getNumAgents(); ++i) { sim_.agents_[i].follows_ = follow_; } } if (save_but_.isOn != sim_.save) { sim_.save = save_but_.isOn; } Vector3 pos3 = Camera.main.transform.position; Camera.main.transform.position = new Vector3(pos3.x, camera_height_.value, pos3.z); int totot = getNumAgents(); for (int i = 0; i < getNumAgents(); ++i) { Vector2 position = sim_.getAgentPosition(i); agents[i].transform.position = new Vector3(position.x(), 0f, position.y()); RVO.Vector2 vector2 = sim_.getAgentVelocity(i); agents[i].rotation = Quaternion.LookRotation(new Vector3(vector2.x_, 0, vector2.y_)); if (human_prefab) { if (Vector2.absSq(sim_.getAgentVelocity(i) * 4) > 1.5f) { agents[i].GetComponent <Animator>().CrossFade("mixamo.com", 10, 1); } agents[i].GetComponent <Animator>().speed = Vector2.absSq(sim_.getAgentVelocity(i) * 4); } if (!human_prefab) { setColor(i); } } } else { for (int i = 0; i < getNumAgents(); ++i) { agents[i].transform.GetComponent <Rigidbody>().isKinematic = true; } } for (int block = 0; block < workers.Count; ++block) { doneEvents_[block].Reset(); ThreadPool.QueueUserWorkItem(workers[block].computeMedVelocity); } WaitHandle.WaitAll(doneEvents_); String tmp = ""; for (int i = 0; i < workers.Count; i++) { tmp += Vector2.abs(workers[i].vit_moy) + "\t"; } using (TextWriter tw = new StreamWriter(name, true)) { tw.WriteLine(tmp); } }
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); }