void GenerateObstacleVOs(VOBuffer vos)
{
var range = maxSpeed * obstacleTimeHorizon;
// Iterate through all obstacles that we might need to avoid
for (int i = 0; i < simulator.obstacles.Count; i++)
{
var obstacle = simulator.obstacles[i];
var vertex = obstacle;
// Iterate through all edges (defined by vertex and vertex.dir) in the obstacle
do
{
// Ignore the edge if the agent should not collide with it
if (vertex.ignore || (vertex.layer & collidesWith) == 0)
{
vertex = vertex.next;
continue;
}
// Start and end points of the current segment
float elevation1, elevation2;
var p1 = To2D(vertex.position, out elevation1);
var p2 = To2D(vertex.next.position, out elevation2);
Vector2 dir = (p2 - p1).normalized;
// Signed distance from the line (not segment, lines are infinite)
// TODO: Can be optimized
float dist = VO.SignedDistanceFromLine(p1, dir, position);
if (dist >= -0.01f && dist < range)
{
float factorAlongSegment = Vector2.Dot(position - p1, p2 - p1) / (p2 - p1).sqrMagnitude;
// Calculate the elevation (y) coordinate of the point on the segment closest to the agent
var segmentY = Mathf.Lerp(elevation1, elevation2, factorAlongSegment);
// Calculate distance from the segment (not line)
var sqrDistToSegment = (Vector2.Lerp(p1, p2, factorAlongSegment) - position).sqrMagnitude;
// Ignore the segment if it is too far away
// or the agent is too high up (or too far down) on the elevation axis (usually y axis) to avoid it.
// If the XY plane is used then all elevation checks are disabled
if (sqrDistToSegment < range * range && (simulator.movementPlane == MovementPlane.XY || (elevationCoordinate <= segmentY + vertex.height && elevationCoordinate + height >= segmentY)))
{
vos.Add(VO.SegmentObstacle(p2 - position, p1 - position, Vector2.zero, radius * 0.01f, 1f / ObstacleTimeHorizon, 1f / simulator.DeltaTime));
}
}
vertex = vertex.next;
} while (vertex != obstacle && vertex != null && vertex.next != null);
}
}
