int GeoLocusIntersect(const LLPoint &gStart, const LLPoint &gEnd, const Locus &loc, LLPoint &intersect,
double dTol, double dEps)
{
InverseResult result;
DistVincenty(gStart, gEnd, result);
const double gAz = result.azimuth;
DistVincenty(loc.locusStart, loc.locusEnd, result);
const double locStAz = result.azimuth;
const double locLength = result.distance;
double crs31, crs32, dist13, dist23;
LLPoint pt1;
if (!CrsIntersect(loc.locusStart, locStAz, crs31, dist13, gStart, gAz, crs32, dist23, dTol, pt1))
return 0;
DistVincenty(loc.geoStart, loc.geoEnd, result);
const double tcrs = result.azimuth;
double crsFromPt, distFromPt;
LLPoint ptInt = PerpIntercept(loc.geoStart, tcrs, pt1, crsFromPt, distFromPt, dTol);
double distLoc = DistToLocusP(loc, ptInt, dTol, dEps);
double distarray[2];
double errarray[2];
errarray[1] = distFromPt - fabs(distLoc);
distarray[1] = dist23;
double distBase = dist23 - errarray[1] / cos(fabs(SignAzimuthDifference(crsFromPt, crs32)));
int k = 0;
const int maxCount = 10;
while (!std::isnan(distBase) && fabs(errarray[1]) > dTol && k < maxCount)
{
pt1 = DestVincenty(gStart, gAz, distBase);
errarray[0] = errarray[1];
distarray[0] = distarray[1];
distarray[1] = distBase;
ptInt = PerpIntercept(loc.geoStart, tcrs, pt1, crsFromPt, distFromPt, dTol);
distLoc = DistToLocusP(loc, ptInt, dTol, dEps);
errarray[1] = distFromPt - fabs(distLoc);
FindLinearRoot(distarray, errarray, distBase);
k++;
}
intersect = pt1;
DistVincenty(intersect, loc.locusStart, result);
const double distLocStPt1 = result.distance;
DistVincenty(intersect, loc.locusEnd, result);
// found intersect point must be on or between locus
// If 5e-3 is to tight a tolerance then try setting to 5e-2
// For the 8260.54A Appendix test cases 1e-3 was to tight, 5e-3
// works just fine.
if (!IsNearZero(locLength - (distLocStPt1 + result.distance), 5e-3))
return 0;
return 1;
}
int LocusPerpIntercept(const Locus &loc, const LLPoint &pt2, double &crsFromPt,
double &distFromPt, LLPoint &intPt, double dTol)
{
InverseResult result;
DistVincenty(loc.geoStart, loc.geoEnd, result);
double gcrs = result.azimuth;
double gdist = result.distance;
if (fabs(loc.startDist - loc.endDist) < dTol)
{
const LLPoint geoPt = PerpIntercept(loc.geoStart, gcrs, pt2, crsFromPt, distFromPt, dTol);
intPt = PointOnLocusP(loc, geoPt, dTol, kEps);
DistVincenty(pt2, intPt, result);
distFromPt = result.distance;
crsFromPt = result.azimuth;
return 1;
}
DistVincenty(loc.locusStart, loc.locusEnd, result);
LLPoint locPt = PerpIntercept(loc.locusStart, result.azimuth, pt2, crsFromPt, distFromPt, dTol);
LLPoint geoPt = PerpIntercept(loc.geoStart, gcrs, locPt, crsFromPt, distFromPt, dTol);
const double locAngle = atan((loc.startDist - loc.endDist) / gdist);
double distarray[2], errarray[2];
distarray[0] = distarray[1] = errarray[0] = errarray[1] = 0.0;
DistVincenty(loc.geoStart, geoPt, result);
distarray[1] = result.distance;
const int maxCount = 15;
double newDist = 0.0;
int k = 0;
while (k == 0 || (!std::isnan(newDist) && fabs(errarray[1]) > dTol && k < maxCount))
{
geoPt = DestVincenty(loc.geoStart, gcrs, distarray[1]);
locPt = PointOnLocusP(loc /*loc.geoStart*/, geoPt, dTol, kEps);
DistVincenty(locPt, pt2, result);
errarray[1] = -result.distance * cos(fabs(
SignAzimuthDifference(LocusCrsAtPoint(loc, locPt, geoPt, 1e-8), result.azimuth)));
if (fabs(errarray[1]) < dTol)
{
distFromPt = result.distance;
crsFromPt = result.reverseAzimuth;
intPt = locPt;
break;
}
if (k == 0)
newDist = distarray[1] + errarray[1] * cos(locAngle);
else
FindLinearRoot(distarray, errarray, newDist);
distarray[0] = distarray[1];
distarray[1] = newDist;
errarray[0] = errarray[1];
k++;
}
intPt = locPt;
DistVincenty(pt2, intPt, result);
distFromPt = result.distance;
crsFromPt = result.azimuth;
return 1;
}
int GeodesicArcIntercept(const LLPoint &pt1, double crs1,
const LLPoint ¢er, double radius,
LLPoint &intPtC1, LLPoint &intPtC2, double dTol)
{
double dCrsFromPt, dDistFromPt;
const LLPoint perpPt = PerpIntercept(pt1, crs1, center, dCrsFromPt, dDistFromPt, dTol);
InverseResult result;
DistVincenty(perpPt, center, result);
if (result.distance > radius)
return 0;
if (fabs(result.distance - radius) < dTol)
{
intPtC1 = perpPt;
return 1;
}
const double perpDist = result.distance;
DistVincenty(perpPt, pt1, result);
if (IsApprox(cos(perpDist / kSphereRadius), 0.0, 1e-8))
return 0;
double crs = result.azimuth;
double dist = kSphereRadius * acos(cos(radius / kSphereRadius) / cos(perpDist / kSphereRadius));
LLPoint pt = DestVincenty(perpPt, crs, dist);
const int nIntersects = 2;
for (int i = 0; i < nIntersects; i++)
{
DistVincenty(center, pt, result);
const double rcrs = result.reverseAzimuth;
const double dErr = radius - result.distance;
double distarray[2], errarray[2];
distarray[0] = dist;
errarray[0] = dErr;
DistVincenty(pt, perpPt, result);
const double bcrs = result.azimuth;
DistVincenty(center, pt, result);
const double dAngle = fabs(SignAzimuthDifference(result.azimuth, result.reverseAzimuth));
const double B = fabs(SignAzimuthDifference(bcrs, rcrs) + M_PI - dAngle);
const double A = acos(sin(B) * cos(fabs(dErr) / kSphereRadius));
double c;
if (fabs(sin(A)) < dTol)
c = dErr;
else if (fabs(A) < dTol)
c = dErr / cos(B);
else
c = kSphereRadius * asin(sin(dErr / kSphereRadius) / sin(A));
dist = dErr > 0 ? dist + c : dist - c;
pt = DestVincenty(perpPt, crs, dist);
DistVincenty(center, pt, result);
distarray[1] = dist;
errarray[1] = radius - result.distance;
while (fabs(dErr) > dTol)
{
FindLinearRoot(distarray, errarray, dist);
if (std::isnan(dist))
break;
pt = DestVincenty(perpPt, crs, dist);
DistVincenty(center, pt, result);
distarray[0] = distarray[1];
errarray[0] = errarray[1];
distarray[1] = dist;
errarray[1] = radius - result.distance;
break;
}
if (i == 0)
intPtC1 = pt;
else if (i == 1)
intPtC2 = pt;
else
break;
crs += M_PI;
pt = DestVincenty(perpPt, crs, dist);
DistVincenty(center, pt, result);
errarray[0] = radius - result.distance;
}
return nIntersects;
}
bool CrsIntersect1(double lat1, double lon1, double az13,
double & az31, double & dist13, double lat2, double lon2, double az23,
double & az32, double & dist23, double dTol, double lati, double loni)
{
LLPoint pt1;
LLPoint pt2;
pt1.latitude = lat1;
pt1.longitude = lon1;
pt2.latitude = lat2;
pt2.longitude = lon2;
double dAz13 = az13;
double dAz23 = az23;
InverseResult result;
DistVincenty(pt1, pt2, &result);
double dist12 = result.distance;
double crs12 = result.azimuth;
double crs21 = result.reverseAzimuth;
double angle1 = fabs(SignAzimuthDifference(dAz13, crs12));
double angle2 = fabs(SignAzimuthDifference(crs21, dAz23));
if(angle1 < 0.0 && angle2 < 0.0)
{
angle1 = -angle1;
angle2 = -angle2;
}
if(sin(angle1) == 0.0 && sin(angle2) == 0.0)
return false;
// step 7
// locate approx intersection of point3 using spherical earth model. Section 3.2
double cosA = cos(angle1);
double sinA = sin(angle1);
double cosB = cos(angle2);
double sinB = sin(angle2);
double C = acos( -cosA * cosB + sinA * sinB * cos(dist12 / SphereRadius()));
double cosC = cos(C);
double sinC = sin(C);
double a = SphereRadius() * acos( (cosA + cosB * cosC) / (sinB * sinC) );
double b = SphereRadius() * acos( (cosB + cosA * cosC) / (sinA * sinC) );
if(!IsNumber(a) || !IsNumber(b))
return false;
LLPoint llIntersect = DestVincenty(pt1, dAz13, b);
DistVincenty(pt1, llIntersect, &result);
dist13 = result.distance;
LLPoint llInv = llIntersect;
llInv.latitude = -llInv.latitude;
llInv.longitude = llInv.longitude + M_PI - (2*M_PI);
DistVincenty(pt1, llInv, &result);
if(dist13 > result.distance)
{
llIntersect = llInv;
dist13 = result.distance;
az31 = result.reverseAzimuth;
dAz13 = dAz13 + M_PI;
dAz23 = dAz23 + M_PI;
}
DistVincenty(pt2, llIntersect, &result);
dist23 = result.distance;
if(dist13 < NmToMeters(1))
{
pt1 = DestVincenty(pt1, dAz13 + M_PI, NmToMeters(1.0));
DistVincenty(pt1, llIntersect, &result);
dAz13 = result.azimuth;
}
if(dist23 < NmToMeters(1))
{
pt2 = DestVincenty(pt2, dAz23 + M_PI, NmToMeters(1.0));
DistVincenty(pt2, llIntersect, &result);
dAz23 = result.azimuth;
}
bool bSwapped = false;
if(dist23 < dist13)
{
LLPoint newPt = pt1;
pt1 = pt2;
pt2 = newPt;
double aaz13 = dAz13;
dAz13 = dAz23;
dAz23 = aaz13;
dist13 = dist23;
bSwapped = true;
}
double distarray[2], errarray[2];
distarray[0] = dist13;
llIntersect = DestVincenty(pt1, dAz13, dist13);
DistVincenty(pt2, llIntersect, &result);
double aacrs23 = result.azimuth;
errarray[0] = SignAzimuthDifference(aacrs23, dAz23);
distarray[1] = 1.01 * dist13;
llIntersect = DestVincenty(pt1, dAz13, distarray[1]);
DistVincenty(pt2, llIntersect, &result);
aacrs23 = result.azimuth;
errarray[1] = SignAzimuthDifference(aacrs23, dAz23);
int k = 0;
double dErr = 0;
int nMaxCount = 15;
while(k == 0 || ((dErr > dTol) && (k <= nMaxCount)))
{
FindLinearRoot(distarray, errarray, dist13);
LLPoint newPt = DestVincenty(pt1, dAz13, dist13);
DistVincenty(pt2, newPt, &result);
aacrs23 = result.azimuth;
DistVincenty(newPt, llIntersect, &result);
dErr = result.distance;
distarray[0] = distarray[1];
distarray[1] = dist13;
errarray[0] = errarray[1];
errarray[1] = SignAzimuthDifference(aacrs23, dAz23);
k++;
llIntersect = newPt;
}
// display if k == maxinteratorcount (10) and show error message
// because results might not have converged.
if(k > nMaxCount && dErr > 1e-8)
return false;
if(bSwapped)
{
LLPoint newPt = pt1;
pt1 = pt2;
pt2 = newPt;
double aaz13 = dAz13;
dAz13 = dAz23;
dAz23 = aaz13;
dist13 = dist23;
}
DistVincenty(llIntersect, pt1, &result);
az31 = result.azimuth;
dist13 = result.distance;
DistVincenty(llIntersect, pt2, &result);
az32 = result.azimuth;
dist23 = result.distance;
return true;
}
