SERVER-14192 Add circle / polygon containment and intersection test

5 files changed, 408 insertions, 10 deletions

diff --git a/src/mongo/db/geo/geoquery.cpp b/src/mongo/db/geo/geoquery.cpp
index 91de6b64f1e..f4f3fbff351 100644
--- a/src/mongo/db/geo/geoquery.cpp
+++ b/src/mongo/db/geo/geoquery.cpp
@@ -343,14 +343,25 @@ namespace mongo {
             if (box.inside(other._min) && box.inside(other._max)) {
                 return true;
             }
-        } else if (_geometry->_cap && FLAT == _geometry->_cap->crs) {
+        }
+
+        // Fast bounds check
+        if (!_bounds.contains(other)) return false;
+
+        if ( _geometry->_cap && FLAT == _geometry->_cap->crs ) {
             const Circle& circle = _geometry->_cap->circle;
             const Point& a = other._min;
             const Point& b = other._max;
-            return distanceWithin(circle.center, a, circle.radius)
-                    && distanceWithin(circle.center, b, circle.radius)
-                    && distanceWithin(circle.center, Point(a.x, b.y), circle.radius)
-                    && distanceWithin(circle.center, Point(b.x, a.y), circle.radius);
+            return distanceWithin( circle.center, a, circle.radius )
+                && distanceWithin( circle.center, b, circle.radius )
+                && distanceWithin( circle.center, Point( a.x, b.y ), circle.radius )
+                && distanceWithin( circle.center, Point( b.x, a.y ), circle.radius );
+        }
+
+        if (_geometry->_polygon && FLAT == _geometry->_polygon->crs) {
+            const Polygon& polygon = _geometry->_polygon->oldPolygon;
+            // Exact test
+            return polygonContainsBox(polygon, other);
         }
 
         // Not sure
@@ -358,10 +369,21 @@ namespace mongo {
     }
 
     bool GeometryContainer::R2BoxRegion::fastDisjoint(const Box& other) const {
-
         if (!_bounds.intersects(other))
             return true;
 
+        if (_geometry->_cap && FLAT == _geometry->_cap->crs) {
+            const Circle& circle = _geometry->_cap->circle;
+            // Exact test
+            return !circleIntersectsWithBox(circle, other);
+        }
+
+        if (_geometry->_polygon && FLAT == _geometry->_polygon->crs) {
+            const Polygon& polygon = _geometry->_polygon->oldPolygon;
+            // Exact test
+            return !polygonIntersectsWithBox(polygon, other);
+        }
+
         // Not sure
         return false;
     }
