public static Point ProjectOnSegment(this Point pnt, Point segStart, Point segEnd) { Contract.Ensures((pnt - Contract.Result<Point>()).LengthSquared <= (pnt - segStart).LengthSquared); Contract.Ensures((pnt - Contract.Result<Point>()).LengthSquared <= (pnt - segEnd).LengthSquared); var v = segEnd - segStart; if (v.LengthSquared <= double.Epsilon) // segment is of length almost zero. Therefore any point is valid. return segStart; var u = pnt - segStart; var t = (u * v) / (v * v); if (t < 0) // to the "left" of segStart return segStart; else if (t > 1) // to the "right" of segEnd return segEnd; else // between segStart and segEnd { // the point segStart + t * v is still considered a candidate because of a numerical error that can occur // in the computation of "t". So we still need to choose the point with minimal distance to "pnt". // We do it to ensure the contract above is correct - that is, we find the closest point to "pnt" on the segment. var candidate = segStart + t * v; var potentialResults = new Tuple<Point, double>[] { Tuple.Create(candidate, (candidate - pnt).LengthSquared), Tuple.Create(segStart, (segStart - pnt).LengthSquared), Tuple.Create(segEnd, (segEnd - pnt).LengthSquared), }; return potentialResults.Minimizer(x => x.Item2).Item1; } }