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