diff --git a/src/mongo/db/geo/r2_region_coverer.cpp b/src/mongo/db/geo/r2_region_coverer.cpp
index 484331acdf9..e602123d878 100644
--- a/src/mongo/db/geo/r2_region_coverer.cpp
+++ b/src/mongo/db/geo/r2_region_coverer.cpp
@@ -175,7 +175,7 @@ namespace mongo {
             // at the same level, we prefer the cells with the smallest number of
             // intersecting children.  Finally, we prefer cells that have the smallest
             // number of children that cannot be refined any further.
-            int priority = -((((candidate->cell.getBits() << 4)
+            int priority = -(((((int)candidate->cell.getBits() << 4)
                               + candidate->numChildren) << 4)
                               + numTerminals);
             _candidateQueue->push(make_pair(priority, candidate)); // queue owns candidate
diff --git a/src/mongo/db/geo/r2_region_coverer_test.cpp b/src/mongo/db/geo/r2_region_coverer_test.cpp
index bc33660fe87..87a87edbd4e 100644
--- a/src/mongo/db/geo/r2_region_coverer_test.cpp
+++ b/src/mongo/db/geo/r2_region_coverer_test.cpp
@@ -33,10 +33,11 @@
 #include "mongo/bson/bsonmisc.h"
 #include "mongo/db/geo/geoquery.h" // TODO: Move GeometryContainer out of geoquery.h
 
-using namespace mongo;
-
 namespace {
 
+    using namespace mongo;
+    using mongo::Polygon; // "windows.h" has another Polygon for Windows GDI.
+
     //
     // GeoHash
     //
@@ -269,6 +270,254 @@ namespace {
                coverer.getCovering(region, &covering);
                checkCovering(converter, region, coverer, covering);
            }
-       }
+    }
+
+    //
+    // Shape Intersection
+    //
+    TEST(ShapeIntersection, Lines) {
+        /*
+         *    E     |D
+         *  A___B   |C   G
+         *    F
+         */
+        Point a(0, 0), b(1, 0), c(2, 0), d(2, 1);
+        Point e(0.5, 1), f(0.5, -0.5), g(3, 0);
+
+        /*
+         * Basic disjoint
+         *   / |
+         *  /  |
+         */
+        ASSERT_FALSE(linesIntersect(a, d, c, b));
+        ASSERT_FALSE(linesIntersect(c, b, a, d)); // commutative
+
+        /*
+         * Basic disjoint (axis aligned)
+         *     |
+         * ___ |
+         */
+        ASSERT_FALSE(linesIntersect(a, b, c, d));
+        ASSERT_FALSE(linesIntersect(c, d, a, b)); // commutative
+
+        /*
+         * Basic intersection
+         * \/
+         * /\
+         */
+        ASSERT_TRUE(linesIntersect(e, c, f, d));
+        ASSERT_TRUE(linesIntersect(f, d, e, c)); // commutative
+
+        /*
+         * Basic intersection (axis aligned)
+         *  _|_
+         *   |
+         */
+        ASSERT_TRUE(linesIntersect(a, b, e, f));
+        ASSERT_TRUE(linesIntersect(f, e, b, a)); // commutative
+
+        /*
+         * One vertex on the line
+         *        \
+         *  ____   \
+         */
+        ASSERT_FALSE(linesIntersect(a, b, e, c));
+        ASSERT_FALSE(linesIntersect(e, c, a, b)); // commutative
+
+        /*
+         * One vertex on the segment
+         *    \
+         *  ___\___
+         */
+        ASSERT_TRUE(linesIntersect(a, c, b, e));
+        ASSERT_TRUE(linesIntersect(e, b, a, c)); // commutative
+
+        /*
+         * Two segments share one vertex
+         *    /
+         *   /____
+         */
+        ASSERT_TRUE(linesIntersect(a, c, a, e));
+        ASSERT_TRUE(linesIntersect(a, e, a, c)); // commutative
+
+        /*
+         * Intersected segments on the same line
+         * A___B===C---G
+         */
+        ASSERT_TRUE(linesIntersect(a, c, b, g));
+        ASSERT_TRUE(linesIntersect(b, g, c, a)); // commutative
+
+        /*
+         * Disjoint segments on the same line
+         * A___B   C---G
+         */
+        ASSERT_FALSE(linesIntersect(a, b, c, g));
+        ASSERT_FALSE(linesIntersect(c, g, a, b)); // commutative
+
+        /*
+         * Segments on the same line share one vertex.
+         *        /D
+         *       /B
+         *     F/
+         */
+        ASSERT_TRUE(linesIntersect(d, b, b, f));
+        ASSERT_TRUE(linesIntersect(f, b, d, b)); // commutative
+        // axis aligned
+        ASSERT_TRUE(linesIntersect(a, c, g, c));
+        ASSERT_TRUE(linesIntersect(c, g, a, c)); // commutative
+    }
+
+    TEST(ShapeIntersection, Polygons) {
+        // Convex polygon (triangle)
+
+        /*
+         * Disjoint, bounds disjoint
+         *        /|
+         *       / |  []
+         *      /__|
+         */
+        vector<Point> triangleVetices;
+        triangleVetices.push_back(Point(0, 0));
+        triangleVetices.push_back(Point(1, 0));
+        triangleVetices.push_back(Point(1, 4));
+        Polygon triangle(triangleVetices);
+        Box box;
+
+        box = Box(1.5, 1.5, 1);
+        ASSERT_FALSE(edgesIntersectsWithBox(triangle.points(), box));
+        ASSERT_FALSE(polygonIntersectsWithBox(triangle, box));
+        ASSERT_FALSE(polygonContainsBox(triangle, box));
+
+        /*
+         * Disjoint, bounds intersect
+         *     [] /|
+         *       / |
+         *      /__|
+         */
+        box = Box(-0.5, 3.5, 1);
+        ASSERT_FALSE(edgesIntersectsWithBox(triangle.points(), box));
+        ASSERT_FALSE(polygonIntersectsWithBox(triangle, box));
+        ASSERT_FALSE(polygonContainsBox(triangle, box));
+
+        /*
+         * Intersect on one polygon vertex
+         *      _____
+         *     |     |
+         *     |_ /|_|
+         *       / |
+         *      /__|
+         */
+        box = Box(0, 3, 2);
+        ASSERT_TRUE(edgesIntersectsWithBox(triangle.points(), box));
+        ASSERT_TRUE(polygonIntersectsWithBox(triangle, box));
+        ASSERT_FALSE(polygonContainsBox(triangle, box));
+
+        /*
+         * Box contains polygon
+         *   __________
+         *  |          |
+         *  |     /|   |
+         *  |    / |   |
+         *  |   /__|   |
+         *  |__________|
+         */
+        box = Box(-1, -1, 6);
+        ASSERT_FALSE(edgesIntersectsWithBox(triangle.points(), box));
+        ASSERT_TRUE(polygonIntersectsWithBox(triangle, box));
+        ASSERT_FALSE(polygonContainsBox(triangle, box));
+
+        /*
+         * Polygon contains box
+         *        /|
+         *       / |
+         *      /  |
+         *     / []|
+         *    /____|
+         */
+        box = Box(0.1, 0.1, 0.2);
+        ASSERT_FALSE(edgesIntersectsWithBox(triangle.points(), box));
+        ASSERT_TRUE(polygonIntersectsWithBox(triangle, box));
+        ASSERT_TRUE(polygonContainsBox(triangle, box));
+
+        /*
+         * Intersect, but no vertex is contained by the other shape.
+         *    ___ /|_
+         *   |   / | |
+         *   |  /  | |
+         *   |_/___|_|
+         *    /____|
+         */
+        box = Box(0, 1, 2);
+        ASSERT_TRUE(edgesIntersectsWithBox(triangle.points(), box));
+        ASSERT_TRUE(polygonIntersectsWithBox(triangle, box));
+        ASSERT_FALSE(polygonContainsBox(triangle, box));
+
+        // Concave polygon
+
+        /*
+         * (0,4)
+         * |\
+         * | \(1,1)
+         * |  `.
+         * |____`. (4,0)
+         * (0,0)
+         */
+        vector<Point> concaveVetices;
+        concaveVetices.push_back(Point(0, 0));
+        concaveVetices.push_back(Point(4, 0));
+        concaveVetices.push_back(Point(1, 1));
+        concaveVetices.push_back(Point(0, 4));
+        Polygon concave(concaveVetices);
+
+        /*
+         * Disjoint
+         * |\
+         * | \
+         * |  `.
+         * |____`.
+         *   []
+         */
+        box = Box(1, -1, 0.9);
+        ASSERT_FALSE(edgesIntersectsWithBox(concave.points(), box));
+        ASSERT_FALSE(polygonIntersectsWithBox(concave, box));
+        ASSERT_FALSE(polygonContainsBox(concave, box));
+
+        /*
+         * Disjoint, bounds intersect
+         * |\
+         * | \[]
+         * |  `.
+         * |____`.
+         */
+        box = Box(1.1, 1.1, 0.2);
+        ASSERT_FALSE(edgesIntersectsWithBox(concave.points(), box));
+        ASSERT_FALSE(polygonIntersectsWithBox(concave, box));
+        ASSERT_FALSE(polygonContainsBox(concave, box));
+
+        /*
+         * Intersect, one box vertex is contained by the polygon.
+         *  |\
+         *  |+\+ (1.5, 1.5)
+         *  |+-`.
+         *  |____`.
+         */
+        box = Box(0.5, 0.5, 1);
+        ASSERT_TRUE(edgesIntersectsWithBox(concave.points(), box));
+        ASSERT_TRUE(polygonIntersectsWithBox(concave, box));
+        ASSERT_FALSE(polygonContainsBox(concave, box));
+
+        /*
+         * Intersect, no vertex is contained by the other shape.
+         *  |\
+         * +| \--+
+         * ||  `.|
+         * ||____`.
+         * +-----+
+         */
+        box = Box(-0.5, -0.5, 3);
+        ASSERT_TRUE(edgesIntersectsWithBox(concave.points(), box));
+        ASSERT_TRUE(polygonIntersectsWithBox(concave, box));
+        ASSERT_FALSE(polygonContainsBox(concave, box));
+    }
 
 } // namespace
diff --git a/src/mongo/db/geo/shapes.cpp b/src/mongo/db/geo/shapes.cpp
index efb6ee1c05b..b0562077fee 100644
--- a/src/mongo/db/geo/shapes.cpp
+++ b/src/mongo/db/geo/shapes.cpp
@@ -486,4 +486,121 @@ namespace mongo {
         return sqrt((a * a) + (b * b));
     }
 
+    static inline Vector2_d toVector2(const Point& p) {
+        return Vector2_d(p.x, p.y);
+    }
+
+    // Given a segment (A, B) and a segment (C, D), check whether they intersect.
+    bool linesIntersect(const Point& pA, const Point& pB, const Point& pC, const Point& pD) {
+        Vector2_d a = toVector2(pA);
+        Vector2_d b = toVector2(pB);
+        Vector2_d c = toVector2(pC);
+        Vector2_d d = toVector2(pD);
+
+        // The normal of line AB
+        Vector2_d normalAB = (b - a).Ortho();
+
+        // Dot products of AC and the normal of AB
+        // = 0 : C is on the line AB
+        // > 0 : C is on one side
+        // < 0 : C is on the other side
+        double dotProdNormalAB_AC = normalAB.DotProd(c - a);
+        double dotProdNormalAB_AD = normalAB.DotProd(d - a);
+
+        // C and D can not on the same side of line AB
+        if (dotProdNormalAB_AC * dotProdNormalAB_AD > 0) return false;
+
+        // AB and CD are on the same line
+        if (dotProdNormalAB_AC == 0 && dotProdNormalAB_AD == 0) {
+            // Test if C or D is on segment AB.
+            return (c - a).DotProd(c - b) <= 0 || (d - a).DotProd(d - b) <= 0;
+        }
+
+        // Check if A and B are on different sides of line CD.
+        Vector2_d normalCD = (d - c).Ortho();
+        double dotProdNormalCD_CA = normalCD.DotProd(a - c);
+        double dotProdNormalCD_CB = normalCD.DotProd(b - c);
+        return dotProdNormalCD_CA * dotProdNormalCD_CB <= 0; // Perhaps A or B is on line CD
+    }
+
+    // Check the intersection by measuring the distance between circle center and box center.
+    bool circleIntersectsWithBox(const Circle& circle, const Box& box) {
+        /* Collapses the four quadrants down into one.
+         *   ________
+         * r|___B___ \  <- a quarter round corner here. Let's name it "D".
+         *  |       | |
+         * h|       | |
+         *  |   A   |C|
+         *  |_______|_|
+         *      w    r
+         */
+
+        Point boxCenter = box.center();
+        double dx = abs(circle.center.x - boxCenter.x);
+        double dy = abs(circle.center.y - boxCenter.y);
+        double w = (box._max.x - box._min.x) / 2;
+        double h = (box._max.y - box._min.y) / 2;
+        const double& r = circle.radius;
+
+        // Check if circle.center is in A, B or C.
+        // The circle center could be above the box (B) or right to the box (C), but close enough.
+        if ((dx <= w + r && dy <= h) || (dx <= w && dy <= h + r)) return true;
+
+        // Now check if circle.center is in the round corner "D".
+        return distanceWithin(Point(dx, dy), Point(w, h), r);
+    }
+
+    bool lineIntersectsWithBox(const Point& a, const Point& b, const Box& box) {
+        Point upperLeft(box._min.x, box._max.y);
+        Point lowerRight(box._max.x, box._min.y);
+
+        return linesIntersect(a, b, upperLeft, box._min)
+                || linesIntersect(a, b, box._min, lowerRight)
+                || linesIntersect(a, b, lowerRight, box._max)
+                || linesIntersect(a, b, box._max, upperLeft);
+    }
+
+    // Doc: The last point specified is always implicitly connected to the first.
+    // [[ 0 , 0 ], [ 3 , 6 ], [ 6 , 0 ]]
+    bool edgesIntersectsWithBox(const vector<Point>& vertices, const Box& box) {
+        for (size_t i = 0; i < vertices.size() - 1; i++) {
+            if (lineIntersectsWithBox(vertices[i], vertices[i+1], box)) return true;
+        }
+        // The last point and first point.
+        return lineIntersectsWithBox(vertices[vertices.size() - 1], vertices[0], box);
+    }
+
+    bool polygonContainsBox(const Polygon& polygon, const Box& box) {
+        // All vertices of box have to be inside the polygon.
+        if (!polygon.contains(box._min)
+                || !polygon.contains(box._max)
+                || !polygon.contains(Point(box._min.x, box._max.y))
+                || !polygon.contains(Point(box._max.x, box._min.y)))
+            return false;
+
+        // No intersection between the polygon edges and the box.
+        return !edgesIntersectsWithBox(polygon.points(), box);
+    }
+
+    bool polygonIntersectsWithBox(const Polygon& polygon, const Box& box) {
+        // 1. Polygon contains the box.
+        // Check the relaxed condition that whether the polygon include any vertex of the box.
+        if (polygon.contains(box._min)
+                || polygon.contains(box._max)
+                || polygon.contains(Point(box._min.x, box._max.y))
+                || polygon.contains(Point(box._max.x, box._min.y)))
+            return true;
+
+        // 2. Box contains polygon.
+        // Check the relaxed condition that whether the box include any vertex of the polygon.
+        for (vector<Point>::const_iterator it = polygon.points().begin();
+                it != polygon.points().end(); it++) {
+            if (box.inside(*it)) return true;
+        }
+
+        // 3. Otherwise they intersect on a portion of both shapes.
+        // Edges intersects
+        return edgesIntersectsWithBox(polygon.points(), box);
+    }
+
 }  // namespace mongo
diff --git a/src/mongo/db/geo/shapes.h b/src/mongo/db/geo/shapes.h
index c03d907b0d7..99594520a1d 100644
--- a/src/mongo/db/geo/shapes.h
+++ b/src/mongo/db/geo/shapes.h
@@ -43,11 +43,20 @@
 namespace mongo {
 
     struct Point;
+    struct Circle;
+    class Box;
+    class Polygon;
+
     double distance(const Point& p1, const Point &p2);
     bool distanceWithin(const Point &p1, const Point &p2, double radius);
     void checkEarthBounds(const Point &p);
     double spheredist_rad(const Point& p1, const Point& p2);
     double spheredist_deg(const Point& p1, const Point& p2);
+    bool linesIntersect(const Point& pA, const Point& pB, const Point& pC, const Point& pD);
+    bool circleIntersectsWithBox(const Circle& circle, const Box& box);
+    bool edgesIntersectsWithBox(const vector<Point>& vertices, const Box& box);
+    bool polygonContainsBox(const Polygon& polygon, const Box& box);
+    bool polygonIntersectsWithBox(const Polygon& polygon, const Box& box);
 
     struct Point {
         Point();
@@ -130,6 +139,7 @@ namespace mongo {
          */
         const Point& centroid() const;
         const Box& bounds() const;
+        const std::vector<Point>& points() const { return _points; }
 
     private: