diff options
| -rw-r--r-- | src/math_util.c | 261 | ||||
| -rw-r--r-- | stsci/sphere/graph.py | 129 | ||||
| -rw-r--r-- | stsci/sphere/great_circle_arc.py | 53 |
3 files changed, 334 insertions, 109 deletions
diff --git a/src/math_util.c b/src/math_util.c index 32c2914..2cbe009 100644 --- a/src/math_util.c +++ b/src/math_util.c @@ -7,6 +7,20 @@ #include "numpy/npy_math.h" /* + The intersects, length and intersects_point calculations use "long + doubles" internally. Emperical testing shows that this level of + precision is required for finding all intersections in typical HST + images that are rotated only slightly from one another. + + Unfortunately, "long double" is not a standard: On x86_64 Linux and + Mac, this is an 80-bit floating point representation, though + reportedly it is equivalent to double on Windows. If + Windows-specific or non x86_64 problems present themselves, we may + want to use a software floating point library or some other method + for this support. +*/ + +/* ***************************************************************************** ** BASICS ** ***************************************************************************** @@ -56,6 +70,15 @@ load_point(const char *in, const intp s, double* out) { } static inline void +load_pointl(const char *in, const intp s, long double* out) { + out[0] = (long double)(*(double *)in); + in += s; + out[1] = (long double)(*(double *)in); + in += s; + out[2] = (long double)(*(double *)in); +} + +static inline void save_point(const double* in, char *out, const intp s) { *(double *)out = in[0]; out += s; @@ -65,6 +88,15 @@ save_point(const double* in, char *out, const intp s) { } static inline void +save_pointl(const long double* in, char *out, const intp s) { + *(double *)out = (double)in[0]; + out += s; + *(double *)out = (double)in[1]; + out += s; + *(double *)out = (double)in[2]; +} + +static inline void cross(const double *A, const double *B, double *C) { C[0] = A[1]*B[2] - A[2]*B[1]; C[1] = A[2]*B[0] - A[0]*B[2]; @@ -72,11 +104,32 @@ cross(const double *A, const double *B, double *C) { } static inline void +crossl(const long double *A, const long double *B, long double *C) { + C[0] = A[1]*B[2] - A[2]*B[1]; + C[1] = A[2]*B[0] - A[0]*B[2]; + C[2] = A[0]*B[1] - A[1]*B[0]; +} + +static inline void normalize(double *A) { - double l = sqrt(A[0]*A[0] + A[1]*A[1] + A[2]*A[2]); - A[0] /= l; - A[1] /= l; - A[2] /= l; + double l = A[0]*A[0] + A[1]*A[1] + A[2]*A[2]; + if (l != 1.0) { + l = sqrt(l); + A[0] /= l; + A[1] /= l; + A[2] /= l; + } +} + +static inline void +normalizel(long double *A) { + long double l = A[0]*A[0] + A[1]*A[1] + A[2]*A[2]; + if (l != 1.0) { + l = sqrtl(l); + A[0] /= l; + A[1] /= l; + A[2] /= l; + } } static inline double @@ -84,16 +137,31 @@ dot(const double *A, const double *B) { return A[0]*B[0] + A[1]*B[1] + A[2]*B[2]; } +static inline long double +dotl(const long double *A, const long double *B) { + return A[0]*B[0] + A[1]*B[1] + A[2]*B[2]; +} + static inline double sign(const double A) { return (A == 0.0) ? 0.0 : ((A < 0.0) ? -1.0 : 1.0); } +static inline long double +signl(const long double A) { + return (A == 0.0) ? 0.0 : ((A < 0.0) ? -1.0 : 1.0); +} + static inline int equals(const double *A, const double *B) { return A[0] == B[0] && A[1] == B[1] && A[2] == B[2]; } +static inline int +equalsl(const long double *A, const long double *B) { + return A[0] == B[0] && A[1] == B[1] && A[2] == B[2]; +} + static inline void mult(double *T, const double f) { T[0] *= f; @@ -101,6 +169,30 @@ mult(double *T, const double f) { T[2] *= f; } +static inline void +multl(long double *T, const long double f) { + T[0] *= f; + T[1] *= f; + T[2] *= f; +} + +static inline long double +lengthl(long double *A, long double *B) { + long double s; + + /* Special case for "exactly equal" that avoids all of the calculation. */ + if (equalsl(A, B)) { + return 0; + } + + s = dotl(A, B); + + /* clip s to range -1.0 to 1.0 */ + s = (s < -1.0L) ? -1.0L : (s > 1.0L) ? 1.0L : s; + + return acosl(s); +} + /* ***************************************************************************** ** UFUNC LOOPS ** @@ -215,19 +307,19 @@ char *intersection_signature = "(i),(i),(i),(i)->(i)"; static void DOUBLE_intersection(char **args, intp *dimensions, intp *steps, void *NPY_UNUSED(func)) { - double A[3]; - double B[3]; - double C[3]; - double D[3]; + long double A[3]; + long double B[3]; + long double C[3]; + long double D[3]; - double ABX[3]; - double CDX[3]; - double T[3]; - double tmp[3]; + long double ABX[3]; + long double CDX[3]; + long double T[3]; + long double tmp[3]; double nans[3]; - double s; + long double s; int match; nans[0] = nans[1] = nans[2] = NPY_NAN; @@ -237,28 +329,28 @@ DOUBLE_intersection(char **args, intp *dimensions, intp *steps, void *NPY_UNUSED BEGIN_OUTER_LOOP_5 char *ip1=args[0], *ip2=args[1], *ip3=args[2], *ip4=args[3], *op=args[4]; - load_point(ip1, is1, A); - load_point(ip2, is2, B); - load_point(ip3, is3, C); - load_point(ip4, is4, D); + load_pointl(ip1, is1, A); + load_pointl(ip2, is2, B); + load_pointl(ip3, is3, C); + load_pointl(ip4, is4, D); - match = !(equals(A, C) | equals(A, D) | equals(B, C) | equals(B, D)); + match = !(equalsl(A, C) | equalsl(A, D) | equalsl(B, C) | equalsl(B, D)); if (match) { - cross(A, B, ABX); - cross(C, D, CDX); - cross(ABX, CDX, T); - normalize(T); + crossl(A, B, ABX); + crossl(C, D, CDX); + crossl(ABX, CDX, T); + normalizel(T); match = 0; - cross(ABX, A, tmp); - s = sign(dot(tmp, T)); - cross(B, ABX, tmp); - if (s == sign(dot(tmp, T))) { - cross(CDX, C, tmp); - if (s == sign(dot(tmp, T))) { - cross(D, CDX, tmp); - if (s == sign(dot(tmp, T))) { + crossl(ABX, A, tmp); + s = signl(dotl(tmp, T)); + crossl(B, ABX, tmp); + if (s == signl(dotl(tmp, T))) { + crossl(CDX, C, tmp); + if (s == signl(dotl(tmp, T))) { + crossl(D, CDX, tmp); + if (s == signl(dotl(tmp, T))) { match = 1; } } @@ -266,8 +358,8 @@ DOUBLE_intersection(char **args, intp *dimensions, intp *steps, void *NPY_UNUSED } if (match) { - mult(T, s); - save_point(T, op, is5); + multl(T, s); + save_pointl(T, op, is5); } else { save_point(nans, op, is5); } @@ -278,6 +370,93 @@ static PyUFuncGenericFunction intersection_functions[] = { DOUBLE_intersection } static void * intersection_data[] = { (void *)NULL }; static char intersection_signatures[] = { PyArray_DOUBLE, PyArray_DOUBLE, PyArray_DOUBLE, PyArray_DOUBLE, PyArray_DOUBLE }; +/*/////////////////////////////////////////////////////////////////////////// + length +*/ + +char *length_signature = "(i),(i)->()"; + +/* + * Finds the length of the given great circle arc AB + */ +static void +DOUBLE_length(char **args, intp *dimensions, intp *steps, void *NPY_UNUSED(func)) +{ + long double A[3]; + long double B[3]; + + long double s; + + INIT_OUTER_LOOP_3 + intp is1=steps[0], is2=steps[1]; + BEGIN_OUTER_LOOP_3 + char *ip1=args[0], *ip2=args[1], *op=args[2]; + + load_pointl(ip1, is1, A); + load_pointl(ip2, is2, B); + + normalizel(A); + normalizel(B); + + s = lengthl(A, B); + + *((double *)op) = (double)s; + END_OUTER_LOOP +} + +static PyUFuncGenericFunction length_functions[] = { DOUBLE_length }; +static void * length_data[] = { (void *)NULL }; +static char length_signatures[] = { PyArray_DOUBLE, PyArray_DOUBLE, PyArray_DOUBLE }; + +/*/////////////////////////////////////////////////////////////////////////// + length +*/ + +char *intersects_point_signature = "(i),(i),(i)->()"; + +/* + * Returns True is if the point C intersects arc AB + */ +static void +DOUBLE_intersects_point(char **args, intp *dimensions, intp *steps, void *NPY_UNUSED(func)) +{ + + long double A[3]; + long double B[3]; + long double C[3]; + + long double total; + long double left; + long double right; + long double diff; + + INIT_OUTER_LOOP_4 + intp is1=steps[0], is2=steps[1], is3=steps[2]; + BEGIN_OUTER_LOOP_4 + char *ip1=args[0], *ip2=args[1], *ip3=args[2], *op=args[3]; + + load_pointl(ip1, is1, A); + load_pointl(ip2, is2, B); + load_pointl(ip3, is3, C); + + normalizel(A); + normalizel(B); + normalizel(C); + + total = lengthl(A, B); + left = lengthl(A, C); + right = lengthl(C, B); + + diff = fabsl((left + right) - total); + + *((uint8_t *)op) = diff < 1e-10; + END_OUTER_LOOP +} + +static PyUFuncGenericFunction intersects_point_functions[] = { DOUBLE_intersects_point }; +static void * intersects_point_data[] = { (void *)NULL }; +static char intersects_point_signatures[] = { PyArray_DOUBLE, PyArray_DOUBLE, PyArray_DOUBLE, PyArray_BOOL }; + /* ***************************************************************************** ** MODULE ** @@ -323,6 +502,24 @@ addUfuncs(PyObject *dictionary) { 0, intersection_signature); PyDict_SetItemString(dictionary, "intersection", f); Py_DECREF(f); + + f = PyUFunc_FromFuncAndDataAndSignature( + length_functions, length_data, length_signatures, 2, 2, 1, + PyUFunc_None, "length", + "length of great circle arc \n" \ + " \"(i),(i)->()\" \n", + 0, length_signature); + PyDict_SetItemString(dictionary, "length", f); + Py_DECREF(f); + + f = PyUFunc_FromFuncAndDataAndSignature( + intersects_point_functions, intersects_point_data, intersects_point_signatures, 2, 3, 1, + PyUFunc_None, "intersects_point", + "True where point C intersects arc AB \n" \ + " \"(i),(i),(i)->()\" \n", + 0, intersects_point_signature); + PyDict_SetItemString(dictionary, "intersects_point", f); + Py_DECREF(f); } #if defined(NPY_PY3K) diff --git a/stsci/sphere/graph.py b/stsci/sphere/graph.py index 2b37522..95503b3 100644 --- a/stsci/sphere/graph.py +++ b/stsci/sphere/graph.py @@ -200,7 +200,6 @@ class Graph: self._nodes = set() self._edges = set() self._source_polygons = set() - self._start_node = None self.add_polygons(polygons) @@ -239,8 +238,6 @@ class Graph: self._source_polygons.add(polygon) start_node = nodeA = self.add_node(points[0], [polygon]) - if self._start_node is None: - self._start_node = start_node for i in range(1, len(points) - 1): nodeB = self.add_node(points[i], [polygon]) # Don't create self-pointing edges @@ -275,6 +272,8 @@ class Graph: # Nodes that are closer together than 1e-10 are automatically # merged, since we can't numerically determine whether those # nodes are the same, or just along an arc. + + # TODO: Vectorize this for node in self._nodes: if great_circle_arc.length(node._point, new_node._point) < 1e-10: node._source_polygons.update(source_polygons) @@ -302,7 +301,8 @@ class Graph: if len(nodeB._edges) == 0: self._nodes.remove(nodeB) self._edges.remove(edge) - self._nodes.remove(node) + if node in self._nodes: + self._nodes.remove(node) def add_edge(self, A, B, source_polygons=[]): """ @@ -332,10 +332,10 @@ class Graph: # one, otherwise the Edge will get hooked up to the nodes but # be orphaned. for edge in self._edges: - if ((A.equals(edge._nodes[0]) and - B.equals(edge._nodes[1])) or - (B.equals(edge._nodes[0]) and - A.equals(edge._nodes[1]))): + if ((A is edge._nodes[0] and + B is edge._nodes[1]) or + (A is edge._nodes[1] and + B is edge._nodes[0])): edge._source_polygons.update(source_polygons) return edge @@ -432,7 +432,8 @@ class Graph: 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), + highlight_edge=None, intersections=[]): from mpl_toolkits.basemap import Basemap from matplotlib import pyplot as plt fig = plt.figure() @@ -460,6 +461,18 @@ class Graph: counts.setdefault(count, []) counts[count].append(list(node._point)) + 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]) + if edge is highlight_edge: + color = 'red' + lw = 2 + else: + color = 'blue' + lw = 0.5 + m.drawgreatcircle(r0, d0, r1, d1, color=color, lw=lw) + for k, v in counts.items(): v = np.array(v) ra, dec = vector.vector_to_radec(v[:, 0], v[:, 1], v[:, 2]) @@ -472,11 +485,11 @@ class Graph: 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 v in intersections: + if np.all(np.isfinite(v)): + ra, dec = vector.vector_to_radec(v[0], v[1], v[2]) + x, y = m(ra, dec) + m.plot(x, y, 'x', markersize=20) plt.xlim(minx, maxx) plt.ylim(miny, maxy) @@ -583,57 +596,59 @@ class Graph: if len(A._edges) == 3 and len(B._edges) == 3: self.remove_edge(AB) - def _find_all_intersections(self): - """ - Find all the intersecting edges in the graph. For each - intersecting pair, four new edges are created around the - intersection point. - """ - def get_edge_points(edges): - return (np.array([x._nodes[0]._point for x in edges]), - np.array([x._nodes[1]._point for x in edges])) + def _get_edge_points(self, edges): + return (np.array([x._nodes[0]._point for x in edges]), + np.array([x._nodes[1]._point for x in edges])) + def _find_point_to_arc_intersections(self): # For speed, we want to vectorize all of the intersection - # calculations. Therefore, there is a list of edges, and two - # arrays containing the end points of those edges. They all - # need to have things added and removed from them at the same - # time to keep them in sync, but of course the interface for - # doing so is different between Python lists and numpy arrays. + # calculations. Therefore, there is a list of edges, and an + # array of points for all of the nodes. Then calculating the + # intersection between an edge and all other nodes becomes a + # fast, vectorized operation. edges = list(self._edges) - starts, ends = get_edge_points(edges) + starts, ends = self._get_edge_points(edges) - # First, split all edges by any nodes that intersect them + nodes = list(self._nodes) + nodes_array = np.array([x._point for x in nodes]) + + # Split all edges by any nodes that intersect them while len(edges) > 1: AB = edges.pop(0) - A = starts[0]; starts = starts[1:] # numpy equiv of "pop(0)" - B = ends[0]; ends = ends[1:] # numpy equiv of "pop(0)" + A, B = list(AB._nodes) - for node in self._nodes: + intersects = great_circle_arc.intersects_point( + A._point, B._point, nodes_array) + intersection_indices = np.nonzero(intersects)[0] + + for index in intersection_indices: + node = nodes[index] if node not in AB._nodes: - if great_circle_arc.intersects_point(A, B, node._point): - newA, newB = self.split_edge(AB, node) + newA, newB = self.split_edge(AB, node) - new_edges = [ - edge for edge in (newA, newB) - if edge not in edges] + new_edges = [ + edge for edge in (newA, newB) + if edge not in edges] + for end_point in AB._nodes: node._source_polygons.update( - self.add_node(A)._source_polygons) - node._source_polygons.update( - self.add_node(B)._source_polygons) - edges = new_edges + edges - new_starts, new_ends = get_edge_points(new_edges) - starts = np.vstack( - (new_starts, starts)) - ends = np.vstack( - (new_ends, ends)) - break + end_point._source_polygons) + edges = new_edges + edges + break + + def _find_arc_to_arc_intersections(self): + # For speed, we want to vectorize all of the intersection + # calculations. Therefore, there is a list of edges, and two + # arrays containing the end points of those edges. They all + # need to have things added and removed from them at the same + # time to keep them in sync, but of course the interface for + # doing so is different between Python lists and numpy arrays. edges = list(self._edges) - starts, ends = get_edge_points(edges) + starts, ends = self._get_edge_points(edges) - # Next, calculate edge-to-edge intersections and break + # Calculate edge-to-edge intersections and break # edges on the intersection point. while len(edges) > 1: AB = edges.pop(0) @@ -655,8 +670,6 @@ class Graph: # we want to eliminate intersections that only intersect # at the end points for j in intersection_indices: - C = starts[j] - D = ends[j] CD = edges[j] E = intersections[j] @@ -686,13 +699,22 @@ class Graph: # against all remaining edges. del edges[j] # CD edges = new_edges + edges - new_starts, new_ends = get_edge_points(new_edges) + new_starts, new_ends = self._get_edge_points(new_edges) starts = np.vstack( (new_starts, starts[:j], starts[j+1:])) ends = np.vstack( (new_ends, ends[:j], ends[j+1:])) break + def _find_all_intersections(self): + """ + Find all the intersecting edges in the graph. For each + intersecting pair, four new edges are created around the + intersection point. + """ + self._find_point_to_arc_intersections() + self._find_arc_to_arc_intersections() + def _remove_interior_edges(self): """ Removes any nodes that are contained inside other polygons. @@ -794,7 +816,6 @@ class Graph: """ polygons = [] edges = set(self._edges) # copy - seen_nodes = set() while len(edges): polygon = [] # Carefully pick out an "original" edge first. Synthetic diff --git a/stsci/sphere/great_circle_arc.py b/stsci/sphere/great_circle_arc.py index c9ddbdf..5df7aa2 100644 --- a/stsci/sphere/great_circle_arc.py +++ b/stsci/sphere/great_circle_arc.py @@ -154,9 +154,6 @@ def intersection(A, B, C, D): http://www.mathworks.com/matlabcentral/newsreader/view_thread/276271 """ - if HAS_C_UFUNCS: - return math_util.intersection(A, B, C, D) - A = np.asanyarray(A) B = np.asanyarray(B) C = np.asanyarray(C) @@ -186,16 +183,20 @@ def intersection(A, B, C, D): # If they share a common point, it's not an intersection. This # gets around some rounding-error/numerical problems with the # above. - equals = (np.all(A == C, axis = -1) | - np.all(A == D, axis = -1) | - np.all(B == C, axis = -1) | - np.all(B == D, axis = -1)) + equals = (np.all(A == C, axis=-1) | + np.all(A == D, axis=-1) | + np.all(B == C, axis=-1) | + np.all(B == D, axis=-1)) equals = np.expand_dims(equals, 2) return np.where(equals, np.nan, cross) +if HAS_C_UFUNCS: + intersection = math_util.intersection + + def length(A, B, degrees=True): r""" Returns the angular distance between two points (in vector space) @@ -223,21 +224,24 @@ def length(A, B, degrees=True): \Delta = \arccos(A \dot B) """ - A = np.asanyarray(A) - B = np.asanyarray(B) + if HAS_C_UFUNCS: + result = math_util.length(A, B) + else: + 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]) + A2 = A ** 2.0 + Al = np.sqrt(np.sum(A2, axis=-1)) + B2 = B ** 2.0 + Bl = np.sqrt(np.sum(B2, axis=-1)) - A = A / np.expand_dims(Al, 2) - B = B / np.expand_dims(Bl, 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'): - result = np.arccos(dot) + dot = inner1d(A, B) + dot = np.clip(dot, -1.0, 1.0) + with np.errstate(invalid='ignore'): + result = np.arccos(dot) if degrees: return np.rad2deg(result) @@ -275,9 +279,8 @@ 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 + if HAS_C_UFUNCS: + return math_util.intersects_point(A, B, C) total_length = length(A, B) left_length = length(A, C) @@ -285,7 +288,11 @@ def intersects_point(A, B, C): length_diff = np.abs((left_length + right_length) - total_length) - return np.any(length_diff < 1e-10) + return length_diff < 1e-10 + + +if HAS_C_UFUNCS: + intersects_point = math_util.intersects_point def angle(A, B, C, degrees=True): |