public Vector3[] GetCurvativePointsRight()
{
Vector3[] cps = new Vector3[leftPointsCurv.Length];
for (int i = 0; i < cps.Length; i++)
{
DiscreteTrackSegment seg = GetSegmentCurv(i);
cps[i] = seg.Pos1Right + (seg.Pos1Left - seg.Pos1Right) * minimumCurvatureTrajectory[i];
}
return(cps);
}
public DiscreteTrackSegment GetSegmentCurv(int index)
{
if (index >= 0 && index < leftPointsCurv.Length)
{
DiscreteTrackSegment segment = new DiscreteTrackSegment();
segment.Pos1Left = leftPointsCurv[index];
segment.Pos1Right = rightPointsCurv[index];
if (index == leftPointsCurv.Length - 1)
{
segment.Pos2Left = leftPointsCurv[0];
segment.Pos2Right = rightPointsCurv[0];
}
else
{
segment.Pos2Left = rightPointsCurv[index + 1];
segment.Pos2Right = rightPointsCurv[index + 1];
}
return(segment);
}
return(null);
}
