void Evaluate()
{
duration -= Engine.timeStep;
if (duration <= 0) ResetStrategy();
bool targetIsGroup = false;
int j = -1; // id del agente o grupo con el ttc mas bajo
min_ttc = Mathf.Infinity;
foreach(var key in ttc.Keys) {
if (ttc[key] < min_ttc)
{
j = key;
min_ttc = ttc[key];
}
}
foreach (var key in group_ttc.Keys)
{
if (group_ttc[key] < min_ttc)
{
j = key;
min_ttc = group_ttc[key];
targetIsGroup = true;
}
}
if (j != -1 && min_ttc < timeHorizon)
{
if (!targetIsGroup) neighbor = Engine.Instance.GetAgent(j);
else neighbor = Engine.Instance.GetVirtualAgent(j);
ApplyStrategy();
}
else { velocity = Behaviours.GetSteering(position, goal, prefSpeed); }
}
public bool DoStep()
{
Vector2 dir = goal - position;
float distSqToGoal = dir.sqrMagnitude;
if (distSqToGoal <= goalRadius * goalRadius)
{
velocity = Vector2.zero;
return true;
}
velocity = Behaviours.GetSteering(position, goal, prefSpeed);
ttc.Clear();
DetectingNeighbors(neighborDist, dir, viewAngle);
DetectingNeighbors(personalSpace, -dir, 360 - viewAngle);
if (useGroups) DetectingGroups();
Evaluate();
// Limit velocity to prefSpeed of agent
if (velocity.sqrMagnitude > (prefSpeed * prefSpeed))
{
velocity.Normalize();
velocity *= prefSpeed;
}
position += velocity * Engine.timeStep;
UpdateDB();
MoveInRealWorld();
return false;
}
void DetermineStrategy(int[] s, bool frontal2 = false)
{
bool sameStrategy = false;
foreach (var ss in s)
{
if (curStrategy == (strategies)ss)
{
sameStrategy = true;
break;
}
}
if (!sameStrategy && System.Array.IndexOf(s, curStrategy) == -1)
{
ResetStrategy();
int i = Random.Range(0, s.Length);
curStrategy = (strategies)s[i];
if (debugLog) Debug.Log(id + " , The current strategy is: " + curStrategy);
}
switch (curStrategy)
{
case strategies.DCC:
velocity = Behaviours.GetSteering(position, goal, prefSpeed);
velocity = Behaviours.DecelerationStrategy(min_ttc, velocity);
break;
case strategies.CH:
int turnTo = TurnTo;
velocity = Behaviours.GetSteering(position, goal, prefSpeed);
velocity = Behaviours.ChangeDirectionStrategy((velocity,
neighbor.position - position, min_ttc, timeHorizon, neighbor.TurnTo, out turnTo, frontal2);
TurnTo = turnTo;
break;
case strategies.F:
velocity = Behaviours.GetSteering(position, goal, prefSpeed);
velocity = Behaviours.FollowStrategy(radius, prefSpeed, position,
velocity, neighbor.position, neighbor.velocity);
break;
case strategies.A:
velocity += Behaviours.CollisionAvoidance(position, velocity,
Behaviours.GetSteering(position, goal, prefSpeed), timeHorizon, ttc, group_ttc);
break;
case strategies.N:
velocity = Behaviours.GetSteering(position, goal, prefSpeed);
break;
}
if (debugLog) Debug.DrawRay(new Vector3(position.x, 1.5f, position.y),
new Vector3(velocity.x, 0, velocity.y), Color.green);
}
void RearCollision()
{
float bearingAngle = Behaviours.BearingAngle(velocity, neighbor.position - position);
int[] s;
// Front
if (bearingAngle <= 90 || bearingAngle > 270)
{
if (debugLog) DebugCollisionType(1);
s = new []{ 1, 2 };
if (neighbor.velocity.sqrMagnitude < 1 ) s = new int[] { 1 };
}
// Back
// (bearingAngle > 90 && bearingAngle <= 270)
else
{
if (debugLog) DebugCollisionType(2);
s = new[] { 1 };
}
DetermineStrategy(s);
}
// Deteccion de puntos extremos tangentes
static void GetTangents(List <int> group, Dictionary <int, float> ttc,
Vector2 position, Vector2 dir, float radius,
out Vector2 closestAgentPosition,
out Vector2 closestAgentVelocity,
out List <Point> tangentsPoints,
out List <Tuple <Point, Point> > points,
int povID, out int turnTo, out List <int> members)
{
float minimoTTC = Mathf.Infinity, minDst = MAX_DISTANCE;
Point agentPos = new Point(position);
Point outer1_p1, outer1_p2, outer2_p1, outer2_p2;
tangentsPoints = new List <Point>();
points = new List <Tuple <Point, Point> >();
members = new List <int>();
closestAgentPosition = Vector2.zero;
closestAgentVelocity = Vector2.zero;
turnTo = 0;
foreach (int id in group)
{
if (group.Contains(povID))
{
break;
}
if (!ttc.ContainsKey(id))
{
continue;
}
// Calculamos el agente mas cercano al observador
AAgent agent_tmp = Engine.Instance.GetAgent(id);
float dst = Vector2.Distance(position, agent_tmp.position);
turnTo += agent_tmp.TurnTo;
// Si este agente esta mas cerca de lo permitido entonces
// no es tomado en cuenta
if (dst < MIN_DISTANCE)
{
continue;
}
if (!Behaviours.ItsInFront(dir, agent_tmp.position - position))
{
continue;
}
if (dst < minDst)
{
closestAgentPosition = agent_tmp.position;
minDst = dst;
}
if (ttc[id] < minimoTTC)
{
closestAgentVelocity = agent_tmp.velocity;
minimoTTC = ttc[id];
}
members.Add(id);
// Find tangents points from this agent to actual iteration neighbour
Global.FindCircleCircleTangents(agentPos, radius,
new Point(agent_tmp.position), agent_tmp.radius,
out outer1_p1, out outer1_p2, out outer2_p1, out outer2_p2);
// agregamos el vector tangente en direccion del agente observador
dst = Point.Distance(outer1_p1, agentPos);
float dst_2 = Point.Distance(outer1_p2, agentPos);
if (dst < dst_2)
{
tangentsPoints.Add(outer1_p2);
points.Add(new Tuple <Point, Point>(outer1_p1, outer1_p2));
}
else
{
tangentsPoints.Add(outer1_p1);
points.Add(new Tuple <Point, Point>(outer1_p2, outer1_p1));
}
// agregamos el vector tangente en direccion del agente observador
dst = Point.Distance(outer2_p1, agentPos);
dst_2 = Point.Distance(outer2_p2, agentPos);
if (dst < dst_2)
{
tangentsPoints.Add(outer2_p2);
points.Add(new Tuple <Point, Point>(outer2_p1, outer2_p2));
}
else
{
tangentsPoints.Add(outer2_p1);
points.Add(new Tuple <Point, Point>(outer2_p2, outer2_p1));
}
}
}
