double _stdcall LocusCrsAtPoint(const Locus & locus, const LLPoint & testPt, LLPoint & geoPt, double & dPerpCrs, const double dTol)
{
if(!PtIsOnLocus(locus, testPt, geoPt, dTol))
return -1.0;
double dLocusCrs = 0.0;
double dPerpDist;
InverseResult result;
DistVincenty(testPt, geoPt, result);
dPerpCrs = result.azimuth;
dPerpDist = result.distance;
DistVincenty(locus.geoStart, locus.geoEnd, result);
double dGeoLen = result.distance;
double dDistToLocus = DistToLocusP(locus, geoPt, dTol, Eps());
double dSlope = atan((locus.endDist - locus.startDist) / dGeoLen);
dPerpCrs = dPerpCrs + dSlope;
if(dDistToLocus < 0)
dLocusCrs = dPerpCrs - M_PI_2;
else
dLocusCrs = dPerpCrs + M_PI_2;
if(dLocusCrs > M_2PI)
dLocusCrs = dLocusCrs - M_2PI;
return dLocusCrs;
}
LLPoint PointOnLocusP(const Locus &loc, const LLPoint &geoPt, double tol, double eps)
{
const double distp = DistToLocusP(loc, geoPt, tol, eps);
if (distp == 0)
return geoPt;
InverseResult result;
DistVincenty(geoPt, loc.geoStart, result);
result.azimuth += distp > 0.0 ? -M_PI / 2.0 : M_PI / 2.0;
return DestVincenty(geoPt, result.azimuth, fabs(distp));
}
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;
}