initial commit

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<ERPathAdapter>();
+            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);
+            }
+        }
+    }
+}

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<ERModularBase>(),
+                "Cant build vertex paths for all roads, ERModularBase not found");
+            ScanRoadsAndAppendWrapper();
+        }
+
+        private void ScanRoadsAndAppendWrapper()
+        {
+            var roads = GetComponentsInChildren<ERModularRoad>();
+            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<ERPathToVertexPathWrapper>();
+            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<ERPathAdapter>();
+            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);
+        }
+    }
+}

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<int, int> findNeighbourIndices(float clampedDistance)
+        {
+            var i1 = distances.ToList().FindLastIndex(d => d <= clampedDistance);
+            
+            var i2 = i1 < distances.Length - 1 ? i1 + 1 : 0;
+            return new KeyValuePair<int, int>(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;
+    }
+}

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<Camera>();
+            Assert.IsNotNull(cameraToFollow, "Cant find Camera component for ERPathCamera");
+            pathAdapter = modularRoad.GetComponent<ERPathAdapter>();
+            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;
+            }
+        }
+    }
+}

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<Vector3>();
+            var directionsList = new List<Vector3>();
+            var rotationsList = new List<Quaternion>();
+            var distanceList = new List<float>();
+
+            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);
+            }
+        }
+    }
+}

ERVertexPath/ERVertexPath.asmdef

@@ -0,0 +1,3 @@
+{
+	"name": "ERVertexPath"
+}

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