/** * compute an approximative distance to a segment. Useful to estimate * distance to a gate. * Parameters: lat/long of point, then lat and long of A & B defining the * segment, and pointers to lat/long of closest point */ double distance_to_line_dichotomy_xing(double latitude, double longitude, double latitude_a, double longitude_a, double latitude_b, double longitude_b, double *x_latitude, double *x_longitude){ double p1_latitude, p1_longitude, p2_latitude, p2_longitude; double ortho_p1, ortho_p2; double limit; limit = PI/(180*60*1852); // 1m precision #ifdef DEBUG printf("Longitude A: %.2f Longitude B: .%2f\n", radToDeg(longitude_a), radToDeg(longitude_b)); #endif /* DEBUG */ if (fabs(longitude_a - longitude_b) > PI) { if (longitude_a > longitude_b) { if (longitude_a > 0.0) { longitude_a -= TWO_PI; } else { longitude_b += TWO_PI; } } else { if (longitude_b > 0.0) { longitude_b -= TWO_PI; } else { longitude_a += TWO_PI; } } } #ifdef DEBUG printf("AB NORM: Longitude A: %.2f Longitude B: %2f\n", radToDeg(longitude_a), radToDeg(longitude_b)); printf("Point: Longitude: %.2f\n", radToDeg(longitude)); #endif /* DEBUG */ p1_latitude = latitude_a; p1_longitude = longitude_a; p2_latitude = latitude_b; p2_longitude = longitude_b; ortho_p1 = ortho_distance(latitude, longitude, p1_latitude, p1_longitude); ortho_p2 = ortho_distance(latitude, longitude, p2_latitude, p2_longitude); // ending test on distance between two points. while (__distance((p1_latitude-p2_latitude), (p1_longitude-p2_longitude)) > limit) { if (ortho_p1 < ortho_p2) { p2_longitude = (p1_longitude+p2_longitude)/2; p2_latitude = yToLat((latToY(p1_latitude)+latToY(p2_latitude))/2); } else { p1_longitude = (p1_longitude+p2_longitude)/2; p1_latitude = yToLat((latToY(p1_latitude)+latToY(p2_latitude))/2); } ortho_p1 = ortho_distance(latitude, longitude, p1_latitude, p1_longitude); ortho_p2 = ortho_distance(latitude, longitude, p2_latitude, p2_longitude); } if (ortho_p1 < ortho_p2) { *x_latitude = p1_latitude; *x_longitude = p1_longitude; return ortho_p1; } *x_latitude = p2_latitude; *x_longitude = p2_longitude; return ortho_p2; }
FLT_DBL find_ortho (FLT_DBL * cc, FLT_DBL * rmat) { int kk; FLT_DBL ccrot[6 * 6]; FLT_DBL ccortho[6 * 6]; FLT_DBL ccrot2[6 * 6]; FLT_DBL ccti[6 * 6]; FLT_DBL rmat_transp[9]; FLT_DBL rmat_temp[9]; FLT_DBL rmat_temp2[9]; FLT_DBL vec[3]; FLT_DBL vec2[3]; FLT_DBL dist; FLT_DBL dista[3]; FLT_DBL qq[4], qq_best[4]; double center[4]; double range[4]; int count[4]; int qindex[4]; double inc[4]; FLT_DBL phi, theta; FLT_DBL temp; FLT_DBL dist_best; /* * Keep the compiler from complaining that this may be uninitialized. */ dist_best = NO_NORM; /* * Search over all possible orientations. * * Any orientation in 3-space can be specified by a unit vector, * giving an axis to rotate around, and an angle to rotate about * the given axis. The orientation is given with respect to some * fixed reference orientation. * * Since rotating by theta degrees about (A,B,C) produces the same * result as rotating -theta degrees about (-A,-B,-C), we only * need to consider 180 degrees worth of angles, not 360. * * In this application, we are finding the orientation of an orthorhombic * medium. Orthorhombic symmetry has three orthogonal symmetry planes, * so any one octant defines the whole. We thus only need to search * over rotation axes within one octant. * * Following the article in EDN, March 2, 1995, on page 95, author * "Do-While Jones" (a pen name of R. David Pogge), * "Quaternions quickly transform coordinates without error buildup", * we use quaternions to express the rotation. The article can be read * online here: * http://www.reed-electronics.com/ednmag/archives/1995/030295/05df3.htm * * If (A,B,C) is a unit vector to rotate theta degrees about, then: * * q0 = Cos (theta/2) * q1 = A * Sin(theta/2) * q2 = B * Sin(theta/2) * q3 = C * Sin(theta/2) * * so that q0^2 + q1^2 + q2^2 + q3^2 = 1. (A unit magnitude quaternion * represents a pure rotation, with no change in scale). * * For our case, taking advantage of the orthorhombic symmetry to * restrict the search space, we have: * 0 <= A <= 1 * 0 <= B <= 1 * 0 <= C <= 1 * 0 <= theta <= 180 degrees. * The rotation axis direction is limited to within one octant, * and the rotation about that axis is limited to half of the full circle. * * In terms of quaternions, this bounds all four elements between 0 and 1, * inclusive. */ /* * How much to subdivide each quaternion axis in the original scan. These * were somewhat arbitrarily chosen. These choices appear to be overkill, * but that ensures we won't accidentally miss the correct result by * insufficient sampling of the search space. */ /* * We sample the rotation angle more finely than the rotation axis. */ count[0] = SUB_ROT; count[1] = SUB_POS; count[2] = SUB_POS; count[3] = SUB_POS; /* * Between 0. and 1. for all 4 Q's (That is .5 +- .5.) */ for (kk = 0; kk < 4; kk++) { range[kk] = .5; center[kk] = .5; /* * A number meaning "not set yet", to get us through the loop the * first time. Needs to be much bigger than END_RES. */ inc[kk] = NOT_SET_YET; } while (inc[0] > END_RES && inc[1] > END_RES && inc[2] > END_RES && inc[3] > END_RES) { /* * Update inc to reflect the increment for the current search */ for (kk = 0; kk < 4; kk++) { inc[kk] = (2. * range[kk]) / (FLT_DBL) (count[kk] - 1); } /* * Start the 4-dimensional search. Keep track of the best result * found so far. The distance must be non-negative; we use -1 to mean * "not set yet". */ dist_best = NO_NORM; for (qindex[3] = 0; qindex[3] < count[3]; qindex[3]++) for (qindex[2] = 0; qindex[2] < count[2]; qindex[2]++) for (qindex[1] = 0; qindex[1] < count[1]; qindex[1]++) for (qindex[0] = 0; qindex[0] < count[0]; qindex[0]++) { /* * Calculate the quaternion for this search point. */ for (kk = 0; kk < 4; kk++) { /* * The term in parenthesis ranges from -1 to +1, * inclusive, so qq ranges from (-range+center) * to (+range + center). */ qq[kk] = range[kk] * (((FLT_DBL) (2 * qindex[kk] - (count[kk] - 1))) / ((FLT_DBL) (count[kk] - 1))) + center[kk]; } /* * Convert from a quaternion to a rotation matrix. * The subroutine also takes care of normalizing the * quaternion. */ quaternion_to_matrix (qq, rmat); /* * Apply the rotation matrix to the elastic stiffness * matrix. */ rotate_tensor (ccrot, cc, rmat); /* * Find the distance of the rotated medium from * orthorhombic aligned with the coordinate axes. */ dist = ortho_distance (ccortho, ccrot); /* * If it's the best found so far, or the first time * through, remember it. */ if (dist < dist_best || dist_best < 0.) { dist_best = dist; for (kk = 0; kk < 4; kk++) qq_best[kk] = qq[kk]; } } /* * Refine for the next, finer, search. To avoid any possible problem * caused by the optimal solution landing at an edge, we search over * twice the distance between the two search points from the previous * iteration. */ for (kk = 0; kk < 4; kk++) { center[kk] = qq_best[kk]; count[kk] = SUBDIVIDE; range[kk] = inc[kk]; } /* * We keep refining and searching the ever finer grid until we * achieve the required accuracy, at which point we fall out the * bottom of the loop here. */ } /* * We've got the answer to sufficient resolution... clean it up a bit, * then output it. */ /* * Convert the best answer from a Quaternion back to a rotation matrix */ quaternion_to_matrix (qq_best, rmat); /* * To make the order of the axes unique, we sort the principal axes * according to how well they work as a TI symmetry axis. * * Specifically, since after rotation the medium is canonically oriented, * with the X, Y, and Z axes the principal axes, the INVERSE rotation * must take the X, Y, and Z axes to the original arbitrarily oriented * principal axes. So we first inverse-rotate a coordinate axis back to a * principal axis. We then use vector_to_angles to give us the Euler * angles theta and phi for the principal axis. make_rotation_matrix then * constructs a rotation matrix that rotates that principal axis to +Z. * We then use that matrix to rotate the tensor. We then measure its * distance from VTI, and remember that distance. */ /* * First we need to find the inverse (the same as the transpose, because * it's _unitary_) of the rotation matrix rmat. */ transpose_matrix (rmat_transp, rmat); /* Test the X axis */ vec[0] = 1.; vec[1] = 0.; vec[2] = 0.; matrix_times_vector (vec2, rmat_transp, vec); vector_to_angles (vec2, &phi, &theta); make_rotation_matrix (theta, phi, 0., rmat_temp); rotate_tensor (ccrot2, cc, rmat_temp); dista[0] = ti_distance (ccti, ccrot2); /* Test the Y axis */ vec[0] = 0.; vec[1] = 1.; vec[2] = 0.; matrix_times_vector (vec2, rmat_transp, vec); vector_to_angles (vec2, &phi, &theta); make_rotation_matrix (theta, phi, 0., rmat_temp); rotate_tensor (ccrot2, cc, rmat_temp); dista[1] = ti_distance (ccti, ccrot2); /* Test the Z axis */ vec[0] = 0.; vec[1] = 0.; vec[2] = 1.; matrix_times_vector (vec2, rmat_transp, vec); vector_to_angles (vec2, &phi, &theta); make_rotation_matrix (theta, phi, 0., rmat_temp); rotate_tensor (ccrot2, cc, rmat_temp); dista[2] = ti_distance (ccti, ccrot2); /* * See which axis best functions as a TI symmetry axis, and make that one * the Z axis. */ if (dista[2] <= dista[1] && dista[2] <= dista[0]) { /* The Z axis is already the best. No rotation needed. */ make_rotation_matrix (0., 0., 0., rmat_temp); } else if (dista[1] <= dista[2] && dista[1] <= dista[0]) { /* Rotate Y to Z */ make_rotation_matrix (0., 90., 0., rmat_temp); temp = dista[2]; dista[2] = dista[1]; dista[1] = temp; } else { /* Rotate X to Z */ make_rotation_matrix (90., 90., -90., rmat_temp); temp = dista[2]; dista[2] = dista[0]; dista[0] = temp; } /* * Accumulate this axis-relabeling rotation (rmat_temp) onto the original * rotation (rmat). */ matrix_times_matrix (rmat_temp2, rmat_temp, rmat); /* * Now find the next-best TI symmetry axis and make that one the Y axis. */ if (dista[1] <= dista[0]) { /* Already there; do nothing. */ make_rotation_matrix (0., 0., 0., rmat_temp); } else { /* Rotate X to Y */ make_rotation_matrix (90., 0., 0., rmat_temp); temp = dista[1]; dista[1] = dista[0]; dista[0] = temp; } /* * Accumulate the new axis relabeling rotation (rmat_temp) onto the * combined previous rotation matrix (rmat_temp2) to produce the final * desired result, rmat. The axes should now be in sorted order. */ matrix_times_matrix (rmat, rmat_temp, rmat_temp2); return dist_best; }
/** * compute an approximative distance to a segment. Useful to estimate * distance to a gate. It is at best an approximation, as the intersection * algorithm is not valid for long distances * Parameters: lat/long of point, then lat and long of A & B defining the * segment */ double distance_to_line_ratio_xing(double latitude, double longitude, double latitude_a, double longitude_a, double latitude_b, double longitude_b, double *x_latitude, double *x_longitude, double *ab_ratio) { double dist_a, dist_b, max_dist, ab_dist, t_dist; double ortho_a, ortho_b; double t_latitude; double longitude_x, latitude_x, intersect; double longitude_y, latitude_y; double xing_latitude, xing_longitude; ortho_a = ortho_distance(latitude, longitude, latitude_a, longitude_a); ortho_b = ortho_distance(latitude, longitude, latitude_b, longitude_b); t_latitude = latToY(latitude); latitude_a = latToY(latitude_a); latitude_b = latToY(latitude_b); /* some normalization */ /* normalize the line */ #ifdef DEBUG printf("Longitude A: %.2f Longitude B: .%2f\n", radToDeg(longitude_a), radToDeg(longitude_b)); #endif /* DEBUG */ if (fabs(longitude_a - longitude_b) > PI) { if (longitude_a > longitude_b) { if (longitude_a > 0.0) { longitude_a -= TWO_PI; } else { longitude_b += TWO_PI; } } else { if (longitude_b > 0.0) { longitude_b -= TWO_PI; } else { longitude_a += TWO_PI; } } } #ifdef DEBUG printf("AB NORM: Longitude A: %.2f Longitude B: %2f\n", radToDeg(longitude_a), radToDeg(longitude_b)); printf("Point: Longitude: %.2f\n", radToDeg(longitude)); #endif /* DEBUG */ /* then the point */ if ((fabs(longitude-longitude_a)>PI) && (fabs(longitude-longitude_b)>PI)) { if (longitude < longitude_a) { longitude += TWO_PI; } else { longitude -= TWO_PI; } } #ifdef DEBUG printf("Point NORM: Longitude: %.2f\n", radToDeg(longitude)); #endif /* DEBUG */ dist_a = __distance((t_latitude-latitude_a), (longitude-longitude_a)); dist_b = __distance((t_latitude-latitude_b), (longitude-longitude_b)); ab_dist = __distance((latitude_a-latitude_b),(longitude_a-longitude_b)); max_dist = fmax(dist_a, dist_b); /* we construct a line form the point, orthogonal to the segment, long of at least max_dist */ latitude_x = t_latitude + (longitude_a - longitude_b) * max_dist / ab_dist; longitude_x = longitude + (latitude_b - latitude_a) * max_dist / ab_dist; latitude_y = t_latitude + (longitude_b - longitude_a) * max_dist / ab_dist; longitude_y = longitude + (latitude_a - latitude_b) * max_dist / ab_dist; #ifdef DEBUG printf("Intersect point: Latitude X: %.2f, Longitude X: %.2f\n", radToDeg(yToLat(latitude_x)), radToDeg(longitude_x)); printf("Intersect point: Latitude Y: %.2f, Longitude Y: %.2f\n", radToDeg(yToLat(latitude_y)), radToDeg(longitude_y)); #endif /* DEBUG */ intersect = __intersects_no_norm(latitude_a, longitude_a, latitude_b, longitude_b, latitude_y, longitude_y, latitude_x, longitude_x, &xing_latitude, &xing_longitude); if (intersect>=INTER_MIN_LIMIT && intersect<=INTER_MAX_LIMIT) { *x_latitude = yToLat(xing_latitude); *x_longitude = xing_longitude; #ifdef DEBUG printf("Intersect point: Latitude X: %.2f\n", radToDeg(*x_latitude)); printf("Intersect point: Longitude Y: %.2f\n", radToDeg(*x_longitude)); printf("Orig point: Latitude X: %.2f\n", radToDeg(latitude)); printf("Orig point: Longitude Y: %.2f\n", radToDeg(longitude)); #endif /* DEBUG */ t_dist = ortho_distance(latitude, longitude, *x_latitude, *x_longitude); #ifdef DEBUG printf("Min dist: %.3f, found dist: %.3f\n", max_dist, t_dist); printf("Ortho_a: %.3f, Ortho_b: %.3f\n", ortho_a, ortho_b); #endif /* DEBUG */ if (t_dist < ortho_a && t_dist < ortho_b) { *ab_ratio = intersect; return t_dist; } } if (ortho_a < ortho_b) { *x_latitude = yToLat(latitude_a); *x_longitude = longitude_a; *ab_ratio = -1.0; return ortho_a; } *x_latitude = yToLat(latitude_b); *x_longitude = longitude_b; *ab_ratio = -2.0; return ortho_b; }