double DiscretizedArcLength(const LLPoint ¢er, double dRadius,
double dStartCrs, double dEndCrs,
int nOrient, int nSegments, double dTol)
{
nSegments = clamp(nSegments, 1, 128);
double dError = 0.0;
double dArcLength = 0.0;
double dOldArcLength = 0.0;
const double dSubtendedAngle = GetArcExtent(dStartCrs, dEndCrs, nOrient, dTol);
// with k equal to 0 then there will only be nSegments subsegments calculated.
// need to figure out how to make this flexible based on the value in dRadius.
// Bigger radius need more segments than 16 and smaller segments need less.
// For now 16 is enough to pass the 8260.54A test case.
int k = 0;
while (k == 0 || ((dError > kTol) && (k <= 0)))
{
const double dTheta = dSubtendedAngle / nSegments;
const double dAltitude = 0.0;
dArcLength = 0.0;
for (int i = 0; i < nSegments; i++)
{
const double theta = dStartCrs + i * dTheta;
const LLPoint p1 = DestVincenty(center, theta, dRadius);
const LLPoint p2 = DestVincenty(center, theta + 0.5 * dTheta, dRadius);
const LLPoint p3 = DestVincenty(center, theta + dTheta, dRadius);
const VMath::Vector3 v1 = ECEF(p1, dAltitude);
const VMath::Vector3 v2 = ECEF(p2, dAltitude);
const VMath::Vector3 v3 = ECEF(p3, dAltitude);
const VMath::Vector3 vChord1 = v2 - v1;
const VMath::Vector3 vChord2 = v2 - v3;
const double x1 = VMath::Vector3::Length(vChord1); //v2 - v1);
const double x2 = VMath::Vector3::Length(vChord2); //v2 - v3);
const double d = VMath::Vector3::Dot(vChord1, vChord2);
if (IsNearZero(x1, kTol) && IsNearZero(x2, kTol) && IsNearZero(d, kTol))
{
dArcLength = 0.0;
break;
}
const double xi = d / (x1 * x2);
const double _x1_x2Sq = x1 / x2 - xi;
const double sigma = sqrt(1.0 - (xi * xi));
const double R = (x2 * sqrt((_x1_x2Sq * _x1_x2Sq) + (sigma * sigma))) / (2.0 * sigma);
const double A = 2 * (M_PI - acos(xi));
const double L = R * A;
dArcLength += L;
}
nSegments *= 2;
dError = fabs(dArcLength - dOldArcLength);
dOldArcLength = dArcLength;
k++;
}
return dArcLength;
}
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;
}
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));
}
/**
* Calculates destination point given start point lat/long, bearing & distance,
* using Vincenty inverse formula for ellipsoids
*
* -> double lat1, lon1: first point in decimal degrees
* -> double brng: initial bearing in decimal degrees
* -> double dist: distance along bearing in nautical miles
* <- double lat2, lat2 final point in decimal degrees
* <- double final bearing in decimal degrees
*/
void DestVincenty(double lat1, double lon1, double brng, double distNM, double* lat2, double* lon2, double* revAz) {
LLPoint pt;
pt.latitude=toRad(lat1);
pt.longitude=toRad(lon1);
double distm=NmToMeters(distNM);
double brngrad=toRad(brng);
double revAzrad;
LLPoint Result_dest=DestVincenty(pt, brngrad, distm, &revAzrad );
*lat2=toDeg(Result_dest.latitude);
*lon2=toDeg(Result_dest.longitude);
*revAz =toDeg(revAzrad); // final bearing, if required
}
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;
}
/**
* Calculates destination point given start point lat/long, bearing & distance,
* using Vincenty inverse formula for ellipsoids
*
* -> pt LLpoint in radius
* -> double brng: initial bearing in radius
* -> double dist: distance along bearing in metres
* <- pt (double lat2, lat2) final point in radius
* <- double final bearing in radius
*/
LLPoint DestVincenty(LLPoint pt, double brng, double dist)
{
double dummy_revAz;
return DestVincenty(pt, brng, dist, &dummy_revAz);
}
