private void adjustPositionsOnBiggerWays(List <Pos> path, List <Vector3> walkPath, WayReference startWay, WayReference endWay)
{
WayReference currentWayReference = startWay;
for (int i = 0; i < path.Count; i++)
{
Pos currentPos = path [i];
if (i == path.Count - 1)
{
currentWayReference = endWay;
}
else if (i > 0)
{
Pos previousPos = path [i - 1];
if (currentPos.Id != -1L && previousPos.Id != -1L)
{
currentWayReference = NodeIndex.getWayReference(currentPos.Id, previousPos.Id);
}
}
// We now have the wayReference that our point should be offset on, if it's a way where cars normally drive
// if (currentWayReference.way.WayWidthFactor >= WayHelper.MINIMUM_DRIVE_WAY) {
Vector3 point = walkPath [i];
// TODO - Position on correct side of way
float offsetWayWidth = +(currentWayReference.transform.localScale.y / 2f) - 0.05f;
Vector3 humanOffsetOnWay = currentWayReference.transform.rotation * new Vector3(0f, offsetWayWidth, 0f);
walkPath[i] = new Vector3(point.x + humanOffsetOnWay.x, point.y + humanOffsetOnWay.y, point.z);
// }
}
}
public static List <DrivePath> Build(List <Vector3> path, List <Pos> posObjs)
{
// Debug.Log("NEW");
List <DrivePath> drivePaths = new List <DrivePath>();
Vector3 previousPoint = path[0];
WayReference previousWayReference = null;
float previousWidth = 0f;
float previousLength = 0f;
for (int i = 1; i < path.Count; i++)
{
bool hadPrevious = i > 1;
bool hasNext = i < path.Count - 1;
// DrivePath for current way
DrivePath currentPath = new DrivePath();
currentPath.startVector = path[i - 1];
currentPath.endVector = path[i];
currentPath.startId = posObjs[i - 1].Id;
currentPath.endId = posObjs[i].Id;
WayReference wayReference = NodeIndex.getWayReference(currentPath.startId, currentPath.endId);
float wayWidth = wayReference.transform.localScale.y;
float wayLength = (currentPath.endVector - currentPath.startVector).magnitude;
currentPath.setUpcomingTrafficLight(posObjs[i].Id, posObjs[i - 1].Id);
if (hadPrevious)
{
Vector3 originalStartVector = currentPath.startVector;
// If we had a previous path, we want to add an extra bezier from last way "half way width from the end" to current road "half way width from the start"
Vector3 endOfPrevious = currentPath.startVector;
if (previousWidth > previousLength)
{
endOfPrevious += (previousPoint - currentPath.startVector) / 2f;
}
else
{
float percentageOfPreviousLength = (previousWidth / 2f) / previousLength;
endOfPrevious += (previousPoint - currentPath.startVector) * percentageOfPreviousLength;
}
Vector3 startOfCurrent = currentPath.startVector;
if (wayWidth > wayLength)
{
startOfCurrent += (currentPath.endVector - currentPath.startVector) / 2f;
}
else
{
float percentageOfCurrentLength = (wayWidth / 2f) / wayLength;
startOfCurrent += (currentPath.endVector - currentPath.startVector) * percentageOfCurrentLength;
}
// Since this moves our start-point, reduce the "straight way" drive length
currentPath.startVector = startOfCurrent;
// Make the bezier
float previousRotation = Misc.GetZRotation(previousPoint, currentPath.startVector);
float currentRotation = Misc.GetZRotation(currentPath.startVector, currentPath.endVector);
float rotationDiff = currentRotation - previousRotation;
if (rotationDiff > 90f)
{
rotationDiff = 90f - (rotationDiff - 90f);
}
else if (rotationDiff < -90f)
{
rotationDiff = -90f - (rotationDiff + 90f);
}
float absRotationDiff = Mathf.Abs(rotationDiff);
// Debug.Log("PART");
// DebugFn.print(previousPoint);
// DebugFn.print(currentPath.startVector);
// DebugFn.print(currentPath.endVector);
// Debug.Log(previousRotation);
// Debug.Log(currentRotation);
// Debug.Log("Rotation: " + rotationDiff + " (" + absRotationDiff + ")");
float bezierResolution = BEZIER_MAX_RESOLUTION;
string blinkDirection = null;
if (absRotationDiff > DEGREES_TO_BLINK_THRESHOLD)
{
// drivePaths[drivePaths.Count - 1].blinkDirection = rotationDiff < 0 ? "right" : "left";
bool pointOnRightSideOfLine = Math3d.PointOnRightSideOfLine(endOfPrevious, path[i - 1], path[i]);
blinkDirection = pointOnRightSideOfLine ? "right" : "left";
drivePaths[drivePaths.Count - 1].blinkDirection = blinkDirection;
drivePaths[drivePaths.Count - 1].blinkStart = DISTANCE_TO_PREPARE_FOR_TURN;
// Debug.Log("Blink: " + drivePaths[drivePaths.Count - 1].blinkDirection);
}
else if (absRotationDiff < 1f)
{
bezierResolution = 2f;
}
else if (absRotationDiff < 5f)
{
bezierResolution = 4f;
}
else if (absRotationDiff < 10f)
{
bezierResolution = 6f;
}
else if (absRotationDiff < 20f)
{
bezierResolution = 12f;
}
float breakFactor = GetTurnBreakFactorForDegrees(absRotationDiff);
MakeBezier(drivePaths, endOfPrevious, originalStartVector - endOfPrevious, startOfCurrent, startOfCurrent - originalStartVector, currentPath.startId, breakFactor, wayReference.way.WayWidthFactor, bezierResolution, blinkDirection);
}
previousPoint = currentPath.startVector;
previousWayReference = wayReference;
previousWidth = wayWidth;
previousLength = wayLength;
if (hadPrevious && hasNext && wayLength < wayWidth)
{
// Special case, if way is too short, don't use the "mid" part of it, only the turn between previous and next way
continue;
}
if (hasNext)
{
float percentageOfCurrentLength = (wayWidth / 2f) / (currentPath.endVector - currentPath.startVector).magnitude;
currentPath.endVector -= (currentPath.endVector - currentPath.startVector) * percentageOfCurrentLength;
}
currentPath.wayWidthFactor = wayReference.way.WayWidthFactor;
currentPath.fullLength = (currentPath.endVector - currentPath.startVector).magnitude;
// Debug.Log(currentPath.fullLength);
currentPath.breakFactor = 1.0f;
currentPath.originalBreakFactor = currentPath.breakFactor;
// Put the path for the "mid" way
drivePaths.Add(currentPath);
}
// If needed, make them all z = 0
drivePaths.ForEach(dp => {
dp.startVector = new Vector3(dp.startVector.x, dp.startVector.y, 0f);
dp.endVector = new Vector3(dp.endVector.x, dp.endVector.y, 0f);
});
// All should be connected... if not, do that!
DrivePath previous = null;
for (int i = 0; i < drivePaths.Count; i++)
{
DrivePath current = drivePaths[i];
if (previous != null)
{
if (previous.endVector != current.startVector)
{
Vector3 midVector = Misc.GetMidVector(previous.endVector, current.startVector);
previous.endVector = midVector;
previous.fullLength = (previous.endVector - previous.startVector).magnitude;
current.startVector = midVector;
current.fullLength = (current.endVector - current.startVector).magnitude;
}
}
if (i < drivePaths.Count - 1 && current.breakFactor == 1f)
{
DrivePath next = drivePaths[i + 1];
if (next.breakFactor < 1f)
{
current.breakStart = DISTANCE_TO_PREPARE_FOR_TURN;
}
}
previous = current;
}
return(drivePaths);
}
