Skip to content
Snippets Groups Projects
Commit f23325b2 authored by Knuiman, Bart's avatar Knuiman, Bart
Browse files

Now interpolates over catmull rom

parent 7d945216
Branches
No related tags found
No related merge requests found
...@@ -16,12 +16,15 @@ namespace WanderOV ...@@ -16,12 +16,15 @@ namespace WanderOV
internal long lastUpdate; internal long lastUpdate;
internal long lastAdvanceTime; internal long lastAdvanceTime;
internal bool reachedLastStop; internal bool reachedLastStop;
internal bool mustTeleport;
internal IJourney journey; internal IJourney journey;
internal float speedMtrPerSec; internal float speedMtrPerSec;
internal float interpolatedX; internal float interpolatedX;
internal float interpolatedY; internal float interpolatedY;
internal float catmullX;
internal float catmullY;
internal float angle; internal float angle;
internal int prevPointIdxInJourney=-1; internal int prevPointIdxInJourney=-1;
...@@ -55,6 +58,7 @@ namespace WanderOV ...@@ -55,6 +58,7 @@ namespace WanderOV
{ {
var timeNow = stopwatch.ElapsedMilliseconds; var timeNow = stopwatch.ElapsedMilliseconds;
foreach( var vkp in vehicles ) foreach( var vkp in vehicles )
{ {
var vehicle = vkp.Value; var vehicle = vkp.Value;
...@@ -69,26 +73,30 @@ namespace WanderOV ...@@ -69,26 +73,30 @@ namespace WanderOV
float advanceDistanceMtr = elapsedSeconds*vehicle.speedMtrPerSec; float advanceDistanceMtr = elapsedSeconds*vehicle.speedMtrPerSec;
if ( network.FindPositionInJourney( if ( network.FindPositionInJourney(
vehicle.journey, vehicle.journey,
ref vehicle.prevInterpolatedPointIdxInJourney, ref vehicle.prevInterpolatedPointIdxInJourney,
new Vector2( vehicle.interpolatedX, vehicle.interpolatedY ), new Vector2( vehicle.interpolatedX, vehicle.interpolatedY ),
advanceDistanceMtr, advanceDistanceMtr,
out (Vector2 point, float dist, Vector2 dir) interpolant out (Vector2 point, float dist, Vector2 dir) interpolant,
out Vector2 catmullPnt
)) ))
{ {
vehicle.interpolatedX = interpolant.point.X; vehicle.interpolatedX = interpolant.point.X;
vehicle.interpolatedY = interpolant.point.Y; vehicle.interpolatedY = interpolant.point.Y;
vehicle.catmullX = catmullPnt.X;
vehicle.catmullY = catmullPnt.Y;
Debug.Assert( MathF.Abs( interpolant.dir.Length()-1) < 0.001f ); Debug.Assert( MathF.Abs( interpolant.dir.Length()-1) < 0.001f );
vehicle.angle = MathF.Atan2( interpolant.dir.X, interpolant.dir.Y ) * 57.29578f; // To Degrees. vehicle.angle = MathF.Atan2( interpolant.dir.X, interpolant.dir.Y ) * 57.29578f; // To Degrees.
// Check if is at last stop. // Check if is at last stop.
float distToLastStop = Vector2.Distance( vehicle.journey.Stop( vehicle.journey.NumStops-1 ).stop.Point, new Vector2( vehicle.interpolatedX, vehicle.interpolatedY ) ); float distToLastStop = Vector2.Distance( vehicle.journey.Stop( vehicle.journey.NumStops-1 ).stop.Point, new Vector2( vehicle.interpolatedX, vehicle.interpolatedY ) );
if ( distToLastStop < 5 ) if ( distToLastStop < 1.5f )
{ {
vehicle.reachedLastStop = true; vehicle.reachedLastStop = true;
} }
} }
// break;
} }
} }
...@@ -112,7 +120,7 @@ namespace WanderOV ...@@ -112,7 +120,7 @@ namespace WanderOV
vehicles.Add( vehicleKey, vehicle ); vehicles.Add( vehicleKey, vehicle );
} }
var prevLineNumber = vehicle.linePrivateNumber; var prevJourneyNumb = vehicle.journeyNumber;
vehicle.punctuality = v.infos[(int)VehicleInfoType.Punctuality]; vehicle.punctuality = v.infos[(int)VehicleInfoType.Punctuality];
vehicle.journeyNumber = v.infos[(int)VehicleInfoType.JourneyNumber]; vehicle.journeyNumber = v.infos[(int)VehicleInfoType.JourneyNumber];
vehicle.lastUpdate = lastTime; vehicle.lastUpdate = lastTime;
...@@ -121,44 +129,53 @@ namespace WanderOV ...@@ -121,44 +129,53 @@ namespace WanderOV
vehicle.rdx = float.Parse( v.infos[(int)VehicleInfoType.Rdx] ); vehicle.rdx = float.Parse( v.infos[(int)VehicleInfoType.Rdx] );
vehicle.rdy = float.Parse( v.infos[(int)VehicleInfoType.Rdy] ); vehicle.rdy = float.Parse( v.infos[(int)VehicleInfoType.Rdy] );
// If journey changes, reset position to new journey. if (vehicle.rdx == -1 && vehicle.rdy == -1)
if (vehicle.linePrivateNumber != prevLineNumber)
{ {
vehicle.journey = network.FindJourney( owner, vehicle.linePrivateNumber ); vehicles.Remove( vehicleKey );
vehicle.prevPointIdxInJourney = -1;
vehicle.prevInterpolatedPointIdxInJourney = -1;
vehicle.lastAdvanceTime = lastTime;
vehicle.speedMtrPerSec = 25/3.6f; // initial speed
// Sync the interpolated X/Y with the actual X/Y as the journey has changed.
vehicle.interpolatedX = vehicle.rdx;
vehicle.interpolatedY = vehicle.rdy;
vehicle.reachedLastStop = false;
} }
else
// Find the actual position in journey and compare it with interpolated position, if ahead, speedup movement, otherwise slowdown.
if (vehicle.Valid &&
network.FindPositionInJourney(
vehicle.journey,
ref vehicle.prevPointIdxInJourney,
new Vector2( vehicle.rdx, vehicle.rdy ),
0,
out (Vector2 point, float dist, Vector2 dir) interpolant
))
{ {
if (vehicle.prevPointIdxInJourney != -1 && vehicle.prevInterpolatedPointIdxInJourney != -1) // If journey changes, reset position to new journey.
if (vehicle.journeyNumber != prevJourneyNumb)
{ {
int segmentDelta = vehicle.prevPointIdxInJourney - vehicle.prevInterpolatedPointIdxInJourney; vehicle.mustTeleport = true;
if (segmentDelta>0) vehicle.journey = network.FindJourney( owner, vehicle.journeyNumber, vehicle.linePrivateNumber );
{ vehicle.prevPointIdxInJourney = -1;
vehicle.speedMtrPerSec *= (1 + 0.2f*segmentDelta); vehicle.prevInterpolatedPointIdxInJourney = -1;
} vehicle.lastAdvanceTime = lastTime;
else if ( segmentDelta < 0) vehicle.speedMtrPerSec = 25/3.6f; // initial speed
// Sync the interpolated X/Y with the actual X/Y as the journey has changed.
vehicle.interpolatedX = vehicle.rdx;
vehicle.interpolatedY = vehicle.rdy;
vehicle.reachedLastStop = false;
}
// Find the actual position in journey and compare it with interpolated position, if ahead, speedup movement, otherwise slowdown.
if (vehicle.Valid &&
network.FindPositionInJourney(
vehicle.journey,
ref vehicle.prevPointIdxInJourney,
new Vector2( vehicle.rdx, vehicle.rdy ),
0,
out _,
out _
))
{
if (vehicle.prevPointIdxInJourney != -1 && vehicle.prevInterpolatedPointIdxInJourney != -1)
{ {
vehicle.speedMtrPerSec /= (1 + 0.2f*-segmentDelta); int segmentDelta = vehicle.prevPointIdxInJourney - vehicle.prevInterpolatedPointIdxInJourney;
if (segmentDelta>0)
{
vehicle.speedMtrPerSec *= (1 + 0.2f*segmentDelta);
}
else if (segmentDelta < 0)
{
vehicle.speedMtrPerSec /= (1 + 0.2f*-segmentDelta);
}
if (vehicle.speedMtrPerSec < 5) vehicle.speedMtrPerSec = 5;
if (vehicle.speedMtrPerSec > 20) vehicle.speedMtrPerSec = 20;
} }
if (vehicle.speedMtrPerSec < 3) vehicle.speedMtrPerSec = 3;
if (vehicle.speedMtrPerSec > 17) vehicle.speedMtrPerSec= 17;
} }
} }
} ); } );
...@@ -170,7 +187,7 @@ namespace WanderOV ...@@ -170,7 +187,7 @@ namespace WanderOV
var pendingRemoves = new List<string>(); var pendingRemoves = new List<string>();
foreach (var v in vehicles) foreach (var v in vehicles)
{ {
if (timeNow - v.Value.lastUpdate > 1000 * 60*2) if (timeNow - v.Value.lastUpdate > 1000 * 60*1.5f)
{ {
pendingRemoves.Add( v.Key ); pendingRemoves.Add( v.Key );
} }
...@@ -183,14 +200,15 @@ namespace WanderOV ...@@ -183,14 +200,15 @@ namespace WanderOV
if (vehicles==null||vehicles.Count==0) if (vehicles==null||vehicles.Count==0)
return null; return null;
StringBuilder sb = new StringBuilder(); StringBuilder sb = new StringBuilder();
sb.AppendLine( $"owner,displayLineNumb,privLineNumb,journyNum,vehicleNum,punctuality,rdx,rdy,interpolated-rdx,interpolated-rdy,angleDeg,speedMtrps" ); sb.AppendLine( $"mustTeleport,owner,displayLineNumb,privLineNumb,journyNum,vehicleNum,punctuality,rdx,rdy,interpolated-rdx,interpolated-rdy,angleDeg,speedMtrps" );
foreach (var kvp in vehicles) foreach (var kvp in vehicles)
{ {
var v = kvp.Value; var v = kvp.Value;
if (v.Valid) if (v.Valid)
{ {
var pubNumber = v.journey.Line.LineDisplayNumber; var pubNumber = v.journey.Line.LineDisplayNumber;
sb.AppendLine( $"{v.owner},{pubNumber},{v.linePrivateNumber},{v.journeyNumber},{v.vehicleNumber},{v.punctuality},{v.rdx},{v.rdy},{v.interpolatedX},{v.interpolatedY},{v.angle},{v.speedMtrPerSec}" ); sb.AppendLine( $"{v.mustTeleport},{v.owner},{pubNumber},{v.linePrivateNumber},{v.journeyNumber},{v.vehicleNumber},{v.punctuality},{v.rdx},{v.rdy},{v.interpolatedX},{v.interpolatedY},{v.angle},{v.speedMtrPerSec}" );
v.mustTeleport=false;
} }
} }
return sb.ToString(); return sb.ToString();
......
...@@ -2,6 +2,7 @@ ...@@ -2,6 +2,7 @@
using System.IO.Compression; using System.IO.Compression;
using System.Numerics; using System.Numerics;
using System.Reflection; using System.Reflection;
using System.Security.Cryptography;
using System.Xml; using System.Xml;
using WanderOV; using WanderOV;
...@@ -41,7 +42,6 @@ namespace OVAPI.Code.OVParsers ...@@ -41,7 +42,6 @@ namespace OVAPI.Code.OVParsers
class Route : IRoute class Route : IRoute
{ {
internal Line line; internal Line line;
internal JourneyPattern journey;
internal List<RouteLink> links; internal List<RouteLink> links;
internal List<(Vector2 point, float dst, Vector2 dir)> points; internal List<(Vector2 point, float dst, Vector2 dir)> points;
internal bool valid; internal bool valid;
...@@ -111,8 +111,7 @@ namespace OVAPI.Code.OVParsers ...@@ -111,8 +111,7 @@ namespace OVAPI.Code.OVParsers
Dictionary<string, Destination> destinations = new Dictionary<string, Destination>(); Dictionary<string, Destination> destinations = new Dictionary<string, Destination>();
Dictionary<string, Stop> stops = new Dictionary<string, Stop>(); Dictionary<string, Stop> stops = new Dictionary<string, Stop>();
Dictionary<string, JourneyPattern> journeyPatterns = new Dictionary<string, JourneyPattern>(); Dictionary<string, JourneyPattern> journeyPatterns = new Dictionary<string, JourneyPattern>();
Dictionary<string, JourneyPattern> journeyNumbersToPattern = new Dictionary<string, JourneyPattern>(); Dictionary<string, Dictionary<string, List<JourneyPattern>>> journeyNumbersToPattern = new Dictionary<string, Dictionary<string, List<JourneyPattern>>>();
Dictionary<string, Line> privLineNumberToLine = new Dictionary<string, Line>();
public void PrintStats() public void PrintStats()
{ {
...@@ -148,7 +147,7 @@ namespace OVAPI.Code.OVParsers ...@@ -148,7 +147,7 @@ namespace OVAPI.Code.OVParsers
ParseElementList( doc, "DestinationDisplay", LoadDestination ); ParseElementList( doc, "DestinationDisplay", LoadDestination );
ParseElementList( doc, "ScheduledStopPoint", LoadStop ); ParseElementList( doc, "ScheduledStopPoint", LoadStop );
ParseElementList( doc, "ServiceJourneyPattern", LoadJourneyPattern ); ParseElementList( doc, "ServiceJourneyPattern", LoadJourneyPattern );
/// ParseElementList( doc, "ServiceJourney", LoadJourneyCode ); ParseElementList( doc, "ServiceJourney", LoadJourneyCode );
} }
void LoadRoutePoint( XmlElement elem ) void LoadRoutePoint( XmlElement elem )
...@@ -245,7 +244,6 @@ namespace OVAPI.Code.OVParsers ...@@ -245,7 +244,6 @@ namespace OVAPI.Code.OVParsers
line.transportType = elem.GetChild( "TransportMode" ).InnerText; line.transportType = elem.GetChild( "TransportMode" ).InnerText;
line.linePlanningNumber = elem.GetChild( "PrivateCode" ).InnerText; line.linePlanningNumber = elem.GetChild( "PrivateCode" ).InnerText;
lines.Add( id, line ); lines.Add( id, line );
privLineNumberToLine.Add( string.Join( ':', activeOwner, line.linePlanningNumber ), line );
} }
catch (Exception e) { Console.WriteLine( e.Message ); } catch (Exception e) { Console.WriteLine( e.Message ); }
} }
...@@ -312,7 +310,6 @@ namespace OVAPI.Code.OVParsers ...@@ -312,7 +310,6 @@ namespace OVAPI.Code.OVParsers
{ {
var journey = new JourneyPattern(); var journey = new JourneyPattern();
journey.route = routes[elem.GetChild( "RouteRef" ).GetAttribute( "ref" )]; journey.route = routes[elem.GetChild( "RouteRef" ).GetAttribute( "ref" )];
journey.route.journey = journey;
var destElem = elem.GetChild( "DestinationDisplayRef" ); var destElem = elem.GetChild( "DestinationDisplayRef" );
if (destElem != null) if (destElem != null)
{ {
...@@ -360,7 +357,19 @@ namespace OVAPI.Code.OVParsers ...@@ -360,7 +357,19 @@ namespace OVAPI.Code.OVParsers
string journeyNumb = elem.GetChild( "PrivateCode" ).InnerText; string journeyNumb = elem.GetChild( "PrivateCode" ).InnerText;
string journeyRef = elem.GetChild( "ServiceJourneyPatternRef" ).GetAttribute("ref"); string journeyRef = elem.GetChild( "ServiceJourneyPatternRef" ).GetAttribute("ref");
var key = string.Join( ':', activeOwner, journeyNumb ); var key = string.Join( ':', activeOwner, journeyNumb );
journeyNumbersToPattern.TryAdd( key, journeyPatterns[journeyRef] ); if (!journeyNumbersToPattern.TryGetValue( key, out var mapOfLinesToPatterns ))
{
mapOfLinesToPatterns = new Dictionary<string, List<JourneyPattern>>();
journeyNumbersToPattern.Add( key, mapOfLinesToPatterns );
}
var journeyPattern = journeyPatterns[journeyRef];
var pviLineNumb = journeyPattern.line.linePlanningNumber;
if (!mapOfLinesToPatterns.TryGetValue( pviLineNumb , out var listOfPatterns))
{
listOfPatterns = new List<JourneyPattern>();
mapOfLinesToPatterns.Add( pviLineNumb, listOfPatterns );
}
listOfPatterns.Add( journeyPattern );
} }
public void LoadAll( string folder ) public void LoadAll( string folder )
...@@ -405,8 +414,8 @@ namespace OVAPI.Code.OVParsers ...@@ -405,8 +414,8 @@ namespace OVAPI.Code.OVParsers
if (link.points.Count < 2) if (link.points.Count < 2)
{ {
route.points.Add( (new Vector2( link.from.rdx, link.from.rdy ), 0, Vector2.Zero) ); route.points.Add( (new Vector2( link.from.rdx, link.from.rdy ), 0, Vector2.Zero) );
if (i == route.links.Count-1) if (i == route.links.Count-1) // 'To' of a link is same as the 'from' from previous link, so only on last link, add the 'to'.
route.points.Add( (new Vector2( link.to.rdx, link.from.rdy ), 0, Vector2.Zero) ); route.points.Add( (new Vector2( link.to.rdx, link.to.rdy ), 0, Vector2.Zero) );
} }
else else
{ {
...@@ -433,21 +442,22 @@ namespace OVAPI.Code.OVParsers ...@@ -433,21 +442,22 @@ namespace OVAPI.Code.OVParsers
var p1 = route.points[j]; var p1 = route.points[j];
var dir = p1.point-p0.point; var dir = p1.point-p0.point;
var len = dir.Length(); var len = dir.Length();
if (len < 0.2f) if (len <= 0.5f)
{ {
route.points.RemoveAt( j ); route.points.RemoveAt( j );
j--; j--;
} }
else else
{ {
Debug.Assert( dir.Length() > 0.3f ); Debug.Assert( len > 0.5f );
Debug.Assert( MathF.Abs( (dir / dir.Length()).Length() - 1 )< 0.1f ); dir /= len;
route.points[i] = (p0.point, len, dir/dir.Length()); Debug.Assert( MathF.Abs( Vector2.Dot(dir, dir)-1) < 0.01f ); // Ensure normalized.
route.points[i] = (p0.point, len, dir);
break; break;
} }
} }
} }
if (route.points.Count>=2) if (route.points.Count>=2) // Set last point direction to previous.
{ {
var lastElem = route.points[route.points.Count-1]; var lastElem = route.points[route.points.Count-1];
lastElem.dir = route.points[route.points.Count-2].dir; lastElem.dir = route.points[route.points.Count-2].dir;
...@@ -456,7 +466,6 @@ namespace OVAPI.Code.OVParsers ...@@ -456,7 +466,6 @@ namespace OVAPI.Code.OVParsers
} }
} }
// Compute dist and direction from point to point in route.
foreach (var kvp in routes) foreach (var kvp in routes)
{ {
var route = kvp.Value; var route = kvp.Value;
...@@ -475,13 +484,15 @@ namespace OVAPI.Code.OVParsers ...@@ -475,13 +484,15 @@ namespace OVAPI.Code.OVParsers
} }
// Network overrides // Network overrides
public IJourney FindJourney( string owner, string privLineNumber ) public IJourney FindJourney( string owner, string journeyNumb, string privLineNumber )
{ {
string key = string.Join(':', owner, privLineNumber); string key = string.Join(':', owner, journeyNumb);
if(privLineNumberToLine.TryGetValue( key, out var line ) ) if (journeyNumbersToPattern.TryGetValue( key, out var lineToJourneys ) )
{ {
if (line.route.valid) if (lineToJourneys.TryGetValue( privLineNumber, out var listOfJourneys ))
return line.route.journey; {
return listOfJourneys[0];
}
} }
return null; return null;
} }
......
...@@ -67,77 +67,129 @@ namespace WanderOV ...@@ -67,77 +67,129 @@ namespace WanderOV
public interface INetwork public interface INetwork
{ {
public IJourney FindJourney( string owner, string privLineNumber ); public IJourney FindJourney( string owner, string journeyNumber, string privLineNumber );
float GetT( float t, float alpha, Vector2 p0, Vector2 p1 )
{
var d = p1 - p0;
float a = Vector2.Dot(d, d);
float b = MathF.Pow( a, alpha*0.5f );
return (b + t);
}
Vector2 CatmullRom( Vector2 p0, Vector2 p1, Vector2 p2, Vector2 p3, float t /* between 0 and 1 */, float alpha = .5f /* between 0 and 1 */ )
{
float t0 = 0.0f;
float t1 = GetT( t0, alpha, p0, p1 );
float t2 = GetT( t1, alpha, p1, p2 );
float t3 = GetT( t2, alpha, p2, p3 );
t = Vector2.Lerp( new Vector2( t1, 0 ), new Vector2( t2, 0 ), t ).X;
var A1 = ( t1-t )/( t1-t0 )*p0 + ( t-t0 )/( t1-t0 )*p1;
var A2 = ( t2-t )/( t2-t1 )*p1 + ( t-t1 )/( t2-t1 )*p2;
var A3 = ( t3-t )/( t3-t2 )*p2 + ( t-t2 )/( t3-t2 )*p3;
var B1 = ( t2-t )/( t2-t0 )*A1 + ( t-t0 )/( t2-t0 )*A2;
var B2 = ( t3-t )/( t3-t1 )*A2 + ( t-t1 )/( t3-t1 )*A3;
var C = ( t2-t )/( t2-t1 )*B1 + ( t-t1 )/( t2-t1 )*B2;
return C;
}
// Given a vehicle's position and journey interpolate the postion along the route in the journey. // Given a vehicle's position and journey interpolate the postion along the route in the journey.
// Returns false, if at end of journey. // Returns false, if at end of journey.
// The returned interpolant.point should be the input for the next request. While the pntCatMull,
// countains a more smoothish output of the request. The angle of the interpolant is also based on the catMull tangent.
// If advanceDistance == 0, then it only finds the point inside the interpolant. Dist and direction rare not valid.
public bool FindPositionInJourney( public bool FindPositionInJourney(
IJourney journey, IJourney journey,
ref int prevPositionIdx, ref int prevPointIndex,
Vector2 oldPosition, Vector2 oldPosition,
float advanceDistance, float advanceDistance,
out (Vector2 pnt, float dst, Vector2 dir) interpolant out (Vector2 pnt, float dst, Vector2 dir) interpolant,
out Vector2 pntCatMull
) )
{ {
// If already at end, just return last point. // If already at end, just return last point.
var route = journey.Route; var route = journey.Route;
if (prevPositionIdx == route.NumPoints-1) if (prevPointIndex == route.NumPoints-1)
{ {
interpolant = route.Point(route.NumPoints-1); interpolant = route.Point(route.NumPoints-1);
pntCatMull = interpolant.pnt;
return false; return false;
} }
// Find closest point on route. // Find closest point on route.
if (prevPositionIdx < 0) if (prevPointIndex < 0)
{ {
float closestDist = 1e30f; float closestDist = 1e30f;
for (int i = 0;i< route.NumPoints;i++) for (int i = 0;i< route.NumPoints;i++)
{ {
var p0 = route.Point(i); var p00 = route.Point(i);
var distSq = Vector2.DistanceSquared(oldPosition, p0.point); var distSq = Vector2.DistanceSquared(oldPosition, p00.point);
if (distSq < closestDist) if (distSq < closestDist)
{ {
closestDist = distSq; closestDist = distSq;
prevPositionIdx = i; prevPointIndex = i;
} }
} }
Debug.Assert( prevPositionIdx >= 0 ); Debug.Assert( prevPointIndex >= 0 );
} }
// Find point in between two points (interpolant/startPoint). // Find point in between two points (interpolant/startPoint).
Vector2 startPoint; Vector2 startPoint;
int nextPointIdx; int nextPointIdx;
if (prevPositionIdx == 0) if (prevPointIndex == 0)
{ {
nextPointIdx = 1; nextPointIdx = 1;
GetPointInSegment( oldPosition, route.Point(0).point, route.Point(1).point, out startPoint ); GetPointInSegment( oldPosition, route.Point(0).point, route.Point(1).point, out startPoint );
} }
else if (prevPositionIdx == route.NumPoints-1) else if (prevPointIndex == route.NumPoints-1)
{ {
nextPointIdx = route.NumPoints-1; nextPointIdx = route.NumPoints-1;
GetPointInSegment( oldPosition, route.Point( route.NumPoints-2 ).point, route.Point( route.NumPoints-1 ).point, out startPoint ); GetPointInSegment( oldPosition, route.Point( route.NumPoints-2 ).point, route.Point( route.NumPoints-1 ).point, out startPoint );
} }
else else
{ {
nextPointIdx = prevPositionIdx; nextPointIdx = prevPointIndex+1;
if (GetPointInSegment( oldPosition, route.Point( nextPointIdx-1 ).point, route.Point( nextPointIdx ).point, out startPoint ) == 1) while (GetPointInSegment( oldPosition, route.Point( prevPointIndex ).point, route.Point( nextPointIdx ).point, out _ ) == 1)
{ {
nextPointIdx = prevPositionIdx+1; prevPointIndex++;
GetPointInSegment( oldPosition, route.Point( nextPointIdx-1 ).point, route.Point( nextPointIdx ).point, out startPoint ); nextPointIdx++;
if ( prevPointIndex >= route.NumPoints-1 )
{
interpolant = route.Point( route.NumPoints-1 );
pntCatMull = interpolant.pnt;
return false;
}
}
while (GetPointInSegment( oldPosition, route.Point( prevPointIndex ).point, route.Point( nextPointIdx ).point, out startPoint ) == -1)
{
prevPointIndex--;
nextPointIdx--;
if (prevPointIndex == 0)
{
break;
}
} }
} }
if ( advanceDistance == 0 )
{
interpolant = (startPoint, 0, route.Point( prevPointIndex ).dir);
}
var prevPoint = startPoint; var prevPoint = startPoint;
float distToNextPoint = (route.Point( nextPointIdx ).point-startPoint).Length(); float distToNextPoint = (route.Point( nextPointIdx ).point-startPoint).Length();
float movedDistance = 0; float movedDistance = 0;
while (advanceDistance > distToNextPoint ) while (advanceDistance > distToNextPoint )
{ {
if (nextPointIdx == route.NumPoints-1) if (nextPointIdx == route.NumPoints-1)
{ {
// We also know that (advanceDistance is > distToNextPoint), so we set interpolant to last point. // We also know that (advanceDistance is > distToNextPoint), so we set interpolant to last point.
prevPositionIdx = nextPointIdx; prevPointIndex = nextPointIdx;
interpolant = route.Point( nextPointIdx ); // Last point.dir is copied from second last point. interpolant = route.Point( nextPointIdx ); // Last point.dir is copied from second last point.
pntCatMull = interpolant.pnt;
interpolant.dst = movedDistance+distToNextPoint; interpolant.dst = movedDistance+distToNextPoint;
return false; return false;
} }
...@@ -148,10 +200,30 @@ namespace WanderOV ...@@ -148,10 +200,30 @@ namespace WanderOV
nextPointIdx++; nextPointIdx++;
} }
Debug.Assert(advanceDistance <= distToNextPoint); Debug.Assert(advanceDistance <= distToNextPoint);
var endPoint = prevPoint + route.Point(nextPointIdx-1).dir*advanceDistance; prevPointIndex = nextPointIdx-1;
interpolant = (endPoint, movedDistance+advanceDistance, route.Point(nextPointIdx-1).dir ); var endPoint = prevPoint + route.Point(prevPointIndex).dir*advanceDistance;
float t = (endPoint-route.Point(prevPointIndex).point).Length() / route.Point(prevPointIndex).dist;
prevPositionIdx = nextPointIdx-1; Vector2 p0, p3;
if (nextPointIdx-2<0)
{
p0 = route.Point( 0 ).point - route.Point( 0 ).dir;
}
else
{
p0 = route.Point( nextPointIdx-2 ).point;
}
if (nextPointIdx+1==route.NumPoints)
{
p3 = route.Point( route.NumPoints -1 ).point + route.Point( route.NumPoints -1 ).dir;
}
else
{
p3 = route.Point( nextPointIdx+1 ).point;
}
pntCatMull = CatmullRom( p0, route.Point(prevPointIndex).point, route.Point(nextPointIdx).point, p3, t, 0.1f );
var ctmPnt2 = CatmullRom( p0, route.Point(prevPointIndex).point, route.Point(nextPointIdx).point, p3, t+0.1f, 0.1f );
interpolant = (endPoint, movedDistance+advanceDistance, Vector2.Normalize ( ctmPnt2-pntCatMull ) );
Debug.Assert( MathF.Abs( interpolant.dir.Length()-1 ) < 0.001f );
return true; return true;
} }
......
...@@ -79,7 +79,7 @@ class App ...@@ -79,7 +79,7 @@ class App
break; break;
case "journey": // Expects owner, journeyNumber and vehicleNumber. case "journey": // Expects owner, journeyNumber and vehicleNumber.
var journey = network.FindJourney( msg.fields[1], msg.fields[4] ); var journey = network.FindJourney( msg.fields[1], msg.fields[2], msg.fields[4] );
if (journey!=null) if (journey!=null)
{ {
// field[1] is owner, field[2] is journeyNumber, field[3] is vehicleNumber // field[1] is owner, field[2] is journeyNumber, field[3] is vehicleNumber
......
...@@ -17,7 +17,7 @@ namespace OVApI ...@@ -17,7 +17,7 @@ namespace OVApI
netex.LoadAll( "_database" ); netex.LoadAll( "_database" );
netex.PrintStats(); netex.PrintStats();
var journey = netex.FindJourney( "HTM", "36005" ); var journey = netex.FindJourney( "HTM", "36005", "5" );
INetwork network = netex; INetwork network = netex;
int prevPos = -1; int prevPos = -1;
...@@ -34,7 +34,8 @@ namespace OVApI ...@@ -34,7 +34,8 @@ namespace OVApI
float totDist = 0; float totDist = 0;
(Vector2 pnt, float dst, Vector2 dir) interpolant; (Vector2 pnt, float dst, Vector2 dir) interpolant;
while ( network.FindPositionInJourney( journey, ref prevPos, prevPoint, advDist, out interpolant) ) Vector2 catmullPnt;
while ( network.FindPositionInJourney( journey, ref prevPos, prevPoint, advDist, out interpolant, out catmullPnt) )
{ {
var len = interpolant.dir.Length(); var len = interpolant.dir.Length();
Debug.Assert( Netex.Sane( len ) ); Debug.Assert( Netex.Sane( len ) );
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment