// Use this for initialization
void Start()
{
group_b = group.isOn;
avoidance_b = avoidance.isOn;
Application.targetFrameRate = 60;
agents = new List <Transform>();
/* Set up the scenario. */
setupScenario();
Instantiate(wall, new Vector3(0, 12.5f, 40.5f), Quaternion.identity);
Instantiate(wall, new Vector3(0, 12.5f, -39.5f), Quaternion.identity);
/* Perform (and manipulate) the simulation. */
for (int i = 0; i < getNumAgents(); ++i)
{
RVO.Vector2 position = getPosition(i);
int mid = (group_b ? getNumAgents() / 2 : 1);
if (i < mid)
{
addAgent(agent_rose, new Vector3(position.x(), 0f, position.y()), sim_.getDefaultRadius());
}
else
{
addAgent(agent_vert, new Vector3(position.x(), 0f, position.y()), sim_.getDefaultRadius());
}
}
}
public static Vector3 SafeNormalized(this RVO.Vector2 _this)
{
float magSqrd = _this.x() * _this.x() + _this.y() * _this.y();
if (magSqrd > 0.0001f)
{
var mag = Mathf.Sqrt(magSqrd);
return(new Vector3(_this.x() / mag, 0, _this.y() / mag));
}
return(Vector3.zero);
}
// Update is called once per frame
void Update()
{
if (!reachedGoal())
{
setPreferredVelocities();
doStep(false);
/* Output the current global time. */
//print(Simulator.Instance.getGlobalTime());
for (int i = 0; i < getNumAgents(); ++i)
{
RVO.Vector2 position = getPosition(i);
if (position.x() >= 80.0f)
{
if (RVO.Vector2.abs(sim_.getAgentVelocity(i)) > sim_.getAgentMaxSpeed() * 3 / 5)
{
setVelocity(i, sim_.getAgentVelocity(i) / 2);
}
position = getPosition(i);
agents[i].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 (position.x() >= 90.0f)
{
setPosition(i, new RVO.Vector2(-140, position.y()));
position = getPosition(i);
agents[i].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_));*/
}
else
{
agents[i].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_));*/
}
setColor(i);
}
}
else
{
for (int i = 0; i < getNumAgents(); ++i)
{
agents[i].GetComponent <Rigidbody>().isKinematic = true;
}
}
}
/*!
* @brief Computes the intersection point of two circles
* @param c1 The center of the first circle
* @param r1 The radius of the first circle
* @param c2 The center of the second circle
* @param r2 The radius of the second circle
* @returns The intersection point
*/
public static Pair <Vector2, Vector2> intersectOf2Circles(Vector2 c1, float r1, Vector2 c2, float r2)
{
Pair <Vector2, Vector2> toReturn = new Pair <Vector2, Vector2>();
toReturn.First = new Vector2(Single.PositiveInfinity, Single.PositiveInfinity);
toReturn.Second = new Vector2(Single.PositiveInfinity, Single.PositiveInfinity);
if (absSq(c1 - c2) < sqr(r1 - r2) - RVOMath.RVO_EPSILON || absSq(c1 - c2) > sqr(r1 + r2) + RVOMath.RVO_EPSILON)
{
// ERROR_ case, no intersection
return(toReturn);
}
else
{
if (Math.Abs(c1.y() - c2.y()) < RVOMath.RVO_EPSILON)
{
float x = (sqr(r1) - sqr(r2) - sqr(c1.x()) + sqr(c2.x())) / (2 * (c2.x() - c1.x()));
// a = 1
float b = -2 * c2.y();
float c = absSq(c2) - sqr(r2) + sqr(x) - 2 * x * c2.x();
float delta = sqr(b) - 4 * c;
float y1 = (float)(-b + Math.Sqrt(delta)) / 2;
float y2 = (float)(-b - Math.Sqrt(delta)) / 2;
toReturn.First = new Vector2(x, y1);
toReturn.Second = new Vector2(x, y2);
}
else
{
float M = (c2.x() - c1.x()) / (c2.y() - c1.y());
float N = (sqr(r1) - sqr(r2) - absSq(c1) + absSq(c2)) / (2 * (c2.y() - c1.y()));
float a = 1 + sqr(M);
float b = 2 * (M * (c2.y() - N) - c2.x());
float c = absSq(c2) + sqr(N) - 2 * N * c2.y() - sqr(r2);
float delta = sqr(b) - 4 * a * c;
float x1 = (float)(-b + Math.Sqrt(delta)) / (2 * a);
float x2 = (float)(-b - Math.Sqrt(delta)) / (2 * a);
float y1 = N - x1 * M;
float y2 = N - x2 * M;
toReturn.First = new Vector2(x1, y1);
toReturn.Second = new Vector2(x2, y2);
}
return(toReturn);
}
}
void move()
{
if (interactScript.isSit)
{
return;
}
RVO.Vector2 agentVelRVO = Simulator.Instance.getAgentVelocity(agentId);
RVO.Vector2 agentPosRVO = Simulator.Instance.getAgentPosition(agentId);
UnityEngine.Vector2 agentVel = toUnityVector2(agentVelRVO);
Vector3 agentPos = toUnityVector(agentPosRVO);
float remDist = Vector3.Distance(agentPos, agent.destination);
shouldMove = agentVel.magnitude * velFactor > 0.01f && remDist > agent.radius;
float velx = Mathf.Lerp(-0.5f, 0.5f, agentVelRVO.x());
velx *= transform.forward.x;
float vely = Mathf.Lerp(-0.5f, 0.5f, agentVelRVO.y());
vely *= transform.forward.z;
// Update animation parameters
anim.SetBool("move", shouldMove);
anim.SetFloat("velx", velx);
anim.SetFloat("vely", vely);
checkPeople(); // look at near people
}
void OnDrawGizmos()
{
if (!Debug)
{
return;
}
Gizmos.color = Color.red;
// ORCA
Vector3 from;
Vector3 to;
foreach (Line msGizmosLine in msGizmosLines)
{
RVO.Vector2 from_ = msGizmosLine.point - msGizmosLine.direction * 100;
RVO.Vector2 to_ = msGizmosLine.point + msGizmosLine.direction * 100;
from = transform.position + new Vector3(from_.x(), 0, from_.y());
to = transform.position + new Vector3(to_.x(), 0, to_.y());
Gizmos.DrawLine(from, to);
}
msGizmosLines.Clear();
// velocity
Gizmos.color = Color.green;
from = transform.position;
to = transform.position + new Vector3(msVelocity.x(), 0, msVelocity.y());
Gizmos.DrawLine(from, to);
}
public void updateVelo()
{
//Debug.Log("Agent " + agentId + " - " + RVOMath.abs(agentReference.velocity_) + " with path length" + path);
//If the agent doesnt have a path but it needs to move to avoid collision
if (pathStatus == -1 && RVOMath.abs(AgentReference.velocity_) >= 0.01f && !forced)
{
forced = true;
stableLocation = transform.position;
}
goalDirection = new Vector2(AgentReference.velocity_.x() / RVOMagnify.Magnify,
AgentReference.velocity_.y() / RVOMagnify.Magnify);
transform.position = new Vector3(AgentReference.position_.x_ / RVOMagnify.Magnify,
transform.position.y, AgentReference.position_.y_ / RVOMagnify.Magnify);
if (new Vector3(goalDirection.x(), 0f, goalDirection.y()).magnitude != 0)
{
Quaternion rotation = Quaternion.LookRotation(new Vector3(goalDirection.x(), 0f, goalDirection.y()));
rotation.x = 0;
rotation.z = 0;
transform.rotation = Quaternion.Slerp(transform.rotation, rotation, Time.deltaTime * RVOMagnify.Magnify);
}
// }
// Debug.Log("goal direction:" + goalDirection + "with length" + RVOMath.abs(goalDirection)+" Velocity is:" + new Vector3(agentReference.velocity_.x(), 0, agentReference.velocity_.y()));
//Debug.Log("Pos" + transform.position + "with RVO" + agentReference.position_ );
}
// Update is called once per frame
void Update()
{
if (!reachedGoal())
{
setPreferredVelocities();
doStep(false);
/* Output the current global time. */
//print(Simulator.Instance.getGlobalTime());
for (int i = 0; i < getNumAgents(); ++i)
{
RVO.Vector2 position = getPosition(i);
agents[i].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_));*/
//setColor(i); To go further
}
}
else
{
for (int i = 0; i < getNumAgents(); ++i)
{
agents[i].GetComponent <Rigidbody>().isKinematic = true;
}
}
}
/**
* <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;
}
}
}
}
}
void calculateVandS()
{
Vector2 pos_new = Simulator.Instance.getAgentPosition(0);
double car_direction = mainCar.transform.eulerAngles.y / 180.0 * Math.PI;
double dx = pos_new.x() - mainCar.transform.position.x;
double dz = pos_new.y() - mainCar.transform.position.z;
double target_direction = -Math.Atan2(dz, dx) + Math.PI / 2;
double delta_direction = target_direction - car_direction;
while (delta_direction > Math.PI)
{
delta_direction -= Math.PI * 2;
}
while (delta_direction < -Math.PI)
{
delta_direction += Math.PI * 2;
}
SteeringAngle = (float)(delta_direction / Math.PI * 180.0 / m_Car.MaxSteerAngleInDegree);
// double dt_direction = car_direction - pre_direction;
// pre_direction = car_direction;
// Debug.Log("dt_direction " + (dt_direction * 50 / Math.PI * 180.0).ToString());
// Debug.Log("delta_direction " + delta_direction.ToString());
// Debug.Log("SteeringAngle " + (SteeringAngle * m_Car.MaxSteerAngleInDegree).ToString());
SteeringAngle = Mathf.Clamp(SteeringAngle, -1, 1);
float current_speed = m_Car.CurrentSpeed;
var desired_velocity = Simulator.Instance.getAgentVelocity(0);
Vector3 des_v = new Vector3(desired_velocity.x(), 0, desired_velocity.y());
float desired_speed = des_v.magnitude;
// Debug.Log("pos_new " + pos_new.ToString());
// Debug.Log("current_speed " + current_speed.ToString());
// Debug.Log("desired_speed " + desired_speed.ToString());
//
// double zz = Math.Sqrt( dx * dx + dz * dz) * 50;
// Debug.Log("zz " + zz.ToString());
Acceleration = (desired_speed - current_speed) / 0.02f;
if (Acceleration > 1)
{
Acceleration = 1;
}
if (Acceleration < 0)
{
if (desired_speed - current_speed > -1)
{
Acceleration = 0;
}
}
//Debug.Log("frameIdx " + frameIdx.ToString() + " Acceleration " + Acceleration.ToString());
frameIdx++;
//if (frameIdx > 100) Acceleration = 0;
}
/*!
* @brief Computes the angle to the x axis of the specified
* two-dimensional vector
* @param vector The two-dimensional vector whose angle
* is to be computed
* @returns The angle of the two-dimensional vector.
*/
public static float angle(Vector2 vector)
{
/*if (vector.x() == 0)
* return (float)M_PI/2 * (-1 + 2*(vector.y()>0));
* else{*/
return((float)Math.Atan2(vector.y(), vector.x()));
//}
}
/**
* <summary>Unity function. Called once at the beginning of each frame. Used to perform the simulation</summary>
*/
void Update()
{
if (!reachedGoal())
{
// Set the preffered velocities of agents in order to assume they are in the good direction
setPreferredVelocities();
doStep(false);
for (int i = 0; i < getNumAgents(); ++i)
{
RVO.Vector2 position = getPosition(i);
// This condition ensure that agents stay at a limited speed in the end of the corridor
if (position.x() >= 80.0f)
{
if (RVO.Vector2.abs(sim_.getAgentVelocity(i)) > sim_.getAgentMaxSpeed() * 3 / 5)
{
setVelocity(i, sim_.getAgentVelocity(i) / 2);
}
position = getPosition(i);
agents[i].position = new Vector3(position.x(), 0f, position.y());
}
//This one is to replace agents at the beginning of the corridor if they are at the end
if (position.x() >= 90.0f)
{
setPosition(i, new RVO.Vector2(0, position.y()));
position = getPosition(i);
agents[i].position = new Vector3(position.x(), sim_.getAgentRadius(i) / 2, position.y());
RVO.Vector2 vector2 = sim_.getAgentVelocity(i);
agents[i].rotation = Quaternion.LookRotation(new Vector3(vector2.x_, 0, vector2.y_));
}
else // Else Juste perform the simulation and change the position if the unity agent to be the same at the RVOAgent
{
agents[i].position = new Vector3(position.x(), sim_.getAgentRadius(i) / 2, position.y());
RVO.Vector2 vector2 = sim_.getAgentVelocity(i);
agents[i].rotation = Quaternion.LookRotation(new Vector3(vector2.x_, 0, vector2.y_));
}
setColor(i);
}
}
else //If every agent reach their goals, block everybody
{ // Might want to do
for (int i = 0; i < getNumAgents(); ++i)
{
agents[i].GetComponent <Rigidbody>().isKinematic = true;
}
}
}
public void Start_Click()
{
if (numAgent.text.ToString() != "")
{
agents_number = Int32.Parse(numAgent.text.ToString());
Debug.Log("Agents oui ");
}
else
{
agents_number = 40;
Debug.Log("Agents non ");
}
if (width.text.ToString() != "")
{
corridor_width = Int32.Parse(width.text.ToString());
Debug.Log("Width oui ");
}
else
{
corridor_width = 10;
Debug.Log("Width non ");
}
if (length.text.ToString() != "")
{
corridor_lenght = Int32.Parse(length.text.ToString());
Debug.Log("Length oui ");
}
else
{
corridor_lenght = 30;
Debug.Log("Length non ");
}
panel.SetActive(false);
Application.targetFrameRate = 60;
agents = new List <Transform>();
setupScenario();
colors.Add(new Color(0.000f, 0.000f, 0.804f));
colors.Add(new Color(0.118f, 0.565f, 1.000f));
colors.Add(new Color(0.251f, 0.878f, 0.816f));
colors.Add(new Color(0.400f, 0.804f, 0.667f));
colors.Add(new Color(0.604f, 0.804f, 0.196f));
for (int i = 0; i < getNumAgents(); i++)
{
RVO.Vector2 position = getPosition(i);
agents.Add(Instantiate(agent, new Vector3(position.x(), 1.5f, position.y()), Quaternion.identity));
agents[i].GetComponent <MeshRenderer>().material.color = colors[0];
agents[i].localScale = new Vector3(sim_.getAgentRadius(i), sim_.getAgentRadius(i), sim_.getAgentRadius(i));
}
}
/**
* <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();
}
}
// Update is called once per frame
void Update()
{
if (sid >= 0)
{
Vector2 pos = Simulator.Instance.getAgentPosition(sid);
Vector2 vel = Simulator.Instance.getAgentPrefVelocity(sid);
transform.position = new Vector3(pos.x(), transform.position.y, pos.y());
if (Math.Abs(vel.x()) > 0.01f && Math.Abs(vel.y()) > 0.01f)
{
transform.forward = new Vector3(vel.x(), 0, vel.y()).normalized;
}
}
if (!Input.GetMouseButton(1))
{
Simulator.Instance.setAgentPrefVelocity(sid, new Vector2(0, 0));
return;
}
Vector2 goalVector = GameMainManager.Instance.mousePosition - Simulator.Instance.getAgentPosition(sid);
//Debug.Log($"--vec1--{RVOMath.absSq(goalVector)}");
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector);
}
//Debug.Log($"--vec2--{RVOMath.absSq(goalVector)}");
Simulator.Instance.setAgentPrefVelocity(sid, goalVector);
/* Perturb a little to avoid deadlocks due to perfect symmetry. */
float angle = (float)m_random.NextDouble() * 2.0f * (float)Math.PI;
float dist = (float)m_random.NextDouble() * 0.0001f;
Simulator.Instance.setAgentPrefVelocity(sid, Simulator.Instance.getAgentPrefVelocity(sid) +
dist *
new Vector2((float)Math.Cos(angle), (float)Math.Sin(angle)));
}
// Use this for initialization
void Start()
{
agents = new List <Transform>(); //Initializing the list of Unity Agents
setupScenario(); //Calling setupScenario
this.gameObject.transform.localScale = new Vector3(7, 1, 7); // The script is attached to a plane, so this will change the size of the plane
this.gameObject.transform.position = new Vector3(0, 0, 0); //This will change the position of this plane
Application.targetFrameRate = 30; // This will fix the maximum framerate of the scene
for (int i = 0; i < getNumAgents(); i++) // loop -> For each agent
{
RVO.Vector2 position = getPosition(i); // Getting the position of the agent in the simulator
addAgent(agent, new Vector3(position.x(), 1.5f, position.y())); //Instantiating the Unity Agent at the correct position
agents[i].localScale = new Vector3(sim_.getAgentRadius(i), sim_.getAgentRadius(i), sim_.getAgentRadius(i)); //Adapting the size of the agent to the radius we set in the simulator
}
}
// Update is called once per frame
void Update()
{
if (!reachedGoal())
{
setAgentsProperties();
setPreferredVelocities();
sim_.initialize_virtual_and_agents();
for (int i = 0; i < getNumAgents(); i++)
{
RVO.Vector2 agent_position = sim_.getAgentPosition(i);
RVO.Vector2 p1 = agent_position + new RVO.Vector2(corridor_length_, 0);
RVO.Vector2 p2 = agent_position - new RVO.Vector2(corridor_length_, 0);
sim_.addVirtualAgent(0, p1);
sim_.addVirtualAgent(0, p2);
}
doStep(true);
int totot = getNumAgents();
for (int i = 0; i < getNumAgents(); ++i)
{
RVO.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 (RVO.Vector2.absSq(sim_.getAgentVelocity(i) * 4) > 1.5f)
{
agents[i].GetComponent <Animator>().CrossFade("mixamo.com", 10, 1);
}
agents[i].GetComponent <Animator>().speed = RVO.Vector2.absSq(sim_.getAgentVelocity(i) * 4);
}
}
}
else
{
for (int i = 0; i < getNumAgents(); ++i)
{
agents[i].transform.GetComponent <Rigidbody>().isKinematic = true;
}
}
}
void Update()
{
fpsTimer += Time.deltaTime;
fpsCounter++;
if (fpsTimer >= 1.0f)
{
fps = fpsCounter / fpsTimer;
fpsTimer = 0;
fpsCounter = 0;
}
if (!isRunning)
{
return;
}
int numAgents = RVO.Simulator.Instance.getNumAgents();
for (int i = 0; i < numAgents; ++i)
{
RVO.Vector2 curPos = RVO.Simulator.Instance.getAgentPosition(robotAgentIds[i]);
robots[i].position = new Vector3(curPos.x(), 0, curPos.y());
RVO.Vector2 goalVector = goals[i] - curPos;
if (RVO.RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVO.RVOMath.normalize(goalVector) * speed;
}
RVO.Simulator.Instance.setAgentPrefVelocity(robotAgentIds[i], goalVector);
/* Perturb a little to avoid deadlocks due to perfect symmetry. */
float angle = (float)random.NextDouble() * 2.0f * (float)Mathf.PI;
float dist = (float)random.NextDouble() * 0.1f;
RVO.Simulator.Instance.setAgentPrefVelocity(robotAgentIds[i],
RVO.Simulator.Instance.getAgentPrefVelocity(robotAgentIds[i]) +
dist * new RVO.Vector2((float)Mathf.Cos(angle), (float)Mathf.Sin(angle)));
}
RVO.Simulator.Instance.doStep();
}
// Use this for initialization
void Start()
{
agents = new List <Transform>();
/* Set up the scenario. */
setupScenario();
Instantiate(wall, new Vector3(-25, 12.5f, 35.5f), Quaternion.identity);
Instantiate(wall, new Vector3(-25, 12.5f, -35.5f), Quaternion.identity);
colors.Add(new Color(0.000f, 0.000f, 0.804f));
colors.Add(new Color(0.118f, 0.565f, 1.000f));
colors.Add(new Color(0.251f, 0.878f, 0.816f));
colors.Add(new Color(0.400f, 0.804f, 0.667f));
colors.Add(new Color(0.604f, 0.804f, 0.196f));
/* Perform (and manipulate) the simulation. */
for (int i = 0; i < getNumAgents(); i++)
{
RVO.Vector2 position = getPosition(i);
addAgent(agent, new Vector3(position.x(), 1.5f, position.y()));
agents[i].localScale = new Vector3(sim_.getAgentRadius(i), sim_.getAgentRadius(i), sim_.getAgentRadius(i));
}
}
void Update()
{
if (!reachedGoal())
{
setPreferredVelocities();
doStep(true);
/* if(sim_.agents_.Count < agents_number) {
* int j = new System.Random().Next(1,3);
* int k = new System.Random().Next(1, 8);
* RVO.Vector2 position2 = new RVO.Vector2(0,k* corridor_width/8);
* sim_.addAgent(position2, 0, true, true,15.0f, 10, 5.0f, 5.0f, j, 1.0f, new RVO.Vector2(0, 0));
* goals.Add(new RVO.Vector2(corridor_lenght * 4, corridor_width / 2));
* RVO.Vector2 position = getPosition(sim_.agents_.Count - 1);
* agents.Add(Instantiate(agent, new Vector3(position.x(), sim_.getAgentRadius(agents.Count - 1)/2, position.y()), Quaternion.identity));
* agents[agents.Count-1].GetComponent<MeshRenderer>().material.color = colors[0];
* agents[agents.Count - 1].localScale = new Vector3(sim_.getAgentRadius(agents.Count - 1), sim_.getAgentRadius(agents.Count - 1), sim_.getAgentRadius(agents.Count - 1));
* }*/
for (int i = 0; i < getNumAgents(); ++i)
{
RVO.Vector2 position = getPosition(i);
if (position.x() >= corridor_lenght / 8 * 7)
{
if (RVO.Vector2.abs(sim_.getAgentVelocity(i)) > sim_.getAgentMaxSpeed() * 3 / 5)
{
setVelocity(i, sim_.getAgentVelocity(i) * 7 / 8);
}
position = getPosition(i);
agents[i].position = new Vector3(position.x(), sim_.getAgentRadius(i) / 2, position.y());
/* RVO.Vector2 vector2 = sim_.getAgentVelocity(i);
* agents[i].rotation = Quaternion.LookRotation(new Vector3(vector2.x_, 0, vector2.y_));*/
}
if (position.x() >= corridor_lenght)
{
Collider[] et = Physics.OverlapSphere(new Vector3(-140, sim_.getDefaultRadius() * 2.1f, position.y_), sim_.getDefaultRadius() * 2);
if (et.Length > 0)
{
setPosition(i, new RVO.Vector2(position.x(), position.y()));
}
else
{
setPosition(i, new RVO.Vector2(0, position.y()));
/* sim_.addAgent(new RVO.Vector2(-140, position.y() + 1), 0, true, true);
* goals.Add(new RVO.Vector2(100.0f, 0f));
* agents.Add(Instantiate(agent, new Vector3(-140, 1.5f, position.y() + 1), Quaternion.identity));
* agents[agents.Count - 1].GetComponent<MeshRenderer>().material.color = colors[0];
* sim_.getAgent(getNumAgents() - 1).leader_ = sim_.getAgent(i);*/
}
position = getPosition(i);
agents[i].position = new Vector3(position.x(), sim_.getAgentRadius(i) / 2, position.y());
//RVO.Vector2 vector2 = sim_.getAgentVelocity(i);
//agents[i].rotation = Quaternion.LookRotation(new Vector3(vector2.x_, 0, vector2.y_));
}
else
{
agents[i].position = new Vector3(position.x(), sim_.getAgentRadius(i) / 2, position.y());
//RVO.Vector2 vector2 = sim_.getAgentVelocity(i);
//agents[i].rotation = Quaternion.LookRotation(new Vector3(vector2.x_, 0, vector2.y_));
}
setColor(i);
}
}
else
{
for (int i = 0; i < getNumAgents(); ++i)
{
agents[i].GetComponent <Rigidbody>().isKinematic = true;
}
}
}
/**
* <summary>Computes the new velocity of this agent.</summary>
*/
internal void computeNewVelocity()
{
orcaLines_.Clear();
float invTimeHorizonObst = 1.0f / timeHorizonObst_;
/* Create obstacle ORCA lines. */
for (int i = 0; i < obstacleNeighbors_.Count; ++i)
{
Obstacle obstacle1 = obstacleNeighbors_[i].Value;
Obstacle obstacle2 = obstacle1.next_;
Vector2 relativePosition1 = obstacle1.point_ - position_;
Vector2 relativePosition2 = obstacle2.point_ - position_;
/*
* Check if velocity obstacle of obstacle is already taken care
* of by previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false;
for (int j = 0; j < orcaLines_.Count; ++j)
{
if (RVOMath.det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON && RVOMath.det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
continue;
}
/* Not yet covered. Check for collisions. */
float distSq1 = RVOMath.absSq(relativePosition1);
float distSq2 = RVOMath.absSq(relativePosition2);
float radiusSq = RVOMath.sqr(radius_);
Vector2 obstacleVector = obstacle2.point_ - obstacle1.point_;
float s = (-relativePosition1 * obstacleVector) / RVOMath.absSq(obstacleVector);
float distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector);
Line line;
if (s < 0.0f && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1.convex_)
{
line.point = new Vector2(0.0f, 0.0f);
line.direction = RVOMath.normalize(new Vector2(-relativePosition1.y(), relativePosition1.x()));
orcaLines_.Add(line);
}
continue;
}
else if (s > 1.0f && distSq2 <= radiusSq)
{
/*
* Collision with right vertex. Ignore if non-convex or if
* it will be taken care of by neighboring obstacle.
*/
if (obstacle2.convex_ && RVOMath.det(relativePosition2, obstacle2.direction_) >= 0.0f)
{
line.point = new Vector2(0.0f, 0.0f);
line.direction = RVOMath.normalize(new Vector2(-relativePosition2.y(), relativePosition2.x()));
orcaLines_.Add(line);
}
continue;
}
else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = new Vector2(0.0f, 0.0f);
line.direction = -obstacle1.direction_;
orcaLines_.Add(line);
continue;
}
/*
* No collision. Compute legs. When obliquely viewed, both legs
* can come from a single vertex. Legs extend cut-off line when
* non-convex vertex.
*/
Vector2 leftLegDirection, rightLegDirection;
if (s < 0.0f && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle2 = obstacle1;
float leg1 = RVOMath.sqrt(distSq1 - radiusSq);
leftLegDirection = new Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
rightLegDirection = new Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
}
else if (s > 1.0f && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle1 = obstacle2;
float leg2 = RVOMath.sqrt(distSq2 - radiusSq);
leftLegDirection = new Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
rightLegDirection = new Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
}
else
{
/* Usual situation. */
if (obstacle1.convex_)
{
float leg1 = RVOMath.sqrt(distSq1 - radiusSq);
leftLegDirection = new Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
}
else
{
/* Left vertex non-convex; left leg extends cut-off line. */
leftLegDirection = -obstacle1.direction_;
}
if (obstacle2.convex_)
{
float leg2 = RVOMath.sqrt(distSq2 - radiusSq);
rightLegDirection = new Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
}
else
{
/* Right vertex non-convex; right leg extends cut-off line. */
rightLegDirection = obstacle1.direction_;
}
}
/*
* Legs can never point into neighboring edge when convex
* vertex, take cutoff-line of neighboring edge instead. If
* velocity projected on "foreign" leg, no constraint is added.
*/
Obstacle leftNeighbor = obstacle1.previous_;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
if (obstacle1.convex_ && RVOMath.det(leftLegDirection, -leftNeighbor.direction_) >= 0.0f)
{
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor.direction_;
isLeftLegForeign = true;
}
if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= 0.0f)
{
/* Right leg points into obstacle. */
rightLegDirection = obstacle2.direction_;
isRightLegForeign = true;
}
/* Compute cut-off centers. */
Vector2 leftCutOff = invTimeHorizonObst * (obstacle1.point_ - position_);
Vector2 rightCutOff = invTimeHorizonObst * (obstacle2.point_ - position_);
Vector2 cutOffVector = rightCutOff - leftCutOff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
float t = obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutOff) * cutOffVector) / RVOMath.absSq(cutOffVector);
float tLeft = (velocity_ - leftCutOff) * leftLegDirection;
float tRight = (velocity_ - rightCutOff) * rightLegDirection;
if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f))
{
/* Project on left cut-off circle. */
Vector2 unitW = RVOMath.normalize(velocity_ - leftCutOff);
line.direction = new Vector2(unitW.y(), -unitW.x());
line.point = leftCutOff + radius_ * invTimeHorizonObst * unitW;
orcaLines_.Add(line);
continue;
}
else if (t > 1.0f && tRight < 0.0f)
{
/* Project on right cut-off circle. */
Vector2 unitW = RVOMath.normalize(velocity_ - rightCutOff);
line.direction = new Vector2(unitW.y(), -unitW.x());
line.point = rightCutOff + radius_ * invTimeHorizonObst * unitW;
orcaLines_.Add(line);
continue;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is
* closest to velocity.
*/
float distSqCutoff = (t <0.0f || t> 1.0f || obstacle1 == obstacle2) ? float.PositiveInfinity : RVOMath.absSq(velocity_ - (leftCutOff + t * cutOffVector));
float distSqLeft = tLeft < 0.0f ? float.PositiveInfinity : RVOMath.absSq(velocity_ - (leftCutOff + tLeft * leftLegDirection));
float distSqRight = tRight < 0.0f ? float.PositiveInfinity : RVOMath.absSq(velocity_ - (rightCutOff + tRight * rightLegDirection));
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
line.direction = -obstacle1.direction_;
line.point = leftCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y(), line.direction.x());
orcaLines_.Add(line);
continue;
}
if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign)
{
continue;
}
line.direction = leftLegDirection;
line.point = leftCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y(), line.direction.x());
orcaLines_.Add(line);
continue;
}
/* Project on right leg. */
if (isRightLegForeign)
{
continue;
}
line.direction = -rightLegDirection;
line.point = rightCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y(), line.direction.x());
orcaLines_.Add(line);
}
int numObstLines = orcaLines_.Count;
float invTimeHorizon = 1.0f / timeHorizon_;
/* Create agent ORCA lines. */
for (int i = 0; i < agentNeighbors_.Count; ++i)
{
Agent other = agentNeighbors_[i].Value;
Vector2 relativePosition = other.position_ - position_;
Vector2 relativeVelocity = velocity_ - other.velocity_;
float distSq = RVOMath.absSq(relativePosition);
float combinedRadius = radius_ + other.radius_;
float combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line;
Vector2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
float wLengthSq = RVOMath.absSq(w);
float dotProduct1 = w * relativePosition;
if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
float wLength = RVOMath.sqrt(wLengthSq);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
/* Project on legs. */
float leg = RVOMath.sqrt(distSq - combinedRadiusSq);
if (RVOMath.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 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
float invTimeStep = 1.0f / Simulator.Instance.timeStep_;
/* Vector from cutoff center to relative velocity. */
Vector2 w = relativeVelocity - invTimeStep * relativePosition;
float wLength = RVOMath.abs(w);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
line.point = velocity_ + 0.5f * u;
orcaLines_.Add(line);
}
int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);
if (lineFail < orcaLines_.Count)
{
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
}
}
// Update is called once per frame
void FixedUpdate()
{
rallyIsReady = GameObject.FindGameObjectWithTag("Manager").GetComponent <UI_ButtonControl>().SpawnIsDone;
//UseAStar = true;
if (!rallyIsReady)
{
//Debug.Log("Is there new unit created? "+ GameObject.Find("AI").GetComponent<CreateAgent>().IsAdded.ToString());
CheckPushAway();
}
if (rallyIsReady)
{
if (Input.GetMouseButtonDown(0))
{
//Debug.Log("New click activated! Current target at click: " + Input.mousePosition.ToString());
Ray ray = cam.ScreenPointToRay(Input.mousePosition);
RaycastHit hit;
if (Physics.Raycast(ray, out hit))
{
target = hit.point;
Debug.Log("current target AFTER update: " + target.ToString());
//depending on the estimated length of path, decide if A* or potential field is used
// A* algorithm time complexity is O(b^d) ~ O((3)^PathLength)
float pathLengthEstimated = (Vector3.Distance(transform.position, new Vector3(target.x, transform.position.y, target.z)) / AstarPath.active.data.gridGraph.nodeSize);
long complexityEstimated = (int)Mathf.Pow(3, (int)pathLengthEstimated);
//Debug.Log("Log of Total number of nodes:" + (Mathf.Log(AstarPath.active.data.gridGraph.Depth * AstarPath.active.data.gridGraph.Width)).ToString());
//Debug.Log("Complexity of A* : " + (Mathf.Log(simulator.agentPositions.Count) + 3 * Mathf.Log(pathLengthEstimated)).ToString());
if (Mathf.Log(simulator.agentPositions.Count) + 3 * Mathf.Log(pathLengthEstimated) > Mathf.Log(AstarPath.active.data.gridGraph.Depth * AstarPath.active.data.gridGraph.Width))
{
Debug.Log("A* is not efficient due to long path or large number of agents...");
//update the pathnodes based on potential field method.
//generate the potential map based on destination target and gridgraph
var gridgraph = AstarPath.active.data.gridGraph;
int[,] potentialmap = new int[gridgraph.Depth, gridgraph.Width];
//generate potential map
potentialmap = GameObject.Find("PotentialMap").GetComponent <PotentialMapSet>().GetPotentialMap(target, gridgraph);
//check the potentialmap
//Debug.Log("potential map is obtained. Above origin, the potential is like : " + potentialmap[48, 49].ToString());
//Debug.Log("potential Map is obtained. Below origin, the potential is like : " + potentialmap[51, 49].ToString());
int[,] padded_potentialmap = GameObject.Find("PotentialMap").GetComponent <PotentialMapSet>().PadMap(potentialmap);
// visualize the potential map real-time(TODO)
// generate the field map by doing the gradient of padded potential map
UnityEngine.Vector2[,] fieldmap = GameObject.Find("PotentialMap").GetComponent <PotentialMapSet>().GetFieldMap(padded_potentialmap);
//Debug.Log("FieldMap is obtained. Above origin, the vector is like : " + fieldmap[49, 49].ToString());
//Debug.Log("FieldMap is obtained. Below origin, the vector is like : " + fieldmap[50, 49].ToString());
pathNodes = GameObject.Find("PotentialMap").GetComponent <PotentialMapSet>().GetPathFromFieldMap(transform.position, new Vector3(target.x, transform.position.y, target.z), fieldmap);
currentNodeIndex = 0;
}
else
{
seeker.StartPath(transform.position, new Vector3(target.x, transform.position.y, target.z), OnPathComplete);
//Debug.Log("new path AFTER update has waypoint list length: " + path.vectorPath.Count.ToString());
}
}
}
if (path == null || pathNodes == null)
{
Debug.Log("No available path found!");
return;
}
if (currentNodeIndex >= (pathNodes.Count - 1))
{
//Debug.Log("Path Finding ended!");
return;
}
if (agentIndex != -1)
{
RVO.Vector2 agent_update = simulator.getAgentPosition(agentIndex);
Vector3 moveTowards = new Vector3(agent_update.x(), transform.position.y, agent_update.y());
//Vector3 velocity = (moveTowards - transform.position).normalized * speed;
transform.position = moveTowards;
// Slow down smoothly upon approaching the end of the path
// This value will smoothly go from 1 to 0 as the agent approaches the last waypoint in the path.
//var speedFactor = reachedEndOfPath ? Mathf.Sqrt(distanceToWaypoint / nextWaypointDistance) : 1f;
//characterController.SimpleMove(velocity);
}
}
}
void doStep()
{
/* Create agent ORCA lines. */
for (int i = 0; i < 1; ++i)
{
Vector2 relativePosition = otherPosition_ - position_;
Vector2 relativeVelocity = velocity_ - otherVelocity_;
float distSq = RVOMath.absSq(relativePosition);
float combinedRadius = radius_ + otherRadius_;
float combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line;
Vector2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
float wLengthSq = RVOMath.absSq(w);
float dotProduct1 = w * relativePosition;
//Debug.Log("w:" + w.ToString() + " dotProduct1:" + dotProduct1.ToString());
if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
float wLength = RVOMath.sqrt(wLengthSq);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
/* Project on legs. */
float leg = RVOMath.sqrt(distSq - combinedRadiusSq);
if (RVOMath.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 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
float invTimeStep = 1.0f / Simulator.Instance.timeStep_;
/* Vector from cutoff center to relative velocity. */
Vector2 w = relativeVelocity - invTimeStep * relativePosition;
float wLength = RVOMath.abs(w);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
line.point = velocity_ + 0.5f * u;
Debug.Log("u:" + u.ToString() + " Point:" + line.point.ToString() + " Direction:" + line.direction.ToString());
orcaLines_.Add(line);
}
float maxSpeed = 2.0f;
Vector2 prefVelocity = new Vector2(1, 1);
Vector2 newVelocity = new Vector2(0, 0);
linearProgram1(orcaLines_, 0, maxSpeed, prefVelocity, false, ref newVelocity);
//Debug.Log("prefVelocity:" + prefVelocity.ToString() + " newVelocity:" + newVelocity.ToString());
/*
* int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);
*
* if (lineFail < orcaLines_.Count)
* {
* linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
* }
*/
}
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;
}
}
}
private void setAgentsProperties()
{
for (int i = 0; i < sim_.getNumAgents(); ++i)
{ // Set Agent Goal
RVO.Vector2 pos = sim_.getAgentPosition(i);
RVO.Vector2 goal = sim_.getAgentGoal(i);
// Position in the corridor referential
RVO.Vector2 local_pos = RVO.Vector2.rotation(pos, -corridor_angle_);
RVO.Vector2 local_goal = RVO.Vector2.rotation(goal, -corridor_angle_);
// Set agent goal
RVO.Vector2 new_goal = new RVO.Vector2(local_goal.x(), local_pos.y());
// Back to world's referential
new_goal = RVO.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
RVO.Vector2 new_pos = new RVO.Vector2(local_pos.x() - (corridor_length_), local_pos.y());
// Back to world's referential
new_pos = RVO.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();
RVO.Vector2 new_pos = new RVO.Vector2(pos.x_, rand.Next((int)corridor_width_ + 1));
// Back to world's referential
new_pos = RVO.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
RVO.Vector2 new_pos = new RVO.Vector2(local_pos.x() + corridor_length_, local_pos.y());
// Back to world's referential
new_pos = RVO.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);
}
}
}
public static RVO.Vector2 RotateVector(RVO.Vector2 _v, float _radians)
{
float cos = Mathf.Cos(_radians), sin = Mathf.Sin(_radians);
return(_v * cos + new RVO.Vector2(_v.y(), -_v.x()) * sin);
}
public static Vector3 XZ(this RVO.Vector2 _this)
{
return(new Vector3(_this.x(), 0, _this.y()));
}
RVO.Vector2 rotation(RVO.Vector2 vector, float angle)
{
float x_prime = (float)Math.Cos(angle) * vector.x() - (float)Math.Sin(angle) * vector.y();
float y_prime = (float)Math.Sin(angle) * vector.x() + (float)Math.Cos(angle) * vector.y();
return(new RVO.Vector2(x_prime, y_prime));
}
// 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;
}
}
}
//some helper functions
Vector3 toUnityPosition(RVO.Vector2 agentVector)
{
return(new Vector3(agentVector.x(), transform.position.y, agentVector.y()));
}