public void deleteRoad(Road r)
{
removeRoad(r);
Node nstart, nend;
findNodeAt(r.curve.At(0f), out nstart);
findNodeAt(r.curve.At(1f), out nend);
Node[] affectedNides = { nstart, nend };
foreach (Node n in affectedNides.ToList())
{
if (n.connection.Count == 0)
{
Destroy(allnodes[Algebra.approximate(n.position)].gameObject);
allnodes.Remove(Algebra.approximate(n.position));
}
else
{
/*
* if (n.connection.Count == 2){
* if (Geometry.sameMotherCurveUponIntersect(n.connection[0].First.curve, n.connection[1].First.curve)){
* Road r1 = n.connection[0].First;
* Road r2 = n.connection[1].First;
* removeRoadWithoutChangingNodes(r1);
* removeRoadWithoutChangingNodes(r2);
* Destroy(allnodes[Algebra.approximate(n.position)].gameObject);
* allnodes.Remove(Algebra.approximate(n.position));
* Curve c2 = r1.curve.concat(r2.curve);
* addPureRoad(c2, r1.laneconfigure); //TODO: deal with different lane configure
* }
* }
*/
n.updateMargins();
foreach (var r1 in n.connection)
{
createRoadObject(r1);
}
}
}
}
public void generate(Road r)
{
generate(r.curve, r.laneconfigure, r.margin0LLength, r.margin0RLength, r.margin1LLength, r.margin1RLength);
}
Road generateVirtualRoad(int i1, int i2)
{
Road r1 = connection[i1];
Road r2 = connection[i2];
if (outLaneRange[i1, i2] == null)
{
return(null);
}
int loOutLaneNum = outLaneRange[i1, i2].First;
int hiOutLaneNum = outLaneRange[i1, i2].Second;
int loInLaneNum = inLaneRange[i2, i1].First;
int hiInLaneNum = inLaneRange[i2, i1].Second;
Debug.Assert(hiOutLaneNum - loOutLaneNum == hiInLaneNum - loInLaneNum);
float r1_margin = startof(r1.curve) ? r1.margin0Param : r1.margin1Param;
float r2_margin = startof(r2.curve) ? r2.margin0Param : r2.margin1Param;
float r1_radiOffset = 0.5f * (r1.getLaneCenterOffset(loOutLaneNum, !startof(r1.curve)) + r1.getLaneCenterOffset(hiOutLaneNum, !startof(r1.curve)));
float r2_radiOffset = 0.5f * (r2.getLaneCenterOffset(loInLaneNum, startof(r2.curve)) + r2.getLaneCenterOffset(hiInLaneNum, startof(r2.curve)));
Vector3 r1_endPos = r1.at(r1_margin) + r1.rightNormal(r1_margin) * r1_radiOffset;
Vector3 r2_endPos = r2.at(r2_margin) + r2.rightNormal(r2_margin) * r2_radiOffset;
List <string> virtualRoadLaneCfg = new List <string>();
int virtualRoadLaneCount = hiOutLaneNum - loOutLaneNum + 1;
for (int i = 0; i != virtualRoadLaneCount; ++i)
{
virtualRoadLaneCfg.Add("lane");
if (i != virtualRoadLaneCount - 1)
{
virtualRoadLaneCfg.Add("dash_white");
}
}
Vector2 r1_direction = startof(r1.curve) ? -r1.curve.direction_2d(r1_margin) : r1.curve.direction_2d(r1_margin);
Vector2 r2_direction = startof(r2.curve) ? -r2.curve.direction_2d(r2_margin) : r2.curve.direction_2d(r2_margin);
if (Geometry.Parallel(r1_direction, r2_direction))
{
/*TODO: perform a U turn when r1 = r2*/
if (Algebra.isRoadNodeClose(r1_endPos, r2_endPos))
{
/*exact same lane config for neighbors, just go straight*/
return(null);
}
//return new Road(Line.TryInit(r1_endPos, r2_endPos), virtualRoadLaneCfg, _noEntity: true);
return(createNoEntityRoadIfNotNull(Line.TryInit(r1_endPos, r2_endPos), virtualRoadLaneCfg));
}
else
{
Curve l1 = Line.TryInit(Algebra.toVector2(r1_endPos), Algebra.toVector2(r1_endPos) + Algebra.InfLength * r1_direction, r1_endPos.y, r1_endPos.y);
Curve l2 = Line.TryInit(Algebra.toVector2(r2_endPos), Algebra.toVector2(r2_endPos) + Algebra.InfLength * r2_direction, r2_endPos.y, r2_endPos.y);
List <Vector3> intereSectionPoint = Geometry.curveIntersect(l1, l2);
if (intereSectionPoint.Count == 1)
{
//return new Road(Bezeir.TryInit(r1_endPos, intereSectionPoint.First(), r2_endPos), virtualRoadLaneCfg, _noEntity: true);
return(createNoEntityRoadIfNotNull(Bezeir.TryInit(r1_endPos, intereSectionPoint.First(), r2_endPos), virtualRoadLaneCfg));
}
else
{
//return new Road(Line.TryInit(r1_endPos, r2_endPos), virtualRoadLaneCfg, _noEntity: true);
return(createNoEntityRoadIfNotNull(Line.TryInit(r1_endPos, r2_endPos), virtualRoadLaneCfg));
}
}
}
Pair <float, float> smoothenCrossing(Road r1, Road r2, out List <Curve> smootheners)
{
float r1_angle = startof(r1.curve) ? r1.curve.angle_ending(true) : r1.curve.angle_ending(false);
float r2_angle = startof(r2.curve) ? r2.curve.angle_ending(true) : r2.curve.angle_ending(false);
float delta_angle = r1_angle < r2_angle ? r2_angle - r1_angle : r2_angle + 2 * Mathf.PI - r1_angle;
this.r1 = r1;
this.r2 = r2;
smootheners = new List <Curve>();
Vector2 streetCorner = approxStreetCorner();
//debugPoints.Add(Algebra.toVector3(streetCorner));
switch (Geometry.getAngleType(delta_angle))
{
case angleType.Sharp:
case angleType.Blunt:
if (c1_offset > 0f && c2_offset > 0f)
{
/*c1,c2>0*/
float extraSmoothingLength = arcSmoothingRadius / Mathf.Tan(delta_angle / 2);
addIfNotNull(smootheners, Arc.TryInit(r1.curve.at_ending_2d(startof(r1.curve), c1_offset + extraSmoothingLength) +
Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve), c1_offset + extraSmoothingLength) + Mathf.PI / 2) * r1.width / 2,
Mathf.PI - delta_angle,
r2.curve.at_ending_2d(startof(r2.curve), c2_offset + extraSmoothingLength) +
Algebra.angle2dir(r2.curve.angle_ending(startof(r2.curve), c2_offset + extraSmoothingLength) - Mathf.PI / 2) * r2.width / 2));
return(new Pair <float, float>(c1_offset + extraSmoothingLength, c2_offset + extraSmoothingLength));
}
if (c1_offset > 0f)
{
/*c1>0, c2<=0*/
float smoothRadius = -c2_offset;
addIfNotNull(smootheners, Arc.TryInit(r1.curve.at_ending_2d(startof(r1.curve), c1_offset + smoothRadius) +
Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve), c1_offset + smoothRadius) + Mathf.PI / 2) * r1.width / 2,
Mathf.PI - delta_angle,
r2.curve.at_ending_2d(startof(r2.curve)) +
Algebra.angle2dir(r2.curve.angle_ending(startof(r2.curve)) - Mathf.PI / 2) * r2.width / 2));
/*TODO: calculate more precise delta_angle*/
return(new Pair <float, float>(c1_offset + smoothRadius, 0));
}
if (c2_offset > 0f)
{
/*c1<0, c2>0*/
float smoothRadius = -c1_offset;
Curve smoothener = Arc.TryInit(r1.curve.at_ending_2d(startof(r1.curve)) +
Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve)) + Mathf.PI / 2) * r1.width / 2,
Mathf.PI - delta_angle,
r2.curve.at_ending_2d(startof(r2.curve), c2_offset + smoothRadius) +
Algebra.angle2dir(r2.curve.angle_ending(startof(r2.curve), c2_offset + smoothRadius) - Mathf.PI / 2) * r2.width / 2);
addIfNotNull(smootheners, smoothener);
return(new Pair <float, float>(0, c2_offset + smoothRadius));
}
Debug.Assert(false);
break;
case angleType.Flat:
if (r1.width == r2.width)
{
return(new Pair <float, float>(0, 0));
}
float widthDiff = Math.Abs(r1.width - r2.width) / 2;
if (r1.width > r2.width)
{
Vector2 P0 = r1.curve.at_ending_2d(startof(r1.curve)) +
Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve)) + Mathf.PI / 2) * r1.width / 2;
Vector2 P1 = r2.curve.at_ending_2d(startof(r2.curve), widthDiff * bezeirSmoothingScale * 0.25f) +
Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve), widthDiff * bezeirSmoothingScale * 0.25f) + Mathf.PI / 2) * r1.width / 2;
Vector2 P4 = r2.curve.at_ending_2d(startof(r2.curve), widthDiff * bezeirSmoothingScale) +
Algebra.angle2dir(r2.curve.angle_ending(startof(r2.curve), widthDiff * bezeirSmoothingScale) - Mathf.PI / 2) * r2.width / 2;
Vector2 P3 = r2.curve.at_ending_2d(startof(r2.curve), widthDiff * bezeirSmoothingScale * 0.75f) +
Algebra.angle2dir(r2.curve.angle_ending(startof(r2.curve), widthDiff * bezeirSmoothingScale * 0.75f) - Mathf.PI / 2) * r2.width / 2;
Vector2 P2 = (P1 + P3) / 2;
addIfNotNull(smootheners, Bezeir.TryInit(P0, P1, P2));
addIfNotNull(smootheners, Bezeir.TryInit(P2, P3, P4));
return(new Pair <float, float>(0f, widthDiff * bezeirSmoothingScale));
}
else
{
Vector2 P0 = r1.curve.at_ending_2d(startof(r1.curve), widthDiff * bezeirSmoothingScale)
+ Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve), widthDiff * bezeirSmoothingScale) + Mathf.PI / 2) * r1.width / 2;
Vector2 P1 = r1.curve.at_ending_2d(startof(r1.curve), widthDiff * bezeirSmoothingScale * 0.75f)
+ Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve), widthDiff * bezeirSmoothingScale * 0.75f) + Mathf.PI / 2) * r1.width / 2;
Vector2 P3 = r1.curve.at_ending_2d(startof(r1.curve), widthDiff * bezeirSmoothingScale * 0.25f)
+ Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve), widthDiff * bezeirSmoothingScale * 0.25f) + Mathf.PI / 2) * r2.width / 2;
Vector2 P4 = r2.curve.at_ending_2d(startof(r2.curve)) +
Algebra.angle2dir(r2.curve.angle_ending(startof(r2.curve)) - Mathf.PI / 2) * r2.width / 2;
Vector2 P2 = (P1 + P3) / 2;
addIfNotNull(smootheners, Bezeir.TryInit(P0, P1, P2));
addIfNotNull(smootheners, Bezeir.TryInit(P2, P3, P4));
return(new Pair <float, float>(widthDiff * bezeirSmoothingScale, 0f));
}
case angleType.Reflex:
float arcRadius = Mathf.Max(r1.width / 2, r2.width / 2);
float bWidthDiff = Mathf.Abs(r1.width - r2.width) / 2;
Curve arcSmoothener = Arc.TryInit(twodPosition,
twodPosition + Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve)) + Mathf.PI / 2) * arcRadius,
delta_angle - Mathf.PI);
if (r1.width == r2.width)
{
addIfNotNull(smootheners, arcSmoothener);
return(new Pair <float, float>(0f, 0f));
}
if (r1.width > r2.width)
{
Vector2 P0 = r2.curve.at_ending_2d(startof(r2.curve)) +
Algebra.angle2dir(r2.curve.angle_ending(startof(r2.curve)) - Mathf.PI / 2) * r1.width / 2;
Vector2 P1 = r2.curve.at_ending_2d(startof(r2.curve), bWidthDiff * bezeirSmoothingScale * 0.25f) +
Algebra.angle2dir(r2.curve.angle_ending(startof(r2.curve), bWidthDiff * bezeirSmoothingScale * 0.25f) - Mathf.PI / 2) * r1.width / 2;
Vector2 P4 = r2.curve.at_ending_2d(startof(r2.curve), bWidthDiff * bezeirSmoothingScale)
+ Algebra.angle2dir(r2.curve.angle_ending(startof(r2.curve), bWidthDiff * bezeirSmoothingScale) - Mathf.PI / 2) * r2.width / 2;
Vector2 P3 = r2.curve.at_ending_2d(startof(r2.curve), bWidthDiff * bezeirSmoothingScale * 0.75f)
+ Algebra.angle2dir(r2.curve.angle_ending(startof(r2.curve), bWidthDiff * bezeirSmoothingScale * 0.75f) - Mathf.PI / 2) * r2.width / 2;
Vector2 P2 = (P1 + P3) / 2;
addIfNotNull(smootheners, arcSmoothener);
addIfNotNull(smootheners, Bezeir.TryInit(P0, P1, P2));
addIfNotNull(smootheners, Bezeir.TryInit(P2, P3, P4));
return(new Pair <float, float>(0f, bWidthDiff * bezeirSmoothingScale));
}
else
{
Vector2 P0 = r1.curve.at_ending_2d(startof(r1.curve), bWidthDiff * bezeirSmoothingScale)
+ Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve), bWidthDiff * bezeirSmoothingScale) + Mathf.PI / 2) * r1.width / 2;
Vector2 P1 = r1.curve.at_ending_2d(startof(r1.curve), bWidthDiff * bezeirSmoothingScale * 0.75f)
+ Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve), bWidthDiff * bezeirSmoothingScale * 0.75f) + Mathf.PI / 2) * r1.width / 2;
Vector2 P3 = r1.curve.at_ending_2d(startof(r1.curve), bWidthDiff * bezeirSmoothingScale * 0.25f)
+ Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve), bWidthDiff * bezeirSmoothingScale * 0.25f) + Mathf.PI / 2) * r2.width / 2;
Vector2 P4 = r1.curve.at_ending_2d(startof(r1.curve))
+ Algebra.angle2dir(r1.curve.angle_ending(startof(r1.curve)) + Mathf.PI / 2) * r2.width / 2;
Vector2 P2 = (P1 + P3) / 2;
addIfNotNull(smootheners, Bezeir.TryInit(P0, P1, P2));
addIfNotNull(smootheners, Bezeir.TryInit(P2, P3, P4));
addIfNotNull(smootheners, arcSmoothener);
return(new Pair <float, float>(bWidthDiff * bezeirSmoothingScale, 0f));
}
default:
return(new Pair <float, float>(-1f, -1f));
}
return(new Pair <float, float>(-1f, -1f));
}
/*
* public Pair<float, float> getMargin(Road r){
* var conn = connection.Find((Pair<Road, ConnectionInfo> obj) => obj.First == r);
* if (!startof(r.curve))
* {
* return conn.Second.margins;
* }
* else
* {
* return new Pair<float, float>(conn.Second.margins.Second, conn.Second.margins.First);
* }
* }
*/
public void addRoad(Road road)
{
Debug.Assert(position.y == Mathf.NegativeInfinity || Algebra.isclose(road.curve.at_ending(startof(road.curve)).y, position.y));
connection.Add(road);
}
// Update is called once per frame
void Update()
{
if (pathOn != null)
{
float distToTravel;
if (acceleration < 0f && speed < (-acceleration) * Time.deltaTime)
{
distToTravel = speed * speed / (2 * (-acceleration));
}
else
{
distToTravel = speed * Time.deltaTime + 0.5f * acceleration * Time.deltaTime * Time.deltaTime;
}
speed += Time.deltaTime * acceleration;
speed = Mathf.Max(0f, speed);
distTraveledOnSeg += distToTravel;
bool termination;
int nextSeg, nextLane;
Pair <Road, float> nextInfo = pathOn.travelAlong(currentSeg, currentParam, distToTravel, laneOn, out nextSeg, out nextLane, out termination);
if (termination)
{
VhCtrlOfCurrentSeg.VehicleLeave(this, laneOn);
stopEvent.Invoke();
Reset();
return;
}
Road roadOn = nextInfo.First;
currentParam = nextInfo.Second;
if (currentSeg != nextSeg)
{
VhCtrlOfCurrentSeg.VehicleLeave(this, laneOn);
distTraveledOnSeg = distToTravel;
laneOn = nextLane;
currentSeg = nextSeg;
VhCtrlOfCurrentSeg.VehicleEnter(this, laneOn);
}
if (!Algebra.isclose(rightOffset, 0f))
{
float lateralAcc = (lateralSpeedMagnitude * lateralSpeedMagnitude > 2 * lateralMaxAcc * Mathf.Abs(rightOffset)) ?
-0.98f * lateralMaxAcc : lateralMaxAcc;
lateralSpeedMagnitude = Mathf.Max(lateralSpeedMagnitude + lateralAcc * Time.deltaTime, 0f);
rightOffset = Mathf.Sign(rightOffset) * Mathf.Max(Mathf.Abs(rightOffset) - lateralSpeedMagnitude * Time.deltaTime, 0f);
}
else
{
lateralSpeedMagnitude = 0f;
}
transform.position = roadOn.at(currentParam, usebuff: true) +
roadOn.rightNormal(currentParam, usebuff: true) * (roadOn.getLaneCenterOffset(laneOn, headingOfCurrentSeg) + rightOffset);
transform.rotation = headingOfCurrentSeg ?
Quaternion.LookRotation(roadOn.frontNormal(currentParam, usebuff: true), roadOn.upNormal(currentParam, usebuff: true)) :
Quaternion.LookRotation(-roadOn.frontNormal(currentParam, usebuff: true), roadOn.upNormal(currentParam, usebuff: true));
if (rightOffset != 0f)
{
if (headingOfCurrentSeg)
{
transform.Rotate(roadOn.upNormal(currentParam, usebuff: true), -Mathf.Sign(rightOffset) * Mathf.Atan(lateralSpeedMagnitude / Mathf.Max(speed, 0.2f)) * Mathf.Rad2Deg);
}
else
{
transform.Rotate(roadOn.upNormal(currentParam, usebuff: true), Mathf.Sign(rightOffset) * Mathf.Atan(lateralSpeedMagnitude / Mathf.Max(speed, 0.2f)) * Mathf.Rad2Deg);
}
}
wheelRotation = (wheelRotation + distToTravel / wheeRadius * Mathf.Rad2Deg) % 360;
/*TODO: calculate wheel radius*/
transform.GetChild(0).GetChild(1).localRotation = transform.GetChild(0).GetChild(2).localRotation =
transform.GetChild(0).GetChild(3).localRotation = transform.GetChild(0).GetChild(4).localRotation =
Quaternion.Euler(wheelRotation, 0f, 0f);
}
}
/*TODO: improve efficiency*/
public Path findPath(Road r1, float param1, Road r2, float param2)
{
if (r1 == r2)
{
/*Special trivial case*/
return(new Path(r1, param1, param2));
}
List <Node> possibleStartNodes = new List <Node>();
List <Node> possibleEndNodes = new List <Node>();
if (r1.directionalLaneCount(false) > 0)
{
Node r10;
findNodeAt(r1.curve.At(0f), out r10);
possibleStartNodes.Add(r10);
}
if (r1.directionalLaneCount(true) > 0)
{
Node r11;
findNodeAt(r1.curve.At(1f), out r11);
possibleStartNodes.Add(r11);
}
if (r2.directionalLaneCount(true) > 0)
{
Node r20;
findNodeAt(r2.curve.At(0f), out r20);
possibleEndNodes.Add(r20);
}
if (r2.directionalLaneCount(false) > 0)
{
Node r21;
findNodeAt(r2.curve.At(1f), out r21);
possibleEndNodes.Add(r21);
}
if (possibleStartNodes.Count * possibleEndNodes.Count == 0)
{
return(null);
}
List <Path> candicatePaths = new List <Path>();
foreach (Node nstart in possibleStartNodes)
{
foreach (Node nend in possibleEndNodes)
{
Path p = Node2NodeSP(nstart, nend);
if (p != null)
{
p.insertAtStart(r1, param1);
p.insertAtEnd(r2, param2);
candicatePaths.Add(p);
}
}
}
if (candicatePaths.Count > 0)
{
return(candicatePaths.MinBy((arg) => arg.length));
}
else
{
return(null);
}
}
/* For non-virtual(existing road), should consider height diff
* For virtual road, always flatten
*/
public Vector3 approxNodeToExistingRoad(Vector3 p, out Road match, List <Curve> additionalInterestedLines = null)
{
List <Road> allInterestedRoad;
if (additionalInterestedLines != null)
{
allInterestedRoad = new List <Road>();
allInterestedRoad.AddRange(allroads);
allInterestedRoad.AddRange(additionalInterestedLines.ConvertAll <Road>((Curve input) => new Road(input, null)));
}
else
{
allInterestedRoad = allroads;
}
//Debug.Assert(additionalInterestedLines == null || additionalInterestedLines.All((arg1) => arg1 is Line));
List <Road> candidates = allInterestedRoad.FindAll(r => r.virtualRoad ?
(Algebra.toVector2(r.curve.AttouchPoint(p)) - Algebra.toVector2(p)).magnitude <= ApproxLimit:
(r.curve.AttouchPoint(p) - p).magnitude <= ApproxLimit);
List <Road> onlyRoadCandidates = candidates.FindAll((obj) => !obj.virtualRoad);
if (onlyRoadCandidates.Count > 0)
{
Road bestMatch = onlyRoadCandidates.OrderBy(r => (r.curve.AttouchPoint(p) - p).magnitude).First();
match = bestMatch;
foreach (Road others in candidates)
{
if (others != bestMatch)
{
List <Vector3> interPoints;
if (others.virtualRoad)
{
Curve othersCurve = others.curve.deepCopy().flattened();
Curve bestmatchCurve = bestMatch.curve.deepCopy().flattened();
interPoints = Geometry.curveIntersect(othersCurve, bestmatchCurve);
foreach (Vector3 point in interPoints)
{
if (Algebra.toVector2(point - p).magnitude <= ApproxLimit)
{
return(bestMatch.curve.At((float)bestMatch.curve.paramOf(point)));
}
}
}
else
{
interPoints = Geometry.curveIntersect(bestMatch.curve, others.curve);
foreach (Vector3 point in interPoints)
{
if ((point - p).magnitude <= ApproxLimit)
{
return(point);
}
}
}
}
}
return(bestMatch.curve.AttouchPoint(p));
}
if (candidates.Count > 0)
{
match = null;
Vector3 flattened = candidates[0].curve.AttouchPoint(p);
return(new Vector3(flattened.x, p.y, flattened.z));
}
match = null;
return(p);
}
