1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70
|
internal struct SxGeoPt
{
internal float lat, lon; // in radians, North Positive, West positive
}
internal class SxGeoCalc
{
internal const float DegPerRad = (float)(180F / Math.PI);
internal const float RadPerDeg = (float)(Math.PI / 180F);
internal static SxGeoPt GEO_CoorPA(SxGeoPt p1, float az, float raddist)
//---GEO_CoorPA------------------------------------------------------------
// This procedure provides coordinates of the point p2 located
// - at a distance raddist of a point p1
// - in the direction of azimuth az
// input p1 <SxGeoPt> coordinates of reference point
// raddist <float> distance in radian between p1 and p2
// az <float> azimut of p2 from p1,
// (az=0, if p1 and p2 on same longitude)
// output p2 <SxGeoPt> coordinates of resulting point
{
SxGeoPt result;
if (p1.lat > SxMath.PIDiv2 - SxMath.RadPerMin)
{ if (az <= SxMath.PI) result.lon = az; else result.lon = az - SxMath.PI; result.lat = SxMath.PIDiv2 - raddist; }
else if (p1.lat < -SxMath.PIDiv2 + SxMath.RadPerMin)
{ if (az <= SxMath.PI) result.lon = -az; else result.lon = -az + SxMath.PI; result.lat = -SxMath.PIDiv2 + raddist; }
else
{
result.lat = (float)Math.Asin((Math.Sin(p1.lat) * Math.Cos(raddist)) +
(Math.Cos(p1.lat) * Math.Sin(raddist) * Math.Cos(az)));
float dlon = (float)Math.Atan2(Math.Sin(az) * Math.Sin(raddist) * Math.Cos(p1.lat),
Math.Cos(raddist) - Math.Sin(p1.lat) * Math.Sin(result.lat));
result.lon = SxMath.RealMod(p1.lon - dlon + SxMath.PI, SxMath.x2PI) - SxMath.PI;
}
return result;
}
internal static float GEO_AzimPtPt(SxGeoPt p1, SxGeoPt p2)
// This function provides the direction of the segment joinning 2 points
// p1 and p2 defined by their geographic coordinates.
// p2 is in azimut GEO_AZIMPP(p1,p2) of p1
// input p1,p2 <SxGeoPt> coordinates of the 2 points
// result <single> angle in radians with North direction (clockwize) : azimuth of p2 from p1
{
float coslat1 = (float)Math.Cos(p1.lat);
if (coslat1 > 0.000001)
return SxMath.RealMod((float)Math.Atan2
(Math.Sin(p1.lon - p2.lon) * Math.Cos(p2.lat),
coslat1 * Math.Sin(p2.lat) - Math.Sin(p1.lat) * Math.Cos(p2.lat) * Math.Cos(p1.lon - p2.lon)),
SxMath.x2PI);
else if (p1.lat > 0) return SxMath.PI;
else return 0;
}
}
internal class SxMath
{
internal const float PI = (float)Math.PI;
internal const float x2PI = PI * 2;
internal const float PIDiv2 = PI/2;
// Example : RealMod(3,2*PI)=3 / RealMod(2*PI+3,2*PI)=3 / RealMod(-3,2*PI)=2*PI-3
internal static float RealMod(float val,float modval)
{
float result = (float)Math.IEEERemainder(val,modval);
if (result<0) result = result + modval;
return result;
}
} |
Partager