I'm a bit in dubio with
this answer. OP was responsive enough to edit their link-only answer to add the code. However, is this a "useful" answer? The question itself asks for an algorithm, in a language agnostic way (Question uses C# code, top answer does Java). This answer just drops a MATLAB code, refers to the top answer (in Java) for the explanation. It does have enough comments within the code to understand it without referring to the java ans