Compare 3D Coordinates with Tolerance

Question

Given two Lists of Coordinates (as double-triplets)

var reference = 
  new List<(double x, double y, double z)()
    {(10,10,10),
     (15,15,15)};

and a set of real-world coordinates, like

var coords =
  new List<(double x, double y, double z)()
    {(9.97,10.02,10),
     (15.01,14.98,15),
     (12.65,18.69,0)};

I needed the items of coords, where the deviation of the values is within +/-0.1, so the expected result was:

res = {coords[0], coords[1]} //resp. the items of course.

Both Lists can be as of several 1000 entries, so Where/Contains seems not be a good choice.

Answer 1

First you will need a function to compare distances. I'm going to use Vector3 instead of a value tuple since it is easier to write:

public bool IsAlmostEqual(Vector3 a, Vector3 b, float epsilon){
    return DistanceSquared(a, b) < (epsilon * epsilon)
}
public double DistanceSquared(Vector3 a, Vector3 b){
    var c = a - b;
    return c.x * c.x + c.y * c.y + c.z * c.z ;
}

If you just want to check the corresponding index for each coordinate you would simply write a loop:

for(var i = 0; i < coords.Count; i++){
    if(IsAlmostEqual(coords[i], reference[i], 0.1){
        ...
    }
    else{
        ...
    }

If you want to check each combination of coords and reference position you simply add another loop. This will not scale well, but 1000 items would result in only about 500000 iterations of the inner loop, and I would expect that to take about a millisecond.

If you need to process larger sets of coordinates you should look into some kind of search structure, like a KD-tree.

Answer 2

I think you are looking to find matches in a list of points that match with a target point. Alternatively you can match with all the points in a list.

First I created a general purpose comparison class ApproxEqual to compare collections and tuples.

Next I iterate the list to find matches in ApproxFind()

See example code below:

class Program
{
    static void Main(string[] args)
    {
        var actual_points = new List<(float, float, float)>()
        {
            (9.97f,10.02f,10),
            (15.01f,14.98f,15),
            (12.65f,18.69f,0),
            (10.03f,9.98f,10),
        };

        var target = (10f, 10f, 10f);
        var match = ApproxFind(actual_points, target, 0.1f);
        foreach (var item in match)
        {
            Console.WriteLine(item);
            // (9.97, 10.02, 10)
            // (10.03, 9.98, 10)
        }

        var targetAll = new[] { (10f, 10f, 10f), (15f, 15f, 15f) };
        var matchAll = ApproxFind(actual_points, targetAll , 0.1f);
        foreach (var item in matchAll)
        {
            Console.WriteLine(item);
            // (9.97, 10.02, 10)
            // (10.03, 9.98, 10)
            // (15.01, 14.98, 15)
        }
    }

    /// <summary>
    /// Find the items in <paramref name="list"/> that are near the <paramref name="target"/> by a tolerance <paramref name="tolerance"/>
    /// </summary>
    public static IEnumerable<(float, float, float)> ApproxFind(IEnumerable<(float, float, float)> list, (float, float, float) target, float tolerance)
    {
        var comp = new ApproxEqual(tolerance);
        foreach (var item in list)
        {
            if (comp.Compare(item, target) == 0)
            {
                yield return item;
            }
        }
    }
    /// <summary>
    /// Find the items in <paramref name="list"/> that are near any of the items in the <paramref name="targets"/> by a tolerance <paramref name="tolerance"/>
    /// </summary>
    public static IEnumerable<(float, float, float)> ApproxFind(IEnumerable<(float, float, float)> list, IEnumerable<(float, float, float)> targets, float tolerance)
    {
        var comp = new ApproxEqual(tolerance);
        foreach (var other in targets)
        {
            foreach (var item in list)
            {
                if (comp.Compare(item, other) == 0)
                {
                    yield return item;
                }
            }
        }
    }
}

/// <summary>
/// Implementation of approximate comparison for tuples and collections
/// </summary>
public class ApproxEqual : 
    IComparer<ICollection<float>>, 
    IComparer<ValueTuple<float,float,float>>,
    System.Collections.IComparer
{
    public ApproxEqual(float tolerance)
    {
        Tolerance = tolerance;
    }

    public float Tolerance { get; }

    int System.Collections.IComparer.Compare(object x, object y)
    {
        if (x is ICollection<float> x_arr && y is ICollection<float> y_arr)
        {
            return Compare(x_arr, y_arr);
        }
        if (x is ValueTuple<float, float, float> x_tuple && y is ValueTuple<float, float, float> y_tuple)
        {
            return Compare(x_tuple, y_tuple);
        }
        return -1;
    }

    public int Compare(ICollection<float> x, ICollection<float> y)
    {
        if (x.Count == y.Count)
        {
            foreach (var delta in x.Zip(y, (xi,yi)=>Math.Abs(xi-yi)))
            {
                if (delta > Tolerance) return -1;
            }
            return 0;
        }
        return -1;
    }

    public int Compare((float, float, float) x, (float, float, float) y)
    {
        if (Math.Abs(x.Item1 - y.Item1) > Tolerance) return -1;
        if (Math.Abs(x.Item2 - y.Item2) > Tolerance) return -1;
        if (Math.Abs(x.Item3 - y.Item3) > Tolerance) return -1;
        return 0;
    }
}

---

If you want to implement IEqualityComparer in ApproxEqual then add the following interfaces

    IEqualityComparer<ICollection<float>>,
    IEqualityComparer<ValueTuple<float,float,float>>,
    System.Collections.IEqualityComparer

and implement them with

    bool System.Collections.IEqualityComparer.Equals(object x, object y)
    {
        if (x is ICollection<float> x_arr && y is ICollection<float> y_arr)
        {
            return Equals(x_arr, y_arr);
        }
        if (x is ValueTuple<float, float, float> x_tuple && y is ValueTuple<float, float, float> y_tuple)
        {
            return Equals(x_tuple, y_tuple);
        }
        return false;
    }
    public bool Equals(ICollection<float> x, ICollection<float> y)
    {
        if (x.Count == y.Count)
        {
            return !x.Zip(y, (xi, yi) => Math.Abs(xi - yi)).Any((delta) => delta > Tolerance);
        }
        return false;
    }


    public bool Equals((float, float, float) x, (float, float, float) y)
    {
        return Math.Abs(x.Item1 - y.Item1)<=Tolerance
            && Math.Abs(x.Item2 - y.Item2)<=Tolerance
            && Math.Abs(x.Item3 - y.Item3)<=Tolerance;
    }

    int System.Collections.IEqualityComparer.GetHashCode(object obj)
    {
        if (obj is ValueTuple<float, float, float> x_tuple)
        {
            return GetHashCode(x_tuple);
        }
        if (obj is ICollection<float> x_arr)
        {
            GetHashCode(x_arr);
        }
        return obj.GetHashCode();
    }
    public int GetHashCode(ICollection<float> obj)
    {
        var array = obj.ToArray();
        unchecked
        {
            int hc = 17;
            for (int i = 0; i < array.Length; i++)
            {
                hc = 23 * hc + array[i].GetHashCode();
            }
            return hc;
        }
    }
    public int GetHashCode((float, float, float) obj)
    {
        return obj.GetHashCode();
    }

finally, change the ApproxFind() methods to use .Equals() instead of .Compare()

    /// <summary>
    /// Find the items in <paramref name="list"/> that are near the <paramref name="target"/> by a tolerance <paramref name="tolerance"/>
    /// </summary>
    public static IEnumerable<(float, float, float)> ApproxFind(IEnumerable<(float, float, float)> list, (float, float, float) target, float tolerance)
    {
        var comp = new ApproxEqual(tolerance);
        foreach (var item in list)
        {
            if (comp.Equals(item, target))
            {
                yield return item;
            }
        }
    }
    /// <summary>
    /// Find the items in <paramref name="list"/> that are near any of the items in the <paramref name="targets"/> by a tolerance <paramref name="tolerance"/>
    /// </summary>
    public static IEnumerable<(float, float, float)> ApproxFind(IEnumerable<(float, float, float)> list, IEnumerable<(float, float, float)> targets, float tolerance)
    {
        var comp = new ApproxEqual(tolerance);
        foreach (var other in targets)
        {
            foreach (var item in list)
            {
                if (comp.Equals(item, other) )
                {
                    yield return item;
                }
            }
        }
    }

---

PS. I am using floats because I wanted to use System.Numerics and got stuck with it. It is easy to convert the above to support double also.