private Vector3 getOutgoingVectorForOneWayIntersection(Intersection intersection, Road connectedRoad)
{
float roadEndWidth = connectedRoad.GetRoadWidth();
float roadEndLength = roadEndWidth / 2;
return(IntersectionMath.getOutgoingVector(intersection, connectedRoad).normalized *roadEndWidth / 2);
}
// private string infoString
// {
// get
//{
// return "Volume : " + volume + " Surface Area : " + surfaceArea + " Perimeter : " + perimeter + " CrossSection : " + crossSectionArea;
// }
// }
private Vector3[] PyramidPlaneIntersect(float planeHeight)
{
//find an intersection in a parallel plane.
Vector3[] vertices = new Vector3[myPyramid.basePolygon.pointList.Count];
Vector3 planePos = Vector3.up * planeHeight;
Vector3 planeNorm = Vector3.up;
foreach (InteractableLineSegment line in myPyramid.allEdges)
{
if (line.point1 == myPyramid.apex)
{
int idx = myPyramid.basePolygon.pointList.IndexOf(line.point2);
//LinePlane intersection uses LinesPos and LineDir.
//vertices[idx] = IntersectionMath.LinePlaneIntersection(line.vertex0, line.vertex1-line.vertex0, planePos,planeNorm).vectordata[0];
//SegmentPlaneIntersection uses LinePos1 and LinePos2
vertices[idx] = IntersectionMath.SegmentPlaneIntersection(line.vertex0, line.vertex1, planePos, planeNorm).vectordata[0];
}
else if (line.point2 == myPyramid.apex)
{
int idx = myPyramid.basePolygon.pointList.IndexOf(line.point1);
//vertices[idx] = IntersectionMath.LinePlaneIntersection(line.vertex0, line.vertex1-line.vertex0, planePos, planeNorm).vectordata[0];
vertices[idx] = IntersectionMath.SegmentPlaneIntersection(line.vertex0, line.vertex1, planePos, planeNorm).vectordata[0];
}
}
return(vertices);
}
private Vector3 CoordinateForRoadWithAbstractRoadInfos(Intersection intersection, Road connectedRoad, RightOrLeft rtlt,
AbstractWayInfo[] infos)
{
infos = AbstractWayInfo.sortAbstractWayInfos(infos);
// convert the road to an abstract way info
AbstractWayInfo info = new AbstractWayInfo(IntersectionMath.getOutgoingVector(intersection, connectedRoad).normalized,
connectedRoad.GetRoadWidth());
// TODO : CHECK FOR nonexistent index
int index = 0;
for (int i = 0; i < infos.Length; i++)
{
if (infos[i] == info)
{
index = i;
}
}
Vector3[] vertices = MeshVerticesForAbstractWayInfos(infos);
switch (rtlt)
{
case RightOrLeft.LEFT:
return(vertices[index - 1 < 0 ? infos.Length - 1 : index - 1]);
case RightOrLeft.RIGHT:
return(vertices[index]);
}
Debug.LogError("Semantics Error, please check");
return(Vector3.zero);
}
private static bool IsLineVisible(LineHL line, double nearPlaneDistance, TriangleHL[] triangles)
{
var rayAxis = GetRaytracingRay(line, nearPlaneDistance);
var(success, edgeIntersection) = GetRayIntersectionWithEdge(rayAxis, line.Edge);
if (!success)
{
return(true);
}
var rayToEdgeDirection = (edgeIntersection - rayAxis.Offset);
var edgeDistanceSquared = rayToEdgeDirection.SquaredLength();
foreach (var triangle in triangles)
{
if (!IsTriangleNeighbourOfLine(triangle, line))
{
if (!IsLineInTrianglePlane(line, triangle))
{
var(hasIntersection, triangleDistanceSquared) = IntersectionMath.GetSquaredDistanceOfIntersectionOfRayAndTriangle(rayAxis.Offset, rayToEdgeDirection, triangle.P1, triangle.P2, triangle.P3);
if (hasIntersection && triangleDistanceSquared < edgeDistanceSquared)
{
return(false);
}
}
}
}
return(true);
}
public void Check2DLineWithLineTest1()
{
var(hasIntersection, ix, iy) = IntersectionMath.Check2DLineWithLine(0, 0, 10, 10, 0, 10, 10, 0);
Assert.True(hasIntersection);
Assert.Equal(5.0, ix, 2);
Assert.Equal(5.0, iy, 2);
}
public static (bool, Position3D, Body) GetIntersectionOfRayAndScene(this Scene scene, Position3D rayOffset, Vector3D rayDirection)
{
var triangles = GetSceneTriangles(scene);
var minIntersectionResult
= triangles.Select(triangle => (triangle.Item1, IntersectionMath.GetIntersectionAndSquaredDistanceOfRayAndTriangle(rayOffset, rayDirection, triangle.Item2, triangle.Item3, triangle.Item4)))
.Where(result => result.Item2.Item1)
.AsParallel()
.Aggregate(((Body)null, (false, new Position3D(), double.MaxValue)), (acc, x) => x.Item2.Item3 < acc.Item2.Item3 ? x : acc);
return(minIntersectionResult.Item2.Item1, minIntersectionResult.Item2.Item2, minIntersectionResult.Item1);
}
public void Check2DLineWithLineTest5()
{
var x1 = 0.0079377614887504739;
var y1 = -0.093839459278913212;
var x2 = 0.0651634776216613;
var y2 = -0.046422111432387225;
var x3 = 0.0651634776216613;
var y3 = -0.046422111432387225;
var x4 = 0.0079377614887504687;
var y4 = -0.0938394592789132;
var(hasIntersection, ix, iy) = IntersectionMath.Check2DLineWithLine(x1, y1, x2, y2, x3, y3, x4, y4);
Assert.False(hasIntersection);
}
private GameObject buildTwoWayIntersection(Intersection intersection, Road road1, Road road2)
{
// create a four way intersection instead of a two way intersection if the angle is less than 90
switch (GetTwoWayIntersectionType(intersection))
{
case TwoWayIntersectionType.SKEW:
case TwoWayIntersectionType.TRANSITION:
AbstractWayInfo[] infos = AbstractWayInfosForTwoWayIntersection(intersection, road1, road2);
return(buildMultiwayIntersectionWithVectors(intersection, infos));
case TwoWayIntersectionType.SMOOTH:
// only when angle > 90
Vector3 vec1 = IntersectionMath.getOutgoingVector(intersection, road1).normalized;
Vector3 vec2 = IntersectionMath.getOutgoingVector(intersection, road2).normalized;
float roadWidth1 = road1.GetRoadWidth();
float roadWidth2 = road2.GetRoadWidth();
Vector3 vec1Norm = Quaternion.Euler(new Vector3(0, 90, 0)) * vec1 * roadWidth1 / 2;
Vector3 vec2Norm = Quaternion.Euler(new Vector3(0, -90, 0)) * vec2 * roadWidth2 / 2;
// check the intersection, if there's its inner, if there isn't, its outer
Vector3 refPoint;
GameObject inner;
GameObject outer;
if (Math3d.LineLineIntersection(out refPoint, vec1Norm, vec1, vec2Norm,
vec2))
{
// we intersected, its inner
inner = buildInner(vec1Norm, vec2Norm, refPoint, intersection.position, intersection.customization);
outer = buildOuter(-vec1Norm, -vec2Norm, 4, intersection.position, intersection.customization);
}
else
{
// we are outer
outer = buildOuter(vec1Norm, vec2Norm, 4, intersection.position, intersection.customization);
// get the real intersection
// we need to * a const since the Math scirpt only considers line to be that long
inner = buildInner(-vec1Norm, -vec2Norm, refPoint, intersection.position, intersection.customization);
}
// add rigidbody to enable selection
return(createParentIntersectionWithChildIntersections("Two way intersection", intersection.customization, inner,
outer));
}
Debug.LogError("Semantics Error, please check");
return(null);
}
// log this
public Vector3[] findCrossSectionSegment()
{
if (IntersectionMath.SegmentPlaneIntersection(l1.vertex0, l1.vertex1, Vector3.up * crossSectionHeight, Vector3.up).figtype == GeoObjType.point && IntersectionMath.SegmentPlaneIntersection(l2.vertex0, l2.vertex1, Vector3.up * crossSectionHeight, Vector3.up).figtype == GeoObjType.point)
{
Vector3 vertex0 = IntersectionMath.SegmentPlaneIntersection(l1.vertex0, l1.vertex1, Vector3.up * crossSectionHeight, Vector3.up).vectordata[0];
Vector3 vertex1 = IntersectionMath.SegmentPlaneIntersection(l2.vertex0, l2.vertex1, Vector3.up * crossSectionHeight, Vector3.up).vectordata[0];
return(new Vector3[] { vertex0, vertex1 });
}
else
{
//cross section height is erroring.
Vector3 vertex0 = IntersectionMath.SegmentPlaneIntersection(l1.vertex0, l1.vertex1, Vector3.up * (height1 + .00001f), Vector3.up).vectordata[0];
Vector3 vertex1 = IntersectionMath.SegmentPlaneIntersection(l2.vertex0, l2.vertex1, Vector3.up * (height1 + .00001f), Vector3.up).vectordata[0];
return(new Vector3[] { vertex0, vertex1 });
}
}
private AbstractWayInfo[] AbstractWayInfosForMultiwayIntersection(Intersection intersection, Road[] connectedRoads)
{
// convert roads to vectos
Vector3[] vectors = connectedRoads.Select(rd => IntersectionMath.getOutgoingVector(intersection, rd)).ToArray();
float[] widths = connectedRoads.Select(rd => rd.GetRoadWidth()).ToArray();
AbstractWayInfo[] infos;
// handle when connectedRoads.Length == 3 << build a four-way intersection instead
if (connectedRoads.Length == 3)
{
// find the vector whose direction is of greatest difference to other two
Vector3 maxVector = vectors[0];
float maxRoadWidth = widths[0];
float maxAngles = 0;
for (int i = 0; i < 3; i++)
{
float accum = 0;
for (int j = 0; j < 3; j++)
{
float zeroTo180Angle = Vector3.Angle(vectors[i], vectors[j]);
float distance = zeroTo180Angle > 90 ? 180 - zeroTo180Angle : zeroTo180Angle;
accum += distance;
}
if (accum > maxAngles)
{
maxVector = vectors[i];
maxRoadWidth = widths[i];
maxAngles = accum;
}
}
Vector3[] resultingVectors = { vectors[0], vectors[1], vectors[2], -maxVector };
float[] resultingWidths = { widths[0], widths[1], widths[2], maxRoadWidth };
infos = AbstractWayInfo.InfosWithDirectionsAndWidths(resultingVectors, resultingWidths);
}
else
{
infos = AbstractWayInfo.InfosWithDirectionsAndWidths(vectors, widths);
}
return(infos);
}
// returns the maximum value of AbstractCarPosition.offset so that the car is not rushing into the intersection
public float stoppingDistanceForCurrentDrive(AbstractCarPosition carPosition)
{
/**
*
* toIntersection ref intersection
*
* toLeftEdge
* \===================================================| ^
* \|< STOPPING DISTANCE > | width(refIntersection)
* \ ------------------------------------------------| x
* \ | width(toIntersection)
* \===============================================| v
* toRightEdge
*
*
*
*/
Intersection toIntersection = carPosition.referenceRoad.otherIntersection(carPosition.referenceIntersection);
Vector3 toLeftEdge = builder.coordinateForRoadAtIntersection(toIntersection, carPosition.referenceRoad,
IntersectionBuilder.RightOrLeft.LEFT);
Vector3 toRightEdge = builder.coordinateForRoadAtIntersection(toIntersection, carPosition.referenceRoad,
IntersectionBuilder.RightOrLeft.RIGHT);
// get the proportion
int totalNumberOfLanes =
carPosition.referenceRoad.GetNumberOfLanesInDirectionWithReferenceIntersection(toIntersection)
+ carPosition.referenceRoad.GetNumberOfLanesInDirectionWithReferenceIntersection(carPosition
.referenceIntersection);
Vector3 stoppingLocation = Vector3.Lerp(toLeftEdge, toRightEdge,
1f / 2 - (1f / 2 + carPosition.laneNumber) / totalNumberOfLanes) * (1f / 2);
// project the stopping location onto the outgoing vector
Vector3 stoppingCompensation = Vector3.Project(stoppingLocation,
IntersectionMath.getOutgoingVector(toIntersection, carPosition.referenceRoad));
float stoppingDistance = carPosition.referenceRoad.GetRoadLength() - stoppingCompensation.magnitude;
return(stoppingDistance);
}
public float startingDistanceForCurrentDrive(AbstractCarPosition carPosition)
{
/**
*
* refIntersection toIntersection
*
* fromLeftEdge
* \===================================================| ^
* \ | width(toIntersection)
* \ ------------------------------------------------| x
* < >\ | width(refIntersection)
* | \===============================================| v
* | fromRightEdge
* |
* +--> startingDistance
*
*/
Intersection toIntersection = carPosition.referenceRoad.otherIntersection(carPosition.referenceIntersection);
Vector3 fromLeftEdge = builder.coordinateForRoadAtIntersection(carPosition.referenceIntersection, carPosition.referenceRoad,
IntersectionBuilder.RightOrLeft.LEFT);
Vector3 fromRightEdge = builder.coordinateForRoadAtIntersection(carPosition.referenceIntersection, carPosition.referenceRoad,
IntersectionBuilder.RightOrLeft.RIGHT);
// get the proportion
int totalNumberOfLanes =
carPosition.referenceRoad.GetNumberOfLanesInDirectionWithReferenceIntersection(toIntersection)
+ carPosition.referenceRoad.GetNumberOfLanesInDirectionWithReferenceIntersection(carPosition
.referenceIntersection);
Vector3 startingLocation = Vector3.Lerp(fromLeftEdge, fromRightEdge,
1f / 2 + (1f / 2 + carPosition.laneNumber) / totalNumberOfLanes) * (1f / 2);
// project the stopping location onto the outgoing vector
Vector3 startignCompensation = Vector3.Project(startingLocation,
IntersectionMath.getOutgoingVector(carPosition.referenceIntersection, carPosition.referenceRoad));
float startingDistance = startignCompensation.magnitude;
return(startingDistance);
}
private AbstractWayInfo[] AbstractWayInfosForTwoWayIntersection(Intersection intersection, Road road1, Road road2)
{
Vector3 vec1 = IntersectionMath.getOutgoingVector(intersection, road1).normalized;
Vector3 vec2 = IntersectionMath.getOutgoingVector(intersection, road2).normalized;
// create a four way intersection instead of a two way intersection if the angle is less than 90
Vector3[] vecs = null;
float[] roadWidths = null;
switch (GetTwoWayIntersectionType(intersection))
{
case TwoWayIntersectionType.SKEW:
vecs = new[] { vec1, vec2, -vec1, -vec2 };
roadWidths = new[] { road1.GetRoadWidth(), road2.GetRoadWidth(), road1.GetRoadWidth(), road2.GetRoadWidth() };
break;
case TwoWayIntersectionType.TRANSITION:
// create a halfway rotation
Quaternion rotation = Quaternion.Euler(Quaternion.FromToRotation(vec1, vec2).eulerAngles / 2);
// this is the virtual directional vector
Vector3 newVec = rotation * vec1;
// this is the virtual road width, set it to be the average of the two
float newWidth = road1.GetRoadWidth() + road2.GetRoadWidth() / 2;
vecs = new[] { vec1, vec2, newVec, -newVec };
roadWidths = new[] { road1.GetRoadWidth(), road2.GetRoadWidth(), newWidth, newWidth };
break;
case TwoWayIntersectionType.SMOOTH:
Debug.LogError("Code Error, this method is to be called without smooth transition");
break;
}
// zip vecs and roadWidths into abstract road infos
AbstractWayInfo[] infos = AbstractWayInfo.InfosWithDirectionsAndWidths(vecs, roadWidths);
return(infos);
}
private GameObject buildAllWayStopSign(GameObject baseObj, Intersection intersection)
{
foreach (Road road in intersection.getConnectedRoads())
{
// get the coordinate of stop sign for that road
// left and right are regarding the viewpoint from the intersection, although we want our stop sign on
// the right, we need to pass in Left since stop sign is on the left of the road if you look at the road
// from the intersection
Vector3 position = intersectionBuilder.
coordinateForRoadAtIntersection(intersection, road, IntersectionBuilder.RightOrLeft.LEFT);
position = position + intersection.position;
// get the orientation
Quaternion orientation = IntersectionMath.getAngle(intersection, road);
GameObject stopSign = Instantiate(stopSignPrefab, position, orientation);
stopSign.transform.parent = baseObj.transform;
}
return(baseObj);
}
/**
* @brief the detailed type for a two-way intersection
*
* @param intersection the intersection to inspect type
*
*/
public TwoWayIntersectionType GetTwoWayIntersectionType(Intersection intersection)
{
Debug.Assert(intersection.getConnectedRoads().Length == 2);
Road road1 = intersection.getConnectedRoads()[0];
Road road2 = intersection.getConnectedRoads()[1];
Vector3 vec1 = IntersectionMath.getOutgoingVector(intersection, road1).normalized;
Vector3 vec2 = IntersectionMath.getOutgoingVector(intersection, road2).normalized;
// create a four way intersection instead of a two way intersection if the angle is less than 90
if (Vector3.Angle(vec1, vec2) < 90)
{
return(TwoWayIntersectionType.SKEW);
}
else //force a four way intersection if two roads are of different width
if (Mathf.Approximately(road1.GetRoadWidth(), road2.GetRoadWidth()) == false)
{
return(TwoWayIntersectionType.TRANSITION);
}
else
{
return(TwoWayIntersectionType.SMOOTH);
}
}
public void Check2DLineWithLineTest4()
{
var(hasIntersection, ix, iy) = IntersectionMath.Check2DLineWithLine(0, 0, 10, 10, 10, 10, 0, 0);
Assert.False(hasIntersection);
}
public void Check2DLineWithLineTest3()
{
var(hasIntersection, ix, iy) = IntersectionMath.Check2DLineWithLine(0, 0, 10, 10, 0, 10, 5, 5);
Assert.True(hasIntersection);
}
public void AreLinesBoundedBoxesOverlappedTest6()
{
Assert.False(IntersectionMath.AreLinesBoundedBoxesOverlapped(1, 1, 10, 10, 1, 20, 10, 10));
}
public void AreLinesBoundedBoxesOverlappedTest2()
{
Assert.True(IntersectionMath.AreLinesBoundedBoxesOverlapped(1, 1, 10, 10, 5 - 1, 5 + 1, 5 + 1, 5 - 1));
}
public static IEnumerable <LineHL> CutLines(this IEnumerable <LineHL> lines)
{
var source = new Stack <LineHL>(lines);
var target = new Stack <LineHL>();
var cutLines = new Stack <LineHL>();
while (source.Count > 0)
{
var first = source.Pop();
// Testcode
while (source.Count > 0)
{
var second = source.Pop();
var x1 = first.Start.X;
var y1 = first.Start.Y;
var x2 = first.End.X;
var y2 = first.End.Y;
var x3 = second.Start.X;
var y3 = second.Start.Y;
var x4 = second.End.X;
var y4 = second.End.Y;
if (IntersectionMath.AreLinesBoundedBoxesOverlapped(x1, y1, x2, y2, x3, y3, x4, y4))
{
var(hasIntersection, x, y) = IntersectionMath.Check2DLineWithLine(x1, y1, x2, y2, x3, y3, x4, y4);
if (hasIntersection)
{
var intersection = new PointHL(x, y);
var first1 = new LineHL {
Start = first.Start, End = intersection, Edge = first.Edge
};
var first2 = new LineHL {
Start = intersection, End = first.End, Edge = first.Edge
};
var second1 = new LineHL {
Start = second.Start, End = intersection, Edge = second.Edge
};
var second2 = new LineHL {
Start = intersection, End = second.End, Edge = second.Edge
};
if (!first2.Length.EqualsTo(0.0))
{
if (!first1.Length.EqualsTo(0.0))
{
target.Push(first2);
first = first1;
}
else
{
first = first2;
}
}
else
{
first = first1;
}
if (!second1.Length.EqualsTo(0.0))
{
target.Push(second1);
}
if (!second2.Length.EqualsTo(0.0))
{
target.Push(second2);
}
}
else
{
target.Push(second);
}
}
else
{
target.Push(second);
}
}
cutLines.Push(first);
var help = source;
source = target;
target = help;
}
return(cutLines);
}
