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;
}
bool _stdcall PtIsOnArc( const LLPoint & llArcCenter, double dArcRadius,
double dArcStartAzimuth, double dArcEndAzimuth, int nArcDirection,
const LLPoint & llTestPt, int & bOnArc )
{
InverseResult invResult;
if(!DistVincenty(llArcCenter, llTestPt, invResult))
return false;
double dDist = invResult.distance;
double dCrs = invResult.azimuth;
bOnArc = false;
if(fabs(dDist - dArcRadius) > 0.5e-3) //Tol())
bOnArc = false;
else {
double dArcExtent = GetArcExtent(dArcStartAzimuth, dArcEndAzimuth, nArcDirection, Tol());
if(dArcExtent == M_2PI)
bOnArc = true;
else
{
double dSubExtent = GetArcExtent(dArcStartAzimuth, dCrs, nArcDirection, Tol());
if(nArcDirection > 0) {
if(dSubExtent <= dArcExtent)
bOnArc = true;
} else {
if(dSubExtent >= dArcExtent)
bOnArc = true;
}
}
}
return true;
}
void distVincenty(LLPoint pt1, LLPoint pt2, InverseResult * result)
{
InverseResult temp;
//bool test =
DistVincenty( pt1, pt2, &temp);
*result=temp;
}
bool PtIsOnArc(const LLPoint &llArcCenter, double dArcRadius,
double dArcStartAzimuth, double dArcEndAzimuth, int nArcDirection,
const LLPoint &llTestPt, int &bOnArc)
{
InverseResult invResult;
if (!DistVincenty(llArcCenter, llTestPt, invResult))
return false;
bOnArc = false;
if (fabs(invResult.distance - dArcRadius) <= 0.5e-3)
{
const double dSubtendedAng1 = GetArcExtent(dArcStartAzimuth, dArcEndAzimuth, nArcDirection, kTol);
if (dSubtendedAng1 == M_2PI)
bOnArc = true;
else
{
const double dSubtendedAng2 = GetArcExtent(dArcStartAzimuth, invResult.azimuth,
nArcDirection, kTol);
if (nArcDirection > 0)
{
if (dSubtendedAng2 <= dSubtendedAng1)
bOnArc = true;
}
else
{
if (dSubtendedAng2 >= dSubtendedAng1)
bOnArc = true;
}
}
}
return true;
}
bool PtIsOnGeodesic(const LLPoint &pt1, const LLPoint &pt2, const LLPoint &pt3, int lengthCode,
PtIsOnGeodesicResult &result)
{
InverseResult invResult;
if (!DistVincenty(pt1, pt3, invResult))
return false;
const double dist13 = invResult.distance;
if (!DistVincenty(pt1, pt2, invResult))
return false;
const double dist12 = invResult.distance;
const double crs12 = invResult.azimuth;
LLPoint testPt2 = DestVincenty(pt1, crs12, dist13);
if (!DistVincenty(pt3, testPt2, invResult))
return false;
if (invResult.distance <= kTolPtIsOnGeodesic)
{
result.result = lengthCode > 0 || dist13 - dist12 <= kTolPtIsOnGeodesic;
}
else if (lengthCode == 2)
{
testPt2 = DestVincenty(pt1, crs12 + M_PI, dist13);
if (!DistVincenty(pt3, testPt2, invResult))
return false;
result.result = invResult.distance <= kTolPtIsOnGeodesic;
}
else
{
result.result = false;
}
result.geoStart = pt1;
result.geoEnd = pt2;
result.geoPt = testPt2;
result.lengthCode = lengthCode;
return true;
}
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));
}
double DistToLocusD(const Locus &loc, double dDistance, double dEps)
{
InverseResult result;
if (!DistVincenty(loc.geoStart, loc.geoEnd, result))
return numeric_limits<double>::signaling_NaN();
if (result.distance > 0.0)
{
return loc.startDist + (dDistance / result.distance) * (loc.endDist - loc.startDist);
}
return 0.0;
}
bool DistVincenty(double lat1, double lon1, double lat2, double lon2, double *dist, double *fwdAz, double *revAz){
InverseResult result;
LLPoint pt1, pt2;
pt1.latitude=toRad(lat1);
pt1.longitude=toRad(lon1);
pt2.latitude=toRad(lat2);
pt2.longitude=toRad(lon2);
bool test=DistVincenty( pt1, pt2, & result);
*dist = mtoNM(result.distance);
*fwdAz = toDeg(result.azimuth);
*revAz = toDeg(result.azimuth);
return test;
}
int TangentFixedRadiusArc(const LLPoint &pt1, double crs12, const LLPoint &pt3,
double crs3, double radius, int &dir,
LLPoint ¢erPt, LLPoint &tanPt1, LLPoint &tanPt2, double dTol)
{
LLPoint pt2;
if (!CrsIntersect(pt1, crs12, pt3, crs3 + M_PI, dTol, pt2))
return 0;
InverseResult result;
DistVincenty(pt1, pt2, result);
const double dist12 = result.distance;
DistVincenty(pt2, pt1, result);
const double crs21 = result.azimuth;
DistVincenty(pt2, pt3, result);
const double vertexAngle = SignAzimuthDifference(crs21, result.azimuth);
if (fabs(sin(vertexAngle)) < dTol)
return 0;
dir = vertexAngle > 0.0 ? -1 : 1;
if (radius > fabs(kSphereRadius * vertexAngle / 2.0))
return 0;
double distToStart = dist12 - fabs(kSphereRadius *
asin(tan(radius / kSphereRadius) / tan(vertexAngle / 2.0)));
LLPoint startPt, endPt;
int k = 0;
double dErr = 0.0;
while (k == 0 || (fabs(dErr) > dTol && k <= 10))
{
distToStart = distToStart - dErr / fabs(sin(vertexAngle));
startPt = DestVincenty(pt1, crs12, distToStart);
DistVincenty(startPt, pt2, result);
result.azimuth += dir < 0 ? M_PI_2 : -M_PI_2;
centerPt = DestVincenty(startPt, result.azimuth, radius);
double dCrsFromPt, dDistFromPt;
endPt = PerpIntercept(pt3, crs3 + M_PI, centerPt, dCrsFromPt, dDistFromPt, dTol);
DistVincenty(centerPt, endPt, result);
dErr = radius - result.distance;
k++;
}
tanPt1 = startPt;
tanPt2 = endPt;
DistVincenty(pt2, tanPt2, result);
return fabs(SignAzimuthDifference(result.azimuth, crs3)) <= M_PI_2;
}
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 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;
}
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;
}
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;
}
