diff --git a/ERVertexPath/DummyHelperPathController.cs b/ERVertexPath/DummyHelperPathController.cs new file mode 100644 index 0000000..ea51f82 --- /dev/null +++ b/ERVertexPath/DummyHelperPathController.cs @@ -0,0 +1,50 @@ +using EasyRoads3Dv3; +using UnityEngine; +using UnityEngine.Assertions; + +namespace ERVertexPath +{ + public class DummyHelperPathController : MonoBehaviour + { + public ERModularRoad modularRoad; + + public float speedMs; + + [Tooltip("Position by closest point or by closest distance")] + public bool positionByClosestPoint; + + public bool alignToPath; + + private ERPathAdapter pathAdapter; + + private void Start() + { + pathAdapter = modularRoad.GetComponent(); + Assert.IsNotNull(pathAdapter, $"Cant find ERPathAdapter for road {modularRoad.name}"); + } + + private void FixedUpdate() + { + if (!alignToPath) + { + return; + } + + var p = transform.position + transform.forward * (speedMs * Time.fixedDeltaTime); + + if (positionByClosestPoint) + { + var pathP = pathAdapter.GetClosestPointOnPath(p, out var closestDistance); + transform.position = pathP; + transform.rotation = pathAdapter.GetRotationAtDistance(closestDistance); + } + else + { + var closestDistance = pathAdapter.GetClosestDistanceAlongPath(p); + var pathP = pathAdapter.GetPointAtDistance(closestDistance); + transform.position = pathP; + transform.rotation = pathAdapter.GetRotationAtDistance(closestDistance); + } + } + } +} \ No newline at end of file diff --git a/ERVertexPath/ERNetworkVertexPathCreator.cs b/ERVertexPath/ERNetworkVertexPathCreator.cs new file mode 100644 index 0000000..c0236ba --- /dev/null +++ b/ERVertexPath/ERNetworkVertexPathCreator.cs @@ -0,0 +1,89 @@ +using EasyRoads3Dv3; +using UnityEngine; +using UnityEngine.Assertions; + +namespace ERVertexPath +{ + public class ERNetworkVertexPathCreator : MonoBehaviour + { + public bool logEnabled; + + public float defaultAngleThreshold = 5f; + public float defaultScanStep = 1f; + + private void Awake() + { + Assert.IsNotNull(GetComponent(), + "Cant build vertex paths for all roads, ERModularBase not found"); + ScanRoadsAndAppendWrapper(); + } + + private void ScanRoadsAndAppendWrapper() + { + var roads = GetComponentsInChildren(); + DebugLog($"Found {roads.Length} roads"); + + foreach (var road in roads) + { + DebugLog($"Processing road {road.name}"); + var wrapper = AppendWrapper(road); + AppendAdapter(road, wrapper); + } + + DebugLog($"All roads were extended with vertex path wrappers/adapters"); + } + + private ERPathToVertexPathWrapper AppendWrapper(ERModularRoad road) + { + var wrapper = road.GetComponent(); + if (!wrapper) + { + wrapper = (ERPathToVertexPathWrapper)road.gameObject.AddComponent(typeof(ERPathToVertexPathWrapper)); + wrapper.angleThreshold = defaultAngleThreshold; + wrapper.scanStep = defaultScanStep; + DebugLog($"Added new wrapper to road {road.name}"); + } + else + { + DebugLog($"Found existing wrapper for road {road.name}"); + } + + InitVertexPathWrapper(road, wrapper); + return wrapper; + } + + private void InitVertexPathWrapper(ERModularRoad road, ERPathToVertexPathWrapper wrapper) + { + wrapper.Init(road); + DebugLog($"Wrapper for road {road.name} initialized with {wrapper.Positions.Length} points and {wrapper.TotalDistance} length"); + } + + private ERPathAdapter AppendAdapter(ERModularRoad road, ERPathToVertexPathWrapper wrapper) + { + var adapter = road.GetComponent(); + if (!adapter) + { + adapter = (ERPathAdapter) road.gameObject.AddComponent(typeof(ERPathAdapter)); + adapter.initFromWrapper(wrapper); + DebugLog($"Added new adapter to road {road.name}"); + } + else + { + DebugLog($"Found existing adapter for road {road.name}"); + } + + adapter.initFromWrapper(wrapper); + return adapter; + } + + private void DebugLog(string message) + { + if (!logEnabled) + { + return; + } + + Debug.Log(message); + } + } +} \ No newline at end of file diff --git a/ERVertexPath/ERPathAdapter.cs b/ERVertexPath/ERPathAdapter.cs new file mode 100644 index 0000000..ef86d94 --- /dev/null +++ b/ERVertexPath/ERPathAdapter.cs @@ -0,0 +1,215 @@ +using System.Collections.Generic; +using System.Linq; +using UnityEngine; + +namespace ERVertexPath +{ + public class ERPathAdapter : MonoBehaviour + { + private ERPathToVertexPathWrapper pathWrapper; + + private float totalDistance; + private Vector3[] positions; + private Vector3[] directions; + private Quaternion[] rotations; + private float[] distances; + + private readonly LinkedLines lastBestSection = new LinkedLines(); + + public void initFromWrapper(ERPathToVertexPathWrapper wrapper) + { + pathWrapper = wrapper; + totalDistance = pathWrapper.TotalDistance; + positions = pathWrapper.Positions; + directions = pathWrapper.Directions; + rotations = pathWrapper.Rotations; + distances = pathWrapper.Distances; + } + + public float TotalDistance => totalDistance; + + public Vector3 GetPointAtDistance(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); + + return Vector3.Lerp(positions[i1], positions[i2], t); + } + + public Quaternion GetRotationAtDistance(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); + + return Quaternion.Lerp(rotations[i1], rotations[i2], t); + } + + public Vector3 GetDirectionAtDistance(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[i1]; + + var t = (clampedDistance - d1) / (d2 - d1); + + return Vector3.Lerp(directions[i1], directions[i2], t); + } + + public float GetLength() + { + return totalDistance; + } + + public float GetClosestDistanceAlongPath(Vector3 p) + { + var bestSection = findLinkedLines(p); + projectPointOnBestSection(bestSection, p, out var distanceOnPath); + return distanceOnPath; + } + + public Vector3 GetClosestPointOnPath(Vector3 p, out float closestDistance) + { + var bestSection = findLinkedLines(p); + return projectPointOnBestSection(bestSection, p, out closestDistance); + } + + public Vector3 GetClosestPointOnPath(Vector3 p) + { + return GetClosestPointOnPath(p, out _); + } + + private float clampDistance(float distance) + { + if (distance < 0) + { + var clampedNegative = distance < -totalDistance + ? (distance / totalDistance - Mathf.Ceil(distance / totalDistance)) * totalDistance + : distance; + return totalDistance + clampedNegative; + } + return distance > totalDistance + ? (distance / totalDistance - Mathf.Floor(distance / totalDistance)) * totalDistance + : distance; + } + + private KeyValuePair findNeighbourIndices(float clampedDistance) + { + var i1 = distances.ToList().FindLastIndex(d => d <= clampedDistance); + + var i2 = i1 < distances.Length - 1 ? i1 + 1 : 0; + return new KeyValuePair(i1, i2); + } + + private Vector3 projectPointOnVector(Vector3 p1, Vector3 p2, + Vector3 p, + float d1, float d2, + out float distanceOnPath) + { + var p1p2 = p2 - p1; + var p1p = p - p1; + var sqrMag = p1p2.sqrMagnitude; + if (sqrMag < 0.0001f) + { + distanceOnPath = d1; + return p1; + } + + var dot = Vector3.Dot(p1p, p1p2); + dot = Mathf.Clamp01(dot / sqrMag); //TODO: is clamp needed? + + distanceOnPath = d1 + dot * (d2 - d1); + + return p1 + dot * p1p2; + } + + private Vector3 projectPointOnBestSection(LinkedLines bestSection, Vector3 p, out float distanceOnPath) + { + var p1p2 = bestSection.p2 - bestSection.p1; + var p1p = p - bestSection.p1; + + if (Vector3.Dot(p1p, p1p2) > 0) + { + return projectPointOnVector(bestSection.p1, bestSection.p2, + p, bestSection.d1, bestSection.d2, + out distanceOnPath); + } + else + { + return projectPointOnVector(bestSection.p0, bestSection.p1, + p, bestSection.d0, bestSection.d1, + out distanceOnPath); + } + } + + private LinkedLines findLinkedLines(Vector3 p) + { + var bestSection = lastBestSection; + var minDistance = float.MaxValue; + + for (var i = 0; i < positions.Length; i++) + { + var isFirstPoint = i == 0; + var isLastPoint = i == positions.Length - 1; + + var i0 = isFirstPoint ? positions.Length - 1 : i - 1; + var i1 = isLastPoint ? 0 : i + 1; + + var p1 = positions[i]; + + var p1p = p - p1; + var distance = p1p.sqrMagnitude; + if (distance < minDistance) + { + minDistance = distance; + bestSection.p0 = positions[i0]; + bestSection.p1 = p1; + bestSection.p2 = positions[i1]; + + bestSection.d0 = distances[i0]; + bestSection.d1 = isFirstPoint ? totalDistance + distances[i] : distances[i]; + if (isFirstPoint) + { + bestSection.d2 = totalDistance + distances[i1]; + } else if (isLastPoint) + { + bestSection.d2 = totalDistance + distances[i1]; + } + else + { + bestSection.d2 = distances[i1]; + } + + bestSection.i0 = i0; + bestSection.i1 = i; + bestSection.i2 = i1; + } + } + + return bestSection; + } + } + + class LinkedLines + { + public Vector3 p0, p1, p2; + public float d0, d1, d2; + public int i0, i1, i2; + } +} \ No newline at end of file diff --git a/ERVertexPath/ERPathCamera.cs b/ERVertexPath/ERPathCamera.cs new file mode 100644 index 0000000..1dbebfc --- /dev/null +++ b/ERVertexPath/ERPathCamera.cs @@ -0,0 +1,38 @@ +using UnityEngine; +using UnityEngine.Assertions; + +namespace ERVertexPath +{ + public class ERPathCamera : MonoBehaviour + { + public GameObject modularRoad; + public float speedMs = 30f; + + private ERPathAdapter pathAdapter; + private Camera cameraToFollow; + private float cameraPosition; + + private void Start() + { + cameraToFollow = GetComponent(); + Assert.IsNotNull(cameraToFollow, "Cant find Camera component for ERPathCamera"); + pathAdapter = modularRoad.GetComponent(); + Assert.IsNotNull(pathAdapter, $"Cant find ERPathAdapter for road {modularRoad.name}"); + } + + private void FixedUpdate() + { + var position = pathAdapter.GetPointAtDistance(cameraPosition); + var lookAt = pathAdapter.GetRotationAtDistance(cameraPosition); + + cameraToFollow.transform.position = position + Vector3.up * 5f; + cameraToFollow.transform.rotation = lookAt; + + cameraPosition += Time.deltaTime * speedMs; + if (cameraPosition > pathAdapter.TotalDistance) + { + cameraPosition = 0f; + } + } + } +} \ No newline at end of file diff --git a/ERVertexPath/ERPathToVertexPathWrapper.cs b/ERVertexPath/ERPathToVertexPathWrapper.cs new file mode 100644 index 0000000..e400e92 --- /dev/null +++ b/ERVertexPath/ERPathToVertexPathWrapper.cs @@ -0,0 +1,84 @@ +using System.Collections.Generic; +using System.Linq; +using Castle.Core.Internal; +using EasyRoads3Dv3; +using UnityEngine; + +namespace ERVertexPath +{ + //Create for every ER road on scene - will approximate spline path to vertex path + public class ERPathToVertexPathWrapper : MonoBehaviour + { + public float angleThreshold = 2f; + public float scanStep = 1f; + + private ERRoad road; + private float totalDistance; + private Vector3[] positions; + private Vector3[] directions; + private Quaternion[] rotations; + private float[] distances; + + public float TotalDistance => totalDistance; + + public Vector3[] Positions => positions; + + public Vector3[] Directions => directions; + + public Quaternion[] Rotations => rotations; + + public float[] Distances => distances; + + public void Init(ERModularRoad modularRoad) + { + road = new ERRoadNetwork().GetRoadByGameObject(modularRoad.gameObject); + totalDistance = road.GetDistance(); + buildRoadVertexPath(); + } + + private void buildRoadVertexPath() + { + var vertexList = new List(); + var directionsList = new List(); + var rotationsList = new List(); + var distanceList = new List(); + + var currentRoadElement = 0; + for (var t = 0f; t < road.GetDistance(); t += scanStep) + { + var p = road.GetPosition(t, ref currentRoadElement); + var d = road.GetLookatSmooth(t, currentRoadElement); + var r = Quaternion.LookRotation(d); + + var isSignificantVertex = vertexList.IsNullOrEmpty() || Vector3.Angle(d, directionsList.Last()) > angleThreshold; + + if (isSignificantVertex) + { + vertexList.Add(p); + directionsList.Add(d); + rotationsList.Add(r); + distanceList.Add(t); + } + } + + positions = vertexList.ToArray(); + directions = directionsList.ToArray(); + rotations = rotationsList.ToArray(); + distances = distanceList.ToArray(); + } + + private void FixedUpdate() + { + var verticalOffset = Vector3.up * 0.5f; + for (var i = 0; i < positions.Length; i++) + { + var start = positions[i]; + var end = i == positions.Length - 1 ? positions[0] : 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); + } + } + } +} \ No newline at end of file diff --git a/ERVertexPath/ERVertexPath.asmdef b/ERVertexPath/ERVertexPath.asmdef new file mode 100644 index 0000000..4824d89 --- /dev/null +++ b/ERVertexPath/ERVertexPath.asmdef @@ -0,0 +1,3 @@ +{ + "name": "ERVertexPath" +} diff --git a/README.md b/README.md index c8e111e..f1d4a6e 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,57 @@ # EasyRoadsVertexPath -Vertex paths wrapper for EasyRoads3D spline road paths with API similar to PathCreator + +Here are scripts to build vertex paths for EasyRoads3d spline road paths, including sample scripts to implement path following. + +**EasyRoads3D**: https://assetstore.unity.com/packages/tools/terrain/easyroads3d-pro-v3-469 + +Inspired by API of **Bezier Path Creator**: https://assetstore.unity.com/packages/tools/utilities/b-zier-path-creator-136082 + + + +## How to use +1. Add `ERNetworkVertexPathCreator` to ER Road Network + It will add (if not exists) `ERNetworkVertexPathCreator` and `ERPathAdapter` to any `ERModularRoad` from children. + `ERNetworkVertexPathCreator` will create vertex path based on ER spline path with given `angleThreshold` and `scanStep` + `ERPathAdapter` will provide API to align objects along the path and calculate nearest position and distance. +2. Add `ERPathCamera` (sample script) to any `Camera` to follow given road path (link `modularRoad` to any `ERModularRoad`) + **Input**: + - ERModularRoad - road to follow (will search for ERPathAdapter component and use it) + - SpeedMs - velocity in meters per second +3. Add DummyHelperPathController (another sample script) to any object to move it along the path + Input: + `ERModularRoad` - road object to follow + `speedMs` - move velocity in m/s + `positionByClosestPoint` - position object by closest point or by closest distance + `alignToPath` - enable/disable path following (useful to check path alignment for objects out of path in different configs) + +## Description of sample scene +Lets review sample config - it will contain single ER Road Network with Road, Camera to move along the Road and two GameObjects +which will also move along road path. + +![](docs/sample_scene.png) + +Required config steps are described by following screenshots + +1. Add `ERNetworkVertexPathCreator` to Road Network +![](docs/config_step_1.png) + +2. Add `ERPathCamera` to `Camera` and link it to `ERModularRoad` +![](docs/config_step_2.png) + +3. Configure `ByPosition` path follower +![](docs/config_step_3.png) + +4. Configure `ByDistance` path follower +![](docs/config_step_4.png) + +Vertex paths gizmos are shown in Play mode: +![](docs/gizmos.png) + +## Scripts +1. `ERNetworkVertexPathCreator` - scan Road Network for roads, attach `ERPathToVertexPathWrapper` and `ERPathAdapter` to each road (if not exists) +2. `ERPathToVertexPathWrapper` - builds vertex path from ER spline path for given road (by default with `angleThreshold` and `scanStep` set for ERNetworkVertexPathCreator). If `angleThreshold` or `scanStep` must be customized, `ERNetworkVertexPathCreator` can be manually attached to the road +3. `ERPathAdapter` - provide API to follow vertex path + +## Notes: +- I'm not using crossings, therefore linked roads are not supported +- All methods distance-related methods are map distance to current road bounds (from zero to total path distance) so you can use negative distance or distance much more than total road length diff --git a/docs/config_step_1.png b/docs/config_step_1.png new file mode 100644 index 0000000..d7b11be Binary files /dev/null and b/docs/config_step_1.png differ diff --git a/docs/config_step_2.png b/docs/config_step_2.png new file mode 100644 index 0000000..03ef431 Binary files /dev/null and b/docs/config_step_2.png differ diff --git a/docs/config_step_3.png b/docs/config_step_3.png new file mode 100644 index 0000000..26dfbab Binary files /dev/null and b/docs/config_step_3.png differ diff --git a/docs/config_step_4.png b/docs/config_step_4.png new file mode 100644 index 0000000..2d85466 Binary files /dev/null and b/docs/config_step_4.png differ diff --git a/docs/gizmos.png b/docs/gizmos.png new file mode 100644 index 0000000..e68e580 Binary files /dev/null and b/docs/gizmos.png differ diff --git a/docs/overview.png b/docs/overview.png new file mode 100644 index 0000000..d2467ac Binary files /dev/null and b/docs/overview.png differ diff --git a/docs/sample_scene.png b/docs/sample_scene.png new file mode 100644 index 0000000..c4b8d9e Binary files /dev/null and b/docs/sample_scene.png differ