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
|
internal struct SxGeoPt
{
internal float lat, lon; // latitude, longitude in radians
}
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 in radians 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 class SxMath
{
internal const float PI = (float)Math.PI;
internal const float x2PI = PI * 2;
internal const float PIDiv2 = PI/2;
internal const float RadPerMin = PI / 10800;
// 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