/**
* 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;
}
