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); }