/**
* <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;
}
}
}
}
}
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_ );
}
/*!
* @brief Computes a rotated vector of the specified two-dimensional
* vector with the specified angle
* @param vector The two-dimensional vector whose rotation
* is to be computed.
* @param double The angle of the rotation
* @returns The rotation of the two-dimensional vector.
*/
public static Vector2 rotation(Vector2 vector, float angle)
{
float x_prime = (float)Math.Round(Math.Cos(angle) * vector.x() - (float)Math.Sin(angle) * vector.y(), 3);
float y_prime = (float)Math.Round(Math.Sin(angle) * vector.x() + (float)Math.Cos(angle) * vector.y(), 3);
return(new Vector2(x_prime, y_prime));
}
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);
}
/*!
* @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);
}
}
/**
* <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());
}
}
/*!
* @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>Updates the two-dimensional position and two-dimensional
* velocity of this agent.</summary>
*/
internal void update()
{
if (float.IsNaN(newVelocity_.x()))
{
//Debug.LogError("0");
}
else
{
velocity_ = newVelocity_;
position_ += velocity_ * Simulator.Instance.timeStep_;
}
}
/**
* <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();
}
}
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();
}
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;
}
}
}
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);
}
private double getDistance(Vector2 v1, Vector2 v2)
{
return(Math.Sqrt(Math.Pow(v2.x() - v1.x(), 2) + Math.Pow(v2.y() - v1.y(), 2)));
}
Vector3 toUnityPosition(RVO.Vector2 agentVector)
{
return(new Vector3(agentVector.x(), transform.position.y, agentVector.y()));
}
public override void interactWith(Obstacle obstacle)
{
Obstacle obstacle1 = obstacle;
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 (Vector2.det((1.0f / timeHorizonObst_) * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - (1.0f / timeHorizonObst_) * radius_ >= -RVO_EPSILON &&
Vector2.det((1.0f / timeHorizonObst_) * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - (1.0f / timeHorizonObst_) * radius_ >= -RVO_EPSILON)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
return;
}
/* Not yet covered. Check for collisions. */
float distSq1 = Vector2.absSq(relativePosition1);
float distSq2 = Vector2.absSq(relativePosition2);
float radiusSq = Vector2.sqr(radius_);
Vector2 obstacleVector = obstacle2.point_ - obstacle1.point_;
float s = (-relativePosition1 * obstacleVector) / Vector2.absSq(obstacleVector);
float distSqLine = Vector2.absSq(-relativePosition1 - s * obstacleVector);
Line line;
if (s < 0 && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1.convex_)
{
line.point = new Vector2(0, 0);
line.direction = Vector2.normalize(new Vector2(-relativePosition1.y(), relativePosition1.x()));
orcaLines_.Add(line);
}
return;
}
else if (s > 1 && distSq2 <= radiusSq)
{
/* Collision with right vertex. Ignore if non-convex
* or if it will be taken care of by neighoring obstace */
if (obstacle2.convex_ && Vector2.det(relativePosition2, obstacle2.direction_) >= 0)
{
line.point = new Vector2(0, 0);
line.direction = Vector2.normalize(new Vector2(-relativePosition2.y(), relativePosition2.x()));
orcaLines_.Add(line);
}
return;
}
else if (s >= 0 && s < 1 && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = new Vector2(0, 0);
line.direction = -obstacle1.direction_;
orcaLines_.Add(line);
return;
}
/*
* No collision.
* Compute legs. When obliquely viewed, both legs can come from a single
* vertex. Legs extend cut-off line when nonconvex vertex.
*/
Vector2 leftLegDirection, rightLegDirection;
if (s < 0 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1.convex_)
{
/* Ignore obstacle. */
return;
}
obstacle2 = obstacle1;
float leg1 = (float)Math.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 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2.convex_)
{
/* Ignore obstacle. */
return;
}
obstacle1 = obstacle2;
float leg2 = (float)Math.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 = (float)Math.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 = (float)Math.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 (obstacle.convex_ && Vector2.det(leftLegDirection, -leftNeighbor.direction_) >= 0.0f)
{
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor.direction_;
isLeftLegForeign = true;
}
if (obstacle2.convex_ && Vector2.det(rightLegDirection, obstacle2.direction_) <= 0.0f)
{
/* Right leg points into obstacle. */
rightLegDirection = obstacle2.direction_;
isRightLegForeign = true;
}
/* Compute cut-off centers. */
Vector2 leftCutoff = (1.0f / timeHorizonObst_) * (obstacle1.point_ - position_);
Vector2 rightCutoff = (1.0f / timeHorizonObst_) * (obstacle2.point_ - position_);
Vector2 cutoffVec = 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) * cutoffVec) / Vector2.absSq(cutoffVec));
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 = Vector2.normalize(velocity_ - leftCutoff);
line.direction = new Vector2(unitW.y(), -unitW.x());
line.point = leftCutoff + radius_ * (1.0f / timeHorizonObst_) * unitW;
orcaLines_.Add(line);
return;
}
else if (t > 1.0f && tRight < 0.0f)
{
/* Project on right cut-off circle. */
Vector2 unitW = Vector2.normalize(velocity_ - rightCutoff);
line.direction = new Vector2(unitW.y(), -unitW.x());
line.point = rightCutoff + radius_ * (1.0f / timeHorizonObst_) * unitW;
orcaLines_.Add(line);
return;
}
/*
* 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) ? Single.PositiveInfinity : Vector2.absSq(velocity_ - (leftCutoff + t * cutoffVec)));
float distSqLeft = ((tLeft < 0.0f) ? Single.PositiveInfinity : Vector2.absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection)));
float distSqRight = ((tRight < 0.0f) ? Single.PositiveInfinity : Vector2.absSq(velocity_ - (rightCutoff + tRight * rightLegDirection)));
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
line.direction = -obstacle1.direction_;
line.point = leftCutoff + radius_ * (1.0f / timeHorizonObst_) * new Vector2(-line.direction.y(), line.direction.x());
orcaLines_.Add(line);
return;
}
else if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign)
{
return;
}
line.direction = leftLegDirection;
line.point = leftCutoff + radius_ * (1.0f / timeHorizonObst_) * new Vector2(-line.direction.y(), line.direction.x());
orcaLines_.Add(line);
return;
}
else
{
/* Project on right leg. */
if (isRightLegForeign)
{
return;
}
line.direction = -rightLegDirection;
line.point = rightCutoff + radius_ * (1.0f / timeHorizonObst_) * new Vector2(-line.direction.y(), line.direction.x());
orcaLines_.Add(line);
return;
}
}
Vector3 toUnityVector2(RVO.Vector2 param)
{
return(new UnityEngine.Vector2(param.x(), param.y()));
}
Vector3 toUnityVector(RVO.Vector2 param)
{
return(new Vector3(param.x(), 0, param.y()));
}
/* Search for the best target velocity. */
public void computeTargetVelocity()
{
orcaLines_.Clear();
float invTimeHorizonObst = 1.0f / timeHorizonObst_;
int numObstLines = orcaLines_.Count;
float invTimeHorizon = 1.0f / timeHorizon_;
/* Create agent ORCA lines. */
for (int i = 0; i < agentNeighbors_.Count; ++i)
{
RVOAgent other = agentNeighbors_[i].Value;
Vector2 relativePosition = other.Position - Position;
Vector2 relativeVelocity = TargetVelocity - other.TargetVelocity;
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 = TargetVelocity + 0.5f * u;
orcaLines_.Add(line);
}
int lineFail = linearProgram2(orcaLines_, MaxSpeed, PreferredVelocity, false, ref TargetVelocity);
if (lineFail < orcaLines_.Count)
{
linearProgram3(orcaLines_, numObstLines, lineFail, MaxSpeed, ref TargetVelocity);
}
}
/**
* <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_);
}
}
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);
}
// 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);
}
}
Vector3 toUnityVector(RVO.Vector2 param)
{
return(new Vector3(param.x(), transform.position.y, param.y()));
}
void setAgentsProperties()
{
for (int i = 0; i < sim_.getNumAgents(); ++i)
{ // Set Agent Goal
Vector2 pos = sim_.getAgentPosition(i);
Vector2 goal = sim_.getAgentGoal(i);
// Position in the corridor referential
Vector2 local_pos = Vector2.rotation(pos, -corridor_angle_);
Vector2 local_goal = Vector2.rotation(goal, -corridor_angle_);
// Set agent goal
Vector2 new_goal = new Vector2(local_goal.x(), local_pos.y());
// Back to world's referential
new_goal = 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
Vector2 new_pos = new Vector2(local_pos.x() - (corridor_length_), local_pos.y());
// Back to world's referential
new_pos = 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();
Vector2 new_pos = new Vector2(pos.x_, rand.Next((int)corridor_width_ + 1));
// Back to world's referential
new_pos = 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
Vector2 new_pos = new Vector2(local_pos.x() + corridor_length_, local_pos.y());
// Back to world's referential
new_pos = 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 Vector3 RVOToVec3(this RVO.Vector2 v)
{
return(new Vector3(v.x(), 0, v.y()));
}
public static Vector3 XZ(this RVO.Vector2 _this)
{
return(new Vector3(_this.x(), 0, _this.y()));
}
public static float dot(Vector2 vector1, Vector2 vector2)
{
return(vector1.x() * vector2.x() + vector1.y() * vector2.y());
}
// 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;
}
}
}
/**
* <summary>Convert Vector2 to Vector3, Set Yaxis to zero.</summary>
*/
public static Vector3 Vec2ToVec3(Vector2 vec2)
{
return(new Vector3(vec2.x(), 0.0f, vec2.y()));
}