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); }