Vector3 CalculateDesiredVelocity()
{
// final goal
Vector3 desiredVelocity = 2f * agent.Seek(targetPosition);
Vector3 avoidAgents = Vector3.zero;
foreach (Agent a in WorldState.GetInstance().agents)
{
//need to test function &might want to implement avaid for agents
if (a != agent)
{
avoidAgents += agent.Evade(a); // .transform.position) / Vector3.Distance(a.transform.position, transform.position);
}
}
Vector3 avoidObstacles = Vector3.zero;
Vector3 AvoidWallsVel = Vector3.zero;
Vector3 print = Vector3.zero;
foreach (Obstacle o in WorldState.GetInstance().obstacles)
{
desiredVelocity += agent.Flee(o.transform.position) / (Vector3.Distance(o.transform.position, transform.position) - 1);
for (int i = 0; i < o.mesh.vertexCount; i++)
{
if (i % 2 == 0)
{
avoidObstacles += agent.Flee(o.mesh.vertices[i] + Vector3.up) / (10 * Vector3.Distance(o.mesh.vertices[i] + Vector3.up, transform.position));
}
}
for (int i = 0; i < o.mesh.vertexCount / 2 - 1; i++)
{
avoidObstacles += agent.Flee((o.mesh.vertices[(i + 1) * 2] - o.mesh.vertices[i * 2]) + Vector3.up) /
(10 * Vector3.Distance((o.mesh.vertices[(i + 1) * 2] - o.mesh.vertices[i * 2]) + Vector3.up, transform.position));
}
for (int i = 0; i < walls.Length; i++)
{
print = agent.Flee(walls[i] / (20 * Vector3.Distance(walls[i], transform.position)));
print.y = 0;
//Debug.Log("print vector:" + print);
AvoidWallsVel -= print;
// Vector3.Distance(walls[i], transform.position)
}
}
return(desiredVelocity
+ 0.5f * avoidObstacles + 0.1f * AvoidWallsVel + 0.1f * avoidAgents);
}