56 lines
1.3 KiB
Java
56 lines
1.3 KiB
Java
/**
|
|
* Calculate the angle defined by 3 points
|
|
* (and deliver it's cosine on the fly)
|
|
*/
|
|
package btools.util;
|
|
|
|
public final class CheapAngleMeter
|
|
{
|
|
private double cosangle;
|
|
|
|
public double getCosAngle()
|
|
{
|
|
return cosangle;
|
|
}
|
|
|
|
public double calcAngle( int lon0, int lat0, int lon1, int lat1, int lon2, int lat2 )
|
|
{
|
|
double[] lonlat2m = CheapRuler.getLonLatToMeterScales( lat1 );
|
|
double lon2m = lonlat2m[0];
|
|
double lat2m = lonlat2m[1];
|
|
double dx10 = (lon1 - lon0) * lon2m;
|
|
double dy10 = (lat1 - lat0) * lat2m;
|
|
double dx21 = (lon2 - lon1) * lon2m;
|
|
double dy21 = (lat2 - lat1) * lat2m;
|
|
|
|
double dd = Math.sqrt( (dx10*dx10 + dy10*dy10)*(dx21*dx21 + dy21*dy21) );
|
|
if ( dd == 0. ) { cosangle = 1.; return 0.; }
|
|
double sinp = (dy10*dx21 - dx10*dy21)/dd;
|
|
double cosp = (dy10*dy21 + dx10*dx21)/dd;
|
|
cosangle = cosp;
|
|
|
|
double offset = 0.;
|
|
double s2 = sinp*sinp;
|
|
if ( s2 > 0.5 )
|
|
{
|
|
if ( sinp > 0. )
|
|
{
|
|
offset = 90.;
|
|
sinp = -cosp;
|
|
}
|
|
else
|
|
{
|
|
offset = -90.;
|
|
sinp = cosp;
|
|
}
|
|
s2 = cosp*cosp;
|
|
}
|
|
else if ( cosp < 0. )
|
|
{
|
|
sinp = -sinp;
|
|
offset = sinp > 0. ? -180. : 180.;
|
|
}
|
|
return offset + sinp * ( 57.4539 + s2 * ( 9.57565 + s2 * ( 4.30904 + s2 * 2.56491 ) ) );
|
|
}
|
|
}
|