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); }