From 096fd3b38a359935dbb551479726961650531b92 Mon Sep 17 00:00:00 2001 From: madc0der Date: Thu, 17 Mar 2022 00:28:08 +0300 Subject: [PATCH] Recent updates (to use with CurbBuilder), moved to new repo --- ERVertexPath/DummyHelperPathController.cs | 6 +- ERVertexPath/ERMultiRoadPathCreator.cs | 157 +++++++++++++++++++++++++++++ ERVertexPath/ERNetworkVertexPathCreator.cs | 19 +++- ERVertexPath/ERPathAdapter.cs | 142 ++++++++++++++++++++++---- ERVertexPath/ERPathCamera.cs | 6 +- ERVertexPath/ERPathToVertexPathWrapper.cs | 33 +++++- 6 files changed, 335 insertions(+), 28 deletions(-) create mode 100644 ERVertexPath/ERMultiRoadPathCreator.cs diff --git a/ERVertexPath/DummyHelperPathController.cs b/ERVertexPath/DummyHelperPathController.cs index ea51f82..77e67ec 100644 --- a/ERVertexPath/DummyHelperPathController.cs +++ b/ERVertexPath/DummyHelperPathController.cs @@ -6,7 +6,7 @@ namespace ERVertexPath { public class DummyHelperPathController : MonoBehaviour { - public ERModularRoad modularRoad; + public GameObject erPathAdapterContainer; public float speedMs; @@ -19,8 +19,8 @@ namespace ERVertexPath private void Start() { - pathAdapter = modularRoad.GetComponent(); - Assert.IsNotNull(pathAdapter, $"Cant find ERPathAdapter for road {modularRoad.name}"); + pathAdapter = erPathAdapterContainer.GetComponent(); + Assert.IsNotNull(pathAdapter, $"Cant find ERPathAdapter for gameObject {erPathAdapterContainer.name}"); } private void FixedUpdate() diff --git a/ERVertexPath/ERMultiRoadPathCreator.cs b/ERVertexPath/ERMultiRoadPathCreator.cs new file mode 100644 index 0000000..0943f1c --- /dev/null +++ b/ERVertexPath/ERMultiRoadPathCreator.cs @@ -0,0 +1,157 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using EasyRoads3Dv3; +using UnityEngine; +using UnityEngine.Assertions; +using UnityEngine.PlayerLoop; + +namespace ERVertexPath +{ + public class ERMultiRoadPathCreator : MonoBehaviour + { + public ERNetworkVertexPathCreator allRoadsPathCreator; + public string roadNameMask; + public bool isShowPathGizmo; + public bool cleanupSourcePaths; + + private ERPathAdapter unionAdapter; + + private bool isInitialized; + + public ERPathAdapter UnionAdapter => unionAdapter; + + private void Awake() + { + Init(); + } + + public void Init(bool forceInit = false) + { + if (!forceInit && isInitialized) + { + return; + } + + isInitialized = true; + + allRoadsPathCreator.Init(); + var lowerNameMask = roadNameMask.ToLower(); + var roads = new ERRoadNetwork().GetRoads().Where(_road => _road.GetName().ToLower().Contains(lowerNameMask)); + + var initialRoad = roads.First(); + var road = initialRoad; + Assert.IsNotNull(road, "Only implicitly closed tracks are supported, can't find any roads with name contains given mask"); + const int traverseLimit = 1000; + var traverseConnectionCount = 0; + + var vertexPathWrappers = new List(); + + DebugLog($"Building union vertex path for roads with mask {roadNameMask}"); + + do + { + vertexPathWrappers.Add(road.roadScript.GetComponent()); + + DebugLog($"Search connections for road: {road.GetName()}"); + var endConnector = road.GetConnectionAtEnd(out _); + + if (endConnector != null) + { + for (var i = 0; i < endConnector.GetConnectionCount(); i++) + { + var connectedRoad = endConnector.GetConnectedRoad(i, out _); + if (connectedRoad != null && connectedRoad != road) + { + road = connectedRoad; + break; + } + } + } + + traverseConnectionCount++; + } while (road != initialRoad && traverseConnectionCount < traverseLimit); + + if (traverseConnectionCount == traverseLimit) + { + throw new Exception($"Too much roads, limit {traverseLimit} reached"); + } + + BuildUnionVertexPath(vertexPathWrappers); + } + + private void BuildUnionVertexPath(IEnumerable wrappers) + { + var vertexList = new List(); + var directionsList = new List(); + var normalsList = new List(); + var rotationsList = new List(); + var distanceList = new List(); + var startIndexToRoad = new Dictionary(); + + var totalDistance = 0f; + var positionIndex = 0; + foreach (var wrapper in wrappers) + { + vertexList.AddRange(wrapper.Positions); + directionsList.AddRange(wrapper.Directions); + normalsList.AddRange(wrapper.Normals); + rotationsList.AddRange(wrapper.Rotations); + // ReSharper disable once AccessToModifiedClosure + var updatedDistances = wrapper.Distances.Select(distance => distance + totalDistance); + distanceList.AddRange(updatedDistances); + totalDistance += wrapper.TotalDistance; + + startIndexToRoad.Add(positionIndex, wrapper.gameObject); + Debug.Log($"Adding road {wrapper.gameObject.name} from index: {positionIndex}"); + + if (cleanupSourcePaths) + { + wrapper.ClearPathData(); + var pathAdapter = wrapper.gameObject.GetComponent(); + if (pathAdapter) + { + pathAdapter.ClearPathData(); + } + } + + positionIndex = vertexList.Count; + } + + unionAdapter = gameObject.AddComponent(); + unionAdapter.InitFromData(totalDistance, vertexList.ToArray(), directionsList.ToArray(), + normalsList.ToArray(), rotationsList.ToArray(), distanceList.ToArray(), startIndexToRoad); + + DebugLog($"Created union adapter for roads with mask = {roadNameMask}, distance = {unionAdapter.TotalDistance}, vertex count = {vertexList.Count}"); + } + + private void OnDrawGizmos() + { + if (!isShowPathGizmo) + { + return; + } + + var verticalOffset = Vector3.up * 0.5f; + for (var i = 0; i < unionAdapter.Positions.Length; i++) + { + var start = unionAdapter.Positions[i]; + var end = i == unionAdapter.Positions.Length - 1 ? unionAdapter.Positions[0] : unionAdapter.Positions[i + 1]; + start += verticalOffset; + end += verticalOffset; + Debug.DrawLine(start, end, i % 2 == 0 ? Color.white : Color.magenta); + Debug.DrawLine(start, start + verticalOffset, Color.yellow); + Debug.DrawLine(start, start + unionAdapter.Normals[i], Color.red); + } + } + + private void DebugLog(string msg) + { + if (!allRoadsPathCreator.logEnabled) + { + return; + } + Debug.Log(msg); + } + } +} \ No newline at end of file diff --git a/ERVertexPath/ERNetworkVertexPathCreator.cs b/ERVertexPath/ERNetworkVertexPathCreator.cs index c0236ba..449638b 100644 --- a/ERVertexPath/ERNetworkVertexPathCreator.cs +++ b/ERVertexPath/ERNetworkVertexPathCreator.cs @@ -10,12 +10,26 @@ namespace ERVertexPath public float defaultAngleThreshold = 5f; public float defaultScanStep = 1f; + public float defaultMaxDistance = 50f; + + private bool isInitialized; private void Awake() { + Init(); + } + + public void Init() + { + if (isInitialized) + { + return; + } + Assert.IsNotNull(GetComponent(), "Cant build vertex paths for all roads, ERModularBase not found"); ScanRoadsAndAppendWrapper(); + isInitialized = true; } private void ScanRoadsAndAppendWrapper() @@ -41,6 +55,7 @@ namespace ERVertexPath wrapper = (ERPathToVertexPathWrapper)road.gameObject.AddComponent(typeof(ERPathToVertexPathWrapper)); wrapper.angleThreshold = defaultAngleThreshold; wrapper.scanStep = defaultScanStep; + wrapper.maxDistance = defaultMaxDistance; DebugLog($"Added new wrapper to road {road.name}"); } else @@ -64,7 +79,7 @@ namespace ERVertexPath if (!adapter) { adapter = (ERPathAdapter) road.gameObject.AddComponent(typeof(ERPathAdapter)); - adapter.initFromWrapper(wrapper); + adapter.InitFromWrapper(wrapper); DebugLog($"Added new adapter to road {road.name}"); } else @@ -72,7 +87,7 @@ namespace ERVertexPath DebugLog($"Found existing adapter for road {road.name}"); } - adapter.initFromWrapper(wrapper); + adapter.InitFromWrapper(wrapper); return adapter; } diff --git a/ERVertexPath/ERPathAdapter.cs b/ERVertexPath/ERPathAdapter.cs index ef86d94..2557237 100644 --- a/ERVertexPath/ERPathAdapter.cs +++ b/ERVertexPath/ERPathAdapter.cs @@ -6,27 +6,76 @@ namespace ERVertexPath { public class ERPathAdapter : MonoBehaviour { - private ERPathToVertexPathWrapper pathWrapper; - private float totalDistance; private Vector3[] positions; private Vector3[] directions; + private Vector3[] normals; private Quaternion[] rotations; private float[] distances; + private Dictionary startIndexToRoadMap; private readonly LinkedLines lastBestSection = new LinkedLines(); + private readonly PathPoint pointInstance = new PathPoint(); + + public float TotalDistance => totalDistance; + + public Vector3[] Positions => positions; + + public Vector3[] Directions => directions; + + public Vector3[] Normals => normals; + + public Quaternion[] Rotations => rotations; - public void initFromWrapper(ERPathToVertexPathWrapper wrapper) + public float[] Distances => distances; + + public void InitFromWrapper(ERPathToVertexPathWrapper wrapper) { - pathWrapper = wrapper; - totalDistance = pathWrapper.TotalDistance; - positions = pathWrapper.Positions; - directions = pathWrapper.Directions; - rotations = pathWrapper.Rotations; - distances = pathWrapper.Distances; + InitFromData(wrapper.TotalDistance, wrapper.Positions, wrapper.Directions, wrapper.Normals, + wrapper.Rotations, wrapper.Distances); + } + + public void InitFromData(float totalDistance, Vector3[] positions, Vector3[] directions, Vector3[] normals, + Quaternion[] rotations, float[] distances, + Dictionary startIndexToRoadMap = null) + { + this.totalDistance = totalDistance; + this.positions = positions; + this.directions = directions; + this.normals = normals; + this.rotations = rotations; + this.distances = distances; + this.startIndexToRoadMap = startIndexToRoadMap; + } + + public virtual PathPoint GetClosestPathPoint(Vector3 p) + { + var closestDistance = GetClosestDistanceAlongPath(p); + return GetPathPoint(closestDistance); + } + + public PathPoint GetPathPoint(float distance) + { + var clampedDistance = clampDistance(distance); + var i1i2 = findNeighbourIndices(clampedDistance); + var i1 = i1i2.Key; + var i2 = i1i2.Value; + + var d1 = distances[i1]; + var d2 = i2 == 0 ? totalDistance : distances[i2]; + + var t = (clampedDistance - d1) / (d2 - d1); + + pointInstance.set( + distance, + Vector3.Lerp(positions[i1], positions[i2], t), + Vector3.Lerp(directions[i1], directions[i2], t), + Vector3.Lerp(normals[i1], normals[i2], t), + Quaternion.Lerp(rotations[i1], rotations[i2], t), + i1, i2 + ); + return pointInstance; } - - public float TotalDistance => totalDistance; public Vector3 GetPointAtDistance(float distance) { @@ -65,29 +114,39 @@ namespace ERVertexPath var i2 = i1i2.Value; var d1 = distances[i1]; - var d2 = i2 == 0 ? totalDistance : distances[i1]; + var d2 = i2 == 0 ? totalDistance : distances[i2]; var t = (clampedDistance - d1) / (d2 - d1); - return Vector3.Lerp(directions[i1], directions[i2], t); + return Vector3.Lerp(directions[i1], directions[i2], t).normalized; } - public float GetLength() + public Vector3 GetSideNormalAtDistance(float distance) { - return totalDistance; + var clampedDistance = clampDistance(distance); + var i1i2 = findNeighbourIndices(clampedDistance); + var i1 = i1i2.Key; + var i2 = i1i2.Value; + + var d1 = distances[i1]; + var d2 = i2 == 0 ? totalDistance : distances[i2]; + + var t = (clampedDistance - d1) / (d2 - d1); + + return Vector3.Lerp(normals[i1], normals[i2], t).normalized; } public float GetClosestDistanceAlongPath(Vector3 p) { var bestSection = findLinkedLines(p); - projectPointOnBestSection(bestSection, p, out var distanceOnPath); + projectPointOnBestSection(bestSection, p, out var distanceOnPath, out _); return distanceOnPath; } public Vector3 GetClosestPointOnPath(Vector3 p, out float closestDistance) { var bestSection = findLinkedLines(p); - return projectPointOnBestSection(bestSection, p, out closestDistance); + return projectPointOnBestSection(bestSection, p, out closestDistance, out _); } public Vector3 GetClosestPointOnPath(Vector3 p) @@ -95,6 +154,21 @@ namespace ERVertexPath return GetClosestPointOnPath(p, out _); } + public void ClearPathData() + { + positions = null; + directions = null; + normals = null; + rotations = null; + distances = null; + } + + public GameObject FindClosestRoadObject(int positionIndex) + { + var key = startIndexToRoadMap.Keys.Last(index => index <= positionIndex); + return startIndexToRoadMap[key]; + } + private float clampDistance(float distance) { if (distance < 0) @@ -139,19 +213,23 @@ namespace ERVertexPath return p1 + dot * p1p2; } - private Vector3 projectPointOnBestSection(LinkedLines bestSection, Vector3 p, out float distanceOnPath) + private Vector3 projectPointOnBestSection(LinkedLines bestSection, Vector3 p, + out float distanceOnPath, + out int startIndex) { var p1p2 = bestSection.p2 - bestSection.p1; var p1p = p - bestSection.p1; if (Vector3.Dot(p1p, p1p2) > 0) { + startIndex = bestSection.i1; return projectPointOnVector(bestSection.p1, bestSection.p2, p, bestSection.d1, bestSection.d2, out distanceOnPath); } else { + startIndex = bestSection.i0; return projectPointOnVector(bestSection.p0, bestSection.p1, p, bestSection.d0, bestSection.d1, out distanceOnPath); @@ -212,4 +290,32 @@ namespace ERVertexPath public float d0, d1, d2; public int i0, i1, i2; } + + public class PathPoint + { + public float distance; + public Vector3 position; + public Vector3 direction; + public Vector3 normal; + public Quaternion rotation; + public int index1; + public int index2; + + public void set(float distance, Vector3 position, Vector3 direction, Vector3 normal, Quaternion rotation, int index1, int index2) + { + this.distance = distance; + this.position = position; + this.direction = direction; + this.normal = normal; + this.rotation = rotation; + this.index1 = index1; + this.index2 = index2; + } + + public PathPoint copyFrom(PathPoint src) + { + set(src.distance, src.position, src.direction, src.normal, src.rotation, src.index1, src.index2); + return this; + } + } } \ No newline at end of file diff --git a/ERVertexPath/ERPathCamera.cs b/ERVertexPath/ERPathCamera.cs index 1dbebfc..8ce0bb3 100644 --- a/ERVertexPath/ERPathCamera.cs +++ b/ERVertexPath/ERPathCamera.cs @@ -5,7 +5,7 @@ namespace ERVertexPath { public class ERPathCamera : MonoBehaviour { - public GameObject modularRoad; + public GameObject erPathAdapterContainer; public float speedMs = 30f; private ERPathAdapter pathAdapter; @@ -16,8 +16,8 @@ namespace ERVertexPath { cameraToFollow = GetComponent(); Assert.IsNotNull(cameraToFollow, "Cant find Camera component for ERPathCamera"); - pathAdapter = modularRoad.GetComponent(); - Assert.IsNotNull(pathAdapter, $"Cant find ERPathAdapter for road {modularRoad.name}"); + pathAdapter = erPathAdapterContainer.GetComponent(); + Assert.IsNotNull(pathAdapter, $"Cant find ERPathAdapter for gameObject {erPathAdapterContainer.name}"); } private void FixedUpdate() diff --git a/ERVertexPath/ERPathToVertexPathWrapper.cs b/ERVertexPath/ERPathToVertexPathWrapper.cs index e400e92..1ad79c1 100644 --- a/ERVertexPath/ERPathToVertexPathWrapper.cs +++ b/ERVertexPath/ERPathToVertexPathWrapper.cs @@ -1,6 +1,5 @@ using System.Collections.Generic; using System.Linq; -using Castle.Core.Internal; using EasyRoads3Dv3; using UnityEngine; @@ -11,11 +10,16 @@ namespace ERVertexPath { public float angleThreshold = 2f; public float scanStep = 1f; + public float maxDistance = 50f; + + public bool isShowPathGizmo; private ERRoad road; private float totalDistance; private Vector3[] positions; private Vector3[] directions; + //Side normals as Up x Direction + private Vector3[] normals; private Quaternion[] rotations; private float[] distances; @@ -25,6 +29,8 @@ namespace ERVertexPath public Vector3[] Directions => directions; + public Vector3[] Normals => normals; + public Quaternion[] Rotations => rotations; public float[] Distances => distances; @@ -40,9 +46,12 @@ namespace ERVertexPath { var vertexList = new List(); var directionsList = new List(); + var normalsList = new List(); var rotationsList = new List(); var distanceList = new List(); + var prevPointDistance = 0f; + var currentRoadElement = 0; for (var t = 0f; t < road.GetDistance(); t += scanStep) { @@ -50,25 +59,44 @@ namespace ERVertexPath var d = road.GetLookatSmooth(t, currentRoadElement); var r = Quaternion.LookRotation(d); - var isSignificantVertex = vertexList.IsNullOrEmpty() || Vector3.Angle(d, directionsList.Last()) > angleThreshold; + var isSignificantVertex = vertexList.Count == 0 + || t - prevPointDistance > maxDistance + || Vector3.Angle(d, directionsList.Last()) > angleThreshold; if (isSignificantVertex) { vertexList.Add(p); directionsList.Add(d); + normalsList.Add(Vector3.Cross(Vector3.up, d)); rotationsList.Add(r); distanceList.Add(t); + prevPointDistance = t; } } positions = vertexList.ToArray(); directions = directionsList.ToArray(); + normals = normalsList.ToArray(); rotations = rotationsList.ToArray(); distances = distanceList.ToArray(); } + public void ClearPathData() + { + positions = null; + directions = null; + normals = null; + rotations = null; + distances = null; + } + private void FixedUpdate() { + if (!isShowPathGizmo) + { + return; + } + var verticalOffset = Vector3.up * 0.5f; for (var i = 0; i < positions.Length; i++) { @@ -78,6 +106,7 @@ namespace ERVertexPath end += verticalOffset; Debug.DrawLine(start, end, i % 2 == 0 ? Color.white : Color.magenta); Debug.DrawLine(start, start + verticalOffset, Color.yellow); + Debug.DrawLine(start, start + normals[i], Color.red); } } }