diff --git a/mono/Assistance/Geometry.cs b/mono/Assistance/Geometry.cs index 2c7835b..6290b73 100644 --- a/mono/Assistance/Geometry.cs +++ b/mono/Assistance/Geometry.cs @@ -2,7 +2,7 @@ using System; namespace Assistance { public static class Geometry { - public static readonly double precision = 0.01; + public static readonly double precision = 1e-8; public static readonly double sqrt2Pi = Math.Sqrt(2.0*Math.PI); public static double logNormalDistribuitionUnscaled(double x, double x0, double w) { diff --git a/mono/Assistance/Guideline.cs b/mono/Assistance/Guideline.cs index 2695fa3..4f731b6 100644 --- a/mono/Assistance/Guideline.cs +++ b/mono/Assistance/Guideline.cs @@ -21,24 +21,29 @@ namespace Assistance { } public double calcTrackWeight(Track track) { - if (track.points.Count < 2) + if (track.points.Count < 1) return double.PositiveInfinity; double sumWeight = 0.0; double sumLength = 0.0; double sumDeviation = 0.0; - for(int i = 1; i < track.points.Count; ++i) { - Point point = track.points[i]; - - double length = (point - track.points[i - 1]).len(); + Point prev = track.points[0]; + foreach(Point p in track.points) { + double length = (p - prev).len(); sumLength += length; - double weight = Geometry.logNormalDistribuitionUnscaled(sumLength, snapLenght, snapScale); - sumWeight += weight; + double midStepLength = sumLength - 0.5*length; + if (midStepLength > Geometry.precision) { + double weight = length*Geometry.logNormalDistribuitionUnscaled(midStepLength, snapLenght, snapScale); + sumWeight += weight; - double deviation = (transformPoint(point) - point).len(); - sumDeviation += weight*deviation; + double deviation = (transformPoint(p) - p).len(); + sumDeviation += weight*deviation; + } + prev = p; } + if (sumWeight < Geometry.precision) + return double.PositiveInfinity; return sumDeviation/sumWeight; }