new void linearProgram3(IList <Line> lines, int numObstLines, int beginLine, float radius, ref Vector2 result)
{
float distance = 0.0f;
for (int i = beginLine; i < lines.Count; ++i)
{
if ((float)Math.Round(Vector2.det(lines[i].direction, lines[i].point - result), 3) > distance)
{
/* Result does not satisfy constraint of line i. */
IList <Line> projLines = new List <Line>();
for (int k = 0; k < numObstLines; ++k)
{
projLines.Add(lines[k]);
}
for (int j = numObstLines; j < i; ++j)
{
Line line;
float determinant = Vector2.det(lines[i].direction, lines[j].direction);
if (Math.Abs(determinant) <= RVO_EPSILON)
{
/* Line i and line j are parallel. */
if (lines[i].direction * lines[j].direction > 0.0f)
{
/* Line i and line j point in the same direction. */
continue;
}
else
{
/* Line i and line j point in opposite direction. */
line.point = 0.5f * (lines[i].point + lines[j].point);
}
}
else
{
line.point = lines[i].point + (Vector2.det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
}
line.direction = Vector2.normalize(lines[j].direction - lines[i].direction);
projLines.Add(line);
}
Vector2 tempResult = result;
if (linearProgram2(projLines, radius, new Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, ref result) < projLines.Count)
{
/* This should in principle not happen. The result is by definition
* already in the feasible region of this linear program. If it fails,
* it is due to small floating point error, and the current result is
* kept.
*/
result = tempResult;
}
distance = Vector2.det(lines[i].direction, lines[i].point - result);
}
}
}
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 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;
}
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 int addObstacle(IList <Vector2> vertices)
{
if (vertices.Count < 2)
{
return(RVO_ERROR);
}
int obstacleNo = obstacles_.Count;
for (int i = 0; i < vertices.Count; ++i)
{
Obstacle obstacle = new Obstacle();
obstacle.point_ = vertices[i];
if (i > 0)
{
obstacle.previous_ = obstacles_[obstacleNo - 1];
obstacle.previous_.next_ = obstacle;
}
if (i == vertices.Count - 1)
{
obstacle.next_ = obstacles_[obstacleNo - 1];
obstacle.next_.previous_ = obstacle;
}
obstacle.direction_ = Vector2.normalize(vertices[(i == vertices.Count - 1 ? 0 : i + 1)] - vertices[i]);
if (vertices.Count == 2)
{
obstacle.convex_ = true;
}
else
{
obstacle.convex_ = (RVOMath.leftOf(vertices[(i == 0 ? vertices.Count - 1 : i - 1)], vertices[(i == vertices.Count - 1 ? 0 : i + 1)], vertices[i]) >= 0);
} // TO DO - Fix leftOf(dunno what is do)
obstacle.id_ = obstacles_.Count;
obstacles_.Add(obstacle);
obstacleNo = obstacles_.Count;
}
return(obstacleNo);
}
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_;
}
}
new int linearProgram2(IList <Line> lines, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
{
if (!directionOpt)
{
/*
* Optimize direction. Note that the optimization velocity is of unit
* length in this case.
*/
result = optVelocity * radius;
}
else if (Vector2.absSq(optVelocity) > Vector2.sqr(radius))
{
/* Optimize closest point and outside circle. */
result = Vector2.normalize(optVelocity) * radius;
}
else
{
/* Optimize closest point and inside circle. */
result = optVelocity;
}
for (int i = 0; i < lines.Count; ++i)
{
if (Vector2.det(lines[i].direction, lines[i].point - result) > 0.0f)
{
/* Result does not satisfy constraint i. Compute new optimal result. */
Vector2 tempResult = result;
if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result))
{
result = tempResult;
return(i);
}
}
}
return(lines.Count);
}
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;
}
}
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);
}
