@@ -262,7 +262,7 @@ public int GetBestLaneFor(Vector3 Position, bool inverted = false)
262262 float closestFactor = GetFactorForPoint ( Position ) ;
263263 if ( inverted ) closestFactor = 1 - closestFactor ;
264264
265- int closestLane = 0 ;
265+ int closestLane = - 1 ;
266266 float closestLaneDistance = float . MaxValue ;
267267 for ( int i = 0 ; i < GetLaneCount ( Side . Left ) ; i ++ )
268268 {
@@ -335,9 +335,9 @@ public OrientedPoint InterpolateLane(float t, Side side, int laneIndex, float ad
335335 {
336336 if ( t < 0 || t > 1 ) throw new ArgumentOutOfRangeException ( nameof ( t ) , "t must be between 0 and 1" ) ;
337337 if ( side == Side . Left && ( laneIndex < 0 || laneIndex >= LeftLaneOffsetsEnd . Length ) )
338- throw new ArgumentOutOfRangeException ( nameof ( laneIndex ) , $ "laneIndex must be between 0 and { LeftLaneOffsetsEnd . Length - 1 } for left side") ;
338+ throw new ArgumentOutOfRangeException ( nameof ( laneIndex ) , $ "laneIndex ( { laneIndex } ) must be between 0 and { LeftLaneOffsetsEnd . Length - 1 } for left side") ;
339339 if ( side == Side . Right && ( laneIndex < 0 || laneIndex >= RightLaneOffsetsEnd . Length ) )
340- throw new ArgumentOutOfRangeException ( nameof ( laneIndex ) , $ "laneIndex must be between 0 and { RightLaneOffsetsEnd . Length - 1 } for right side") ;
340+ throw new ArgumentOutOfRangeException ( nameof ( laneIndex ) , $ "laneIndex ( { laneIndex } ) must be between 0 and { RightLaneOffsetsEnd . Length - 1 } for right side") ;
341341
342342 float offset = side == Side . Left ? LeftLaneOffsetsEnd [ laneIndex ] : RightLaneOffsetsEnd [ laneIndex ] ;
343343 float lastOffset = side == Side . Left ? ( LeftLaneOffsetsStart != null ? LeftLaneOffsetsStart [ laneIndex ] : offset )
@@ -460,12 +460,61 @@ public OrientedPoint InterpolateBetweenLanesDist(float dist, Side side, float la
460460 /// <returns>Factor from 0-1 along the road.</returns>
461461 public float GetFactorForPoint ( Vector3 point )
462462 {
463- Vector3 ab = Road . Node . Position - Road . ForwardNode . Position ;
464- float lengthSquared = Vector3 . Dot ( ab , ab ) ;
465- if ( lengthSquared == 0 ) return 0 ;
463+ // The road is split into N segments, then we find the best segment and do
464+ // a projection to that. After that there's a slight refining step to avoid twiching
465+ // when going from segment to segment.
466+
467+ // The reason we can't project across the start / end points, is that on curved roads that will
468+ // result in incorrect projections. Imagine a 180 degree curve, if we project across the start / end
469+ // you'll result in a half circle, where the start and end move "faster" than the middle.
466470
467- float t = Vector3 . Dot ( point - Road . ForwardNode . Position , ab ) / lengthSquared ;
468- return Math . Clamp ( t , 0 , 1 ) ;
471+ const int SEGMENTS = 8 ;
472+ const float POINT_DIST = 1f / SEGMENTS ;
473+
474+ float bestT = 0 ;
475+ float minDistanceSq = float . MaxValue ;
476+
477+ Vector3 prevPoint = Interpolate ( 0 ) . Position ;
478+ for ( int i = 0 ; i < SEGMENTS ; i ++ )
479+ {
480+ Vector3 nextPoint = Interpolate ( ( i + 1 ) * POINT_DIST ) . Position ;
481+ Vector3 v = nextPoint - prevPoint ;
482+ float lenSq = Vector3 . Dot ( v , v ) ;
483+
484+ if ( lenSq > 0 )
485+ {
486+ float tLocal = Math . Clamp ( Vector3 . Dot ( point - prevPoint , v ) / lenSq , 0 , 1 ) ;
487+ Vector3 projected = prevPoint + tLocal * v ;
488+ float distSq = Vector3 . DistanceSquared ( point , projected ) ;
489+
490+ if ( distSq < minDistanceSq )
491+ {
492+ minDistanceSq = distSq ;
493+ bestT = ( i + tLocal ) * POINT_DIST ;
494+ }
495+ }
496+ prevPoint = nextPoint ;
497+ }
498+
499+ // Four iterations around bestT
500+ float searchRange = POINT_DIST ;
501+ for ( int r = 0 ; r < 4 ; r ++ )
502+ {
503+ float step = searchRange * 0.25f ;
504+ float t1 = Math . Clamp ( bestT - step , 0 , 1 ) ;
505+ float t2 = Math . Clamp ( bestT + step , 0 , 1 ) ;
506+
507+ float d1 = Vector3 . DistanceSquared ( point , Interpolate ( t1 ) . Position ) ;
508+ float dMid = Vector3 . DistanceSquared ( point , Interpolate ( bestT ) . Position ) ;
509+ float d2 = Vector3 . DistanceSquared ( point , Interpolate ( t2 ) . Position ) ;
510+
511+ if ( d1 < dMid && d1 < d2 ) { bestT = t1 ; }
512+ else if ( d2 < dMid ) { bestT = t2 ; }
513+
514+ searchRange *= 0.5f ;
515+ }
516+
517+ return bestT ;
469518 }
470519
471520 /// <summary>
@@ -596,13 +645,30 @@ public ParsedRoad GetParsedRoadForFactor(float factor)
596645 float accumulatedLength = 0 ;
597646 foreach ( var road in Roads )
598647 {
599- if ( accumulatedLength + road . Road . Length >= distance )
648+ if ( accumulatedLength + road . Road . Length >= distance - 1f )
600649 return road ;
601650 accumulatedLength += road . Road . Length ;
602651 }
603652 return Roads [ Roads . Count - 1 ] ;
604653 }
605654
655+ public float FactorToRoadFactor ( float factor , ParsedRoad road )
656+ {
657+ float distance = factor * TotalLength ;
658+ float accumulatedLength = 0 ;
659+ foreach ( var r in Roads )
660+ {
661+ if ( accumulatedLength + r . Road . Length >= distance - 1f )
662+ {
663+ float localDistance = distance - accumulatedLength ;
664+ float localFactor = localDistance / r . Road . Length ;
665+ return localFactor ;
666+ }
667+ accumulatedLength += r . Road . Length ;
668+ }
669+ return 1f ; // Return 1f if no road is found (should not happen)
670+ }
671+
606672 /// <summary>
607673 /// Get the best lane for a specific position. Negative lanes indicate left-side lanes, while positives <br/>
608674 /// right side lanes. Each lane is from 1 to X (so -1, -2, -3 etc...)
@@ -695,6 +761,7 @@ public OrientedPoint InterpolateLane(float t, Side side, int laneIndex, float ad
695761 throw new ArgumentOutOfRangeException ( nameof ( laneIndex ) , $ "laneIndex must be between 0 and { GetLaneCount ( Side . Right ) - 1 } for right side") ;
696762
697763 ParsedRoad closestRoad = GetParsedRoadForFactor ( t ) ;
764+ t = FactorToRoadFactor ( t , closestRoad ) ;
698765
699766 float offset = side == Side . Left ? closestRoad . LeftLaneOffsetsEnd [ laneIndex ] : closestRoad . RightLaneOffsetsEnd [ laneIndex ] ;
700767 float lastOffset = side == Side . Left ? ( closestRoad . LeftLaneOffsetsStart != null ? closestRoad . LeftLaneOffsetsStart [ laneIndex ] : offset )
@@ -1039,6 +1106,16 @@ public Node GetNodeInCommon(ParsedPrefab other)
10391106 throw new ArgumentException ( "No common node between the two prefabs" ) ;
10401107 }
10411108
1109+ public Node GetNodeInCommon ( ParsedRoadList roadList )
1110+ {
1111+ foreach ( var node in Prefab . Nodes )
1112+ {
1113+ if ( node . Uid == roadList . StartNode . Uid || node . Uid == roadList . EndNode . Uid )
1114+ return ( Node ) node ;
1115+ }
1116+ throw new ArgumentException ( "No common node between the prefab and the road list" ) ;
1117+ }
1118+
10421119 public Node GetNodeInCommon ( ParsedRoad road )
10431120 {
10441121 foreach ( var node in Prefab . Nodes )
0 commit comments