/**
* <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);
}
