Fix some numerical problems that prevented some very short arcs that were colocated with the edges of a polygon from being recognized as inside the polygon.

git-svn-id: http://svn.stsci.edu/svn/ssb/stsci_python/stsci.sphere/trunk@32335 fe389314-cf27-0410-b35b-8c050e845b92

Former-commit-id: df6e3d7c90b358c56d4f84df17b19a742e405337

3 files changed, 111 insertions, 38 deletions

diff --git a/stsci/sphere/graph.py b/stsci/sphere/graph.py
index d8ccf75..0e5b1d3 100644
--- a/stsci/sphere/graph.py
+++ b/stsci/sphere/graph.py
@@ -124,9 +124,7 @@ class Graph:
                 emphirical test cases. Relative threshold based on
                 the actual sizes of polygons is not implemented.
             """
-            # return np.array_equal(self._point, other._point)
-            return great_circle_arc.length(self._point, other._point,
-                                           degrees=False) < thres
+            return np.array_equal(self._point, other._point)
 
 
     class Edge:
@@ -425,42 +423,72 @@ class Graph:
             import traceback
             traceback.print_exc()
             self._dump_graph(title=title)
+            import pdb
+            pdb.set_trace()
             raise
 
     def _dump_graph(self, title=None, lon_0=0, lat_0=90,
-                    projection='vandg', func=lambda x: len(x._edges)):
+                    projection='vandg', func=lambda x: len(x._edges), poly=None,
+                    edge=None, file=None, show=True):
         from mpl_toolkits.basemap import Basemap
         from matplotlib import pyplot as plt
         fig = plt.figure()
         m = Basemap()
 
+        in_edge = edge
+
         counts = {}
         for node in self._nodes:
             count = func(node)
             counts.setdefault(count, [])
             counts[count].append(list(node._point))
 
-        minx = np.inf
-        miny = np.inf
-        maxx = -np.inf
-        maxy = -np.inf
+        if poly is not None:
+            poly.draw(m, lw=20.0, color="black", alpha=0.3)
+
+        if edge is not None:
+            A, B = [x._point for x in edge._nodes]
+            r0, d0 = vector.vector_to_radec(A[0], A[1], A[2])
+            r1, d1 = vector.vector_to_radec(B[0], B[1], B[2])
+            x0, y0 = m(r0, d0)
+            x1, y1 = m(r1, d1)
+            m.drawgreatcircle(r0, d0, r1, d1, color="green", lw=10, alpha=0.5)
+            in_edge = None
+            minx = min(x0, x1)
+            maxx = max(x0, x1)
+            miny = min(y0, y1)
+            maxy = max(y0, y1)
+
+        for edge in list(self._edges):
+            count = getattr(edge, '_count', 0)
+            A, B = [x._point for x in edge._nodes]
+            r0, d0 = vector.vector_to_radec(A[0], A[1], A[2])
+            r1, d1 = vector.vector_to_radec(B[0], B[1], B[2])
+            if count:
+                m.drawgreatcircle(r0, d0, r1, d1, color="red", lw=2*count)
+            else:
+                m.drawgreatcircle(r0, d0, r1, d1, color="black", lw=0.5)
+
+        if in_edge is None:
+            minx = np.inf
+            miny = np.inf
+            maxx = -np.inf
+            maxy = -np.inf
         for k, v in counts.items():
             v = np.array(v)
             ra, dec = vector.vector_to_radec(v[:, 0], v[:, 1], v[:, 2])
             x, y = m(ra, dec)
             m.plot(x, y, 'o', label=str(k))
-            for x0 in x:
-                minx = min(x0, minx)
-                maxx = max(x0, maxx)
-            for y0 in y:
-                miny = min(y0, miny)
-                maxy = max(y0, maxy)
+            if in_edge is None:
+                for x0 in x:
+                    minx = min(x0, minx)
+                    maxx = max(x0, maxx)
+                for y0 in y:
+                    miny = min(y0, miny)
+                    maxy = max(y0, maxy)
 
-        for edge in list(self._edges):
-            A, B = [x._point for x in edge._nodes]
-            r0, d0 = vector.vector_to_radec(A[0], A[1], A[2])
-            r1, d1 = vector.vector_to_radec(B[0], B[1], B[2])
-            m.drawgreatcircle(r0, d0, r1, d1, color='blue')
+        # for polygon in self._source_polygons:
+        #     polygon.draw(m)
 
         plt.xlim(minx, maxx)
         plt.ylim(miny, maxy)
@@ -468,7 +496,11 @@ class Graph:
             plt.title("%s, %d v, %d e" % (
                 title, len(self._nodes), len(self._edges)))
         plt.legend()
-        plt.show()
+        if file is not None:
+            plt.savefig(file)
+        elif show:
+            plt.show()
+            plt.close('all')
 
     def union(self):
         """
@@ -615,12 +647,9 @@ class Graph:
             A = starts[0]; starts = starts[1:]  # numpy equiv of "pop(0)"
             B = ends[0];   ends = ends[1:]      # numpy equiv of "pop(0)"
 
-            distance = great_circle_arc.length(A, B)
             for node in self._nodes:
                 if node not in AB._nodes:
