public NodeAIPath(SegmentAIPath source, SegmentAIPath destination, ILoftPath path)
{
this.source = source;
this.destination = destination;
this.path = path;
this.agents = new LinkedAgentChain();
}
private SegmentAIPath Create(AISegment segment, LaneDescription laneDescription, ref float offset, byte id)
{
var path = new SegmentAIPath(segment, laneDescription, offset + laneDescription.Width / 2, id);
offset += path.Lane.Width;
return(path);
}
private static Vector3 GetPoint(SegmentAIPath iPath, bool end)
{
var point = new Vector3(iPath.Reverse ^ end ? iPath.PathOffsetStart : iPath.PathOffsetEnd, 0, 0);
var p = iPath.Reverse ^ end ? iPath.LoftPath.Length : 0;
point = iPath.LoftPath.GetTransformedPoint(p, point);
return(point);
}
private ILoftPath createLineLoft(SegmentAIPath iPath, AISegmentNodeConnection from, SegmentAIPath oPath, AISegmentNodeConnection to)
{
var t1 = iPath.GetEndTransform();
var t2 = oPath.GetStartTransform();
var point1 = t1.MultiplyPoint3x4(Vector3.zero);
var point2 = t2.MultiplyPoint3x4(Vector3.zero);
var f1 = t1.MultiplyVector(Vector3.forward).normalized;
var f2 = t2.MultiplyVector(Vector3.forward).normalized;
// var l = (point2 - point1).magnitude;
//return new LinearPath(point2, point2 + f2 * l);
return(new BiArcLoftPath(point1, -f1, point2, -f2));
}
public SegmentAIRoute[] CreateRoutes(AISegment segment)
{
var lanes = segment.Description.Lanes;
var paths = new SegmentAIPath[lanes.Length];
float offset = -lanes.Sum(l => l.Width) / 2;
for (byte i = 0; i < paths.Length; i++)
{
var path = Create(segment, lanes[i], ref offset, i);
if (i > 0)
{
if (paths[i - 1].Reverse == path.Reverse)
{
SegmentAIPath.ConnectParralel(paths[i - 1], path);
}
}
paths[i] = path;
}
return(paths.GroupBy(t => t.Reverse).Select(t => new SegmentAIRoute(segment, t.ToArray(), t.Key)).ToArray());
}
public static AISegmentNodeConnection GetEnd(this SegmentAIPath path) => path.Reverse ? path.Segment.Start : path.Segment.End;
public static void ConnectParralel(SegmentAIPath left, SegmentAIPath right)
{
left.RightParralel = right;
right.LeftParralel = left;
}
