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 void unitTests()
{
// Test Compute tangents Points
setAgentDefaults(15.0f, 10, 1.0f, 10.0f, 0.5f, 2.0f, new Vector2());
addAgent(new Vector2(-1, 1), 0, true, true);
addAgent(new Vector2(0, 1), 0, true, true);
setAgentRadius(1, 1.0f);
addAgent(new Vector2(0, 0), 0, true, true);
addAgent(new Vector2(1, 2), 0, true, true);
addAgent(new Vector2(1, 0), 0, true, true);
GroupAgent group = new GroupAgent(this);
Pair <Vector2, Vector2> p1 = group.computeTangentsPoints(getAgent(0), getAgent(1));
Pair <Vector2, Vector2> p2 = group.computeTangentsPoints(getAgent(2), getAgent(1));
Pair <Vector2, Vector2> p3 = group.computeTangentsPoints(getAgent(3), getAgent(1));
Pair <Vector2, Vector2> p4 = group.computeTangentsPoints(getAgent(4), getAgent(1));
Pair <Vector2, Vector2> p5 = group.computeTangentsPoints(getAgent(1), getAgent(0));
Pair <Vector2, Vector2> p6 = group.computeTangentsPoints(getAgent(1), getAgent(2));
Pair <Vector2, Vector2> p7 = group.computeTangentsPoints(getAgent(1), getAgent(3));
Pair <Vector2, Vector2> p8 = group.computeTangentsPoints(getAgent(1), getAgent(4));
Console.Write("Compute Tangents Points\n");
Console.Write("T1" + p1.First + "Expected ~(-1,0.5) - T2" + p1.Second + "Expected ~(-1,1.5)\n");
Console.Write("T1" + p2.First + "Expected ~(0.5,0) - T2" + p2.Second + "Expected ~(-0.5,0)\n");
Console.Write("T1" + p3.First + "Expected ~(0.64,2.35) - T2" + p3.Second + "Expected ~(1.35,1.64)\n");
Console.Write("T1" + p4.First + "Expected ~(1.35,0.35) - T2" + p4.Second + "Expected ~(0.64,-0.35)\n\n");
Console.Write("T1" + p5.First + "Expected ~(0,1.5) - T2" + p5.Second + "Expected ~(0,0.5)\n");
Console.Write("T1" + p6.First + "Expected ~(-0.5,1) - T2" + p6.Second + "Expected ~(0.5,1)\n");
Console.Write("T1" + p7.First + "Expected ~(0.35,0.64) - T2" + p7.Second + "Expected ~(-0.35,1.35)\n");
Console.Write("T1" + p8.First + "Expected ~(-0.35,0.64) - T2" + p8.Second + "Expected ~(0.35,1.35)\n\n");
// Test detect groups
setAgentRadius(1, 0.5f);
addAgent(new Vector2(1, 0), 0, true, true);
setAgentPosition(0, new Vector2(0.0f, 3 * getDefaultRadius()));
setAgentPosition(1, new Vector2(0.0f, 0.0f));
setAgentPosition(2, new Vector2(-5 * getDefaultRadius(), 0.0f));
setAgentPosition(3, new Vector2(-5 * getDefaultRadius(), -3 * getDefaultRadius()));
setAgentPosition(4, new Vector2(3 * getDefaultRadius(), 0.0f));
setAgentPosition(5, new Vector2(6 * getDefaultRadius(), 0.0f));
setAgentVelocity(0, new Vector2(0.87f, 0.0f));
setAgentVelocity(1, new Vector2(1.0f, 0.0f));
setAgentVelocity(2, new Vector2(0.93f, 0.0f));
setAgentVelocity(3, Vector2.rotation(new Vector2(0.93f, 0.0f), (float)Math.PI / 6 + 0.1f));
setAgentVelocity(4, Vector2.rotation(new Vector2(0.93f, 0.0f), (float)Math.PI / 6 - 0.1f));
setAgentVelocity(5, Vector2.rotation(new Vector2(0.87f, 0.0f), 2 * ((float)Math.PI / 6 - 0.1f)));
detectGroups(false);
Console.Write("Detect Groups\n");
for (int i = 0; i < agents_.Count; i++)
{
Console.Write("Agent:" + i + " Address:" + agents_[i] + " group:" + agents_[i].groupBelongingTo_ + "\n");
}
GroupAgent group1 = agents_[1].groupBelongingTo_;
Console.Write("\n Group1 : Agents = ");
for (int i = 0; i < group1.agents_.Count; i++)
{
Console.Write(group1.agents_[i] + " ");
}
Console.Write("\n\n");
// Test Represent group
setAgentVelocity(0, new Vector2(1, 0));
detectGroups(false);
Console.Write("Detect Groups\n");
for (int i = 0; i < agents_.Count; i++)
{
Console.Write("Agent:" + i + " Address:" + agents_[i] + " group:" + agents_[i].groupBelongingTo_ + "\n");
}
group1 = agents_[1].groupBelongingTo_;
Console.Write("\n Group1 : Agents = ");
for (int i = 0; i < group1.agents_.Count; i++)
{
Console.Write(group1.agents_[i] + " ");
}
Console.Write("\n\n");
addAgent(new Vector2(3, 3), 0, true, true);
SuperAgent sa = group1.RepresentGroup(agents_.Last());
Console.Write("Represent group\n");
Console.Write("Position: " + sa.position_ + ". Velocity: " + sa.velocity_ + ". Radius: " + sa.radius_ + "\n");
setAgentPosition(agents_.Count - 1, new Vector2(2, 1.5f));
sa = group1.RepresentGroup(agents_.Last());
Console.Write("Position: " + sa.position_ + ". Velocity: " + sa.velocity_ + ". Radius: " + sa.radius_ + "\n");
setAgentPosition(agents_.Count - 1, new Vector2(0.75f, 0.75f));
sa = group1.RepresentGroup(agents_.Last());
Console.Write("Position: " + sa.position_ + ". Velocity: " + sa.velocity_ + ". Radius: " + sa.radius_ + "\n");
}
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);
}
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);
}
/**
* <summary>Computes the interaction between this agent and all the others agents.</summary>
*/
public void interactWithAgents()
{
IList <GroupAgent> groups = new List <GroupAgent>();
for (int i = 0; i < agentNeighbors_.Count; ++i)
{
Agent neighbor = agentNeighbors_[i].Value;
if (considerGroups_)
{
bool present = false;
GroupAgent agentGroup = neighbor.groupBelongingTo_;
// If the agent does not belong to any group or if it belongs to the same group, interact with it
if (agentGroup == null || agentGroup == groupBelongingTo_)
{
interactWith(neighbor);
}
// Otherwise, add its group to the list of the groups to interact with
else
{
foreach (GroupAgent g in groups)
{
if (agentGroup == g)
{
present = true;
}
}
// If the group has not been selected yet, add it to the group list
if (!present)
{
groups.Add(agentGroup);
}
}
}
else
{
interactWith(neighbor);
}
}
// Interact with each group of the list
for (int i = 0; i < groups.Count; ++i)
{
GroupAgent group = groups[i];
// If the group's representation succeded, interact with it
SuperAgent groupRepresentation = group.representGroup(this);
if (groupRepresentation.sim_ != null)
{
interactWith(groupRepresentation);
}
// Otherwise, interact with each agent of the group
else
{
IList <Agent> agentsOfTheGroup = group.getAgents();
foreach (Agent a in agentsOfTheGroup)
{
interactWith(a);
}
}
}
}
/** * <summary>Computes the interaction between this agent and the one in parameter.</summary> */ public abstract void interactWith(SuperAgent agent);