-                    distanceA = great_circle_arc.length(node._point, A)
-                    distanceB = great_circle_arc.length(node._point, B)
-                    if np.abs((distanceA + distanceB) - distance) < 1e-8:
+                    if great_circle_arc.intersects_point(A, B, node._point):
                         newA, newB = self.split_edge(AB, node)
 
                         new_edges = [
@@ -707,16 +736,21 @@ class Graph:
 
         for edge in self._edges:
             edge._count = 0
+            A, B = edge._nodes
             for polygon in polygons:
                 if (not polygon in edge._source_polygons and
-                    polygon.intersects_arc(
-                        edge._nodes[0]._point, edge._nodes[1]._point)):
+                    ((polygon in A._source_polygons or
+                      polygon.contains_point(A._point)) and
+                     (polygon in B._source_polygons or
+                      polygon.contains_point(B._point)))):
                     edge._count += 1
 
         for edge in list(self._edges):
             if edge._count >= 1:
                 self.remove_edge(edge)
 
+        self._remove_orphaned_nodes()
+
     def _remove_exterior_edges(self):
         """
         Removes any edges that are not contained in all of the source
@@ -726,16 +760,23 @@ class Graph:
 
         for edge in self._edges:
             edge._count = 0
+            A, B = edge._nodes
             for polygon in polygons:
-                if (polygon in edge._source_polygons or
-                    polygon.intersects_arc(
-                        edge._nodes[0]._point, edge._nodes[1]._point)):
+                if polygon in edge._source_polygons:
                     edge._count += 1
+                else:
+                    if ((polygon in A._source_polygons or
+                         polygon.contains_point(A._point)) and
+                        (polygon in B._source_polygons or
+                         polygon.contains_point(B._point))):
+                        edge._count += 1
 
         for edge in list(self._edges):
             if edge._count < len(polygons):
                 self.remove_edge(edge)
 
+        self._remove_orphaned_nodes()
+
     def _remove_3ary_edges(self, large_first=False):
         """
         Remove edges between pairs of nodes that have 3 or more edges.
diff --git a/stsci/sphere/great_circle_arc.py b/stsci/sphere/great_circle_arc.py
index d5e78f2..0d60389 100644
--- a/stsci/sphere/great_circle_arc.py
+++ b/stsci/sphere/great_circle_arc.py
@@ -71,7 +71,7 @@ def _fast_cross(a, b):
     """
     if HAS_C_UFUNCS:
         return math_util.cross(a, b)
-    
+
     cp = np.empty(np.broadcast(a, b).shape)
     aT = a.T
     bT = b.T
@@ -83,9 +83,11 @@ def _fast_cross(a, b):
 
     return cp
 
+
 def _cross_and_normalize(A, B):
     if HAS_C_UFUNCS:
-        return math_util.cross_and_norm(A, B)
+        with np.errstate(invalid='ignore'):
+            return math_util.cross_and_norm(A, B)
 
     T = _fast_cross(A, B)
     # Normalization
@@ -154,7 +156,7 @@ def intersection(A, B, C, D):
     """
     if HAS_C_UFUNCS:
         return math_util.intersection(A, B, C, D)
-    
+
     A = np.asanyarray(A)
     B = np.asanyarray(B)
     C = np.asanyarray(C)
@@ -224,6 +226,14 @@ def length(A, B, degrees=True):
     A = np.asanyarray(A)
     B = np.asanyarray(B)
 
+    A2 = A ** 2.0
+    Al = np.sqrt(A2[..., 0] + A2[..., 1] + A2[..., 2])
+    B2 = B ** 2.0
+    Bl = np.sqrt(B2[..., 0] + B2[..., 1] + B2[..., 2])
+
+    A = A / np.expand_dims(Al, 2)
+    B = B / np.expand_dims(Bl, 2)
+
     dot = inner1d(A, B)
     dot = np.clip(dot, -1.0, 1.0)
     with np.errstate(invalid='ignore'):
@@ -257,9 +267,27 @@ def intersects(A, B, C, D):
     """
     with np.errstate(invalid='ignore'):
         intersections = intersection(A, B, C, D)
+
     return np.isfinite(intersections[..., 0])
 
 
+def intersects_point(A, B, C):
+    """
+    Returns True if point C is along the great circle arc AB.
+    """
+    # Check for exact match at the endpoint first
+    if np.any(np.all(A == C, axis=-1)):
+        return True
+
+    total_length = length(A, B)
+    left_length = length(A, C)
+    right_length = length(C, B)
+
+    length_diff = np.abs((left_length + right_length) - total_length)
+
+    return np.any(length_diff < 1e-8)
+
+
 def angle(A, B, C, degrees=True):
     """
     Returns the angle at *B* between *AB* and *BC*.
diff --git a/stsci/sphere/polygon.py b/stsci/sphere/polygon.py
index 06f6b80..2d67ff2 100644
--- a/stsci/sphere/polygon.py
+++ b/stsci/sphere/polygon.py
@@ -498,16 +498,20 @@ class SphericalPolygon(object):
         Determines if this `SphericalPolygon` intersects or contains
         the given arc.
         """
-        return self.contains_point(great_circle_arc.midpoint(a, b))
+        P = self._points
 
-        if self.contains_point(a) and self.contains_point(b):
+        if self.contains_arc(a, b):
             return True
 
-        P = self._points
         intersects = great_circle_arc.intersects(P[:-1], P[1:], a, b)
-        if np.any(intersects):
-            return True
-        return False
+        return np.any(intersects)
+
+    def contains_arc(self, a, b):
+        """
+        Returns `True` if the polygon fully encloses the arc given by a
+        and b.
+        """
+        return self.contains_point(a) and self.contains_point(b)
 
     def area(self):
         r"""