N closest points to the reference point Here is working code to get N closest points to some reference point. 

Please help to improve it, specifically by commenting on my use of std algorithms and vectors/iterators manipulations.

Nearest.h

#pragma once

#include &lt;iostream&gt;
#include &lt;algorithm&gt;
#include &lt;cmath&gt;
#include &lt;vector&gt;

namespace nearest {

struct Point {
  double x;
  double y;
  double z;

  bool operator==(const Point&amp; rhp) {
    return x == rhp.x &amp;&amp; y == rhp.y &amp;&amp; z == rhp.z;
  }

};

// Return N closest points to first point in vector in order
std::vector&lt;Point&gt; nearestN(const std::vector&lt;Point&gt;&amp; points,
                            int N);


// Return N closest points to a reference point, in order - all
// within maximum distance threshold
std::vector&lt;Point&gt; nearestN(const std::vector&lt;Point&gt;&amp; points,
                            int N,
                            const Point&amp; reference,
                            double distanceThreshold);

}


Nearest.cpp

#include "Nearest.h"

using namespace std;

namespace nearest {

    // to print Point to cout
    std::ostream&amp; operator&lt;&lt;(std::ostream &amp;o, const nearest::Point&amp; p) {
        o &lt;&lt; "(" &lt;&lt; p.x &lt;&lt; ", " &lt;&lt; p.y &lt;&lt; ", " &lt;&lt; p.z &lt;&lt; ")" &lt;&lt; std::endl;
        return o;
    }

    // defines sorting order for points
    struct ByDistance {
        const Point&amp; origin;

        bool operator()(const Point&amp; lp, const Point&amp; rp) {
            return dist(origin, lp) &lt; dist(origin, rp);
        }

        static double dist(const Point&amp; lp, const Point&amp; rp){
            double xs = (lp.x - rp.x) * (lp.x - rp.x);
            double ys = (lp.y - rp.y) * (lp.y - rp.y);
            double zs = (lp.z - rp.z) * (lp.z - rp.z);
            return sqrt(xs + ys + zs);
        }
    };

    vector&lt;Point&gt; nearestN(const vector&lt;Point&gt;&amp; points, int N) {
        if (points.size() == 0) {
            vector&lt;Point&gt; empty;
            return empty;
        }
        return nearestN(points, N, points[0], INFINITY);
    }

    vector&lt;Point&gt; nearestN(const vector&lt;Point&gt;&amp; points,
                           int N,
                           const Point&amp; reference,
                           double distanceThreshold) {

        vector&lt;Point&gt; temp;
        temp.insert(temp.begin(), points.begin(), points.end());

        // filtering vector to remove all points far then distance from reference
        temp.erase(remove_if(temp.begin(), 
                             temp.end(),
                             [&amp;reference, distanceThreshold](Point&amp; p){ return ByDistance::dist(p, reference) &gt; distanceThreshold; }),
                   temp.end());

        ByDistance distance = {reference};
        sort(temp.begin(), temp.end(), distance);

        auto sz = min(static_cast&lt;int&gt;(temp.size()), N);
        vector&lt;Point&gt; result(sz);
        copy_n(temp.begin(), sz, result.begin());
        return result;
    }
}
Faster sorting
If the only purpose of the distance computation is for comparison, then you can speed it up by removing the call to sqrt().  You'll end up comparing the squares of the distances, which is equivalent to comparing the actual distances.