// calculate distance between two points in earth
// will not 100% accurate
double GeoCalc::geodeticDistance(double lat1, double lon1, double alt1, double lat2, double lon2, double alt2)
{
bool isInputError = false;
// Check inputs
// http://docs.obspy.org/coverage/_opt_obspy_python_src_obspy_obspy_signal_obspy_signal_rotate.html
if (lat1 > 90 || lat1 < -90)
{
DebugInfo("Latitude of point 1 out of bounds (-90 <= lat1 <=90)", DebugInfo::Warning);
isInputError = true;
}
while (lon1 > 180)
lon1 -= 360;
while (lon1 < -180)
lon1 += 360;
if (lat2 > 90 || lat2 < -90)
{
DebugInfo("Latitude of point 2 out of bounds (-90 <= lat2 <=90)", DebugInfo::Warning);
isInputError = true;
}
while (lon2 > 180)
lon2 -= 360;
while (lon2 < -180)
lon2 += 360;
// ardhi's
if(alt1 < -6340000) // approximate earth's core depth
{
DebugInfo("Altitude of point 1 out of bounds (alt1 >= -6340000)", DebugInfo::Warning);
isInputError = true;
}
if(alt2 < -6340000) // approximate earth's core depth
{
DebugInfo("Altitude of point 2 out of bounds (alt1 >= -6340000)", DebugInfo::Warning);
isInputError = true;
}
if(isInputError)
return -1;
if ((abs(lat1 - lat2) < 1e-8) && (abs(lon1 - lon2) < 1e-8) && (abs(alt1 - alt1) < 1e-8)) // 1e8 = 0.00000001
return 0.0f;
/* http://www.faqs.org/faqs/geography/infosystems-faq/
Pythagorean Theorem
d = sqrt((X2 - X1)^2 + (Y2 - Y1)^2)
will result in an error of
less than 30 meters (100 ft) for latitudes less than 70 degrees
less than 20 meters ( 66 ft) for latitudes less than 50 degrees
less than 9 meters ( 30 ft) for latitudes less than 30 degrees
*/
if(lat1 > 30 || lat1 < -30 || lat2 > 30 || lat2 < -30) // vector based distance will results calculation error > 9 meters if latitude is < 30 degrees
return greatCircleDistanceHaversine(lat1, lon1, lat2, lon2);
if(vectorDistance(lat1, lon1, 0, lat2, lon2, 0) > 5000) // is distance > 5 km, use haversine method, but neglects altitude
return greatCircleDistanceHaversine(lat1, lon1, lat2, lon2);
return vectorDistance(lat1, lon1, alt1, lat2, lon2, alt2);
}
static float heuristic( const BTriangle *triangle, const BVector *destination )
{
assert( triangle != NULL );
assert( destination != NULL );
const float cost = ( float )vectorDistance( &triangle->center, destination );
return cost;
}
static float movementCost( const BTriangle *triangleA, const BTriangle *triangleB )
{
assert( triangleA != NULL );
assert( triangleB != NULL );
assert( triangleA != triangleB );
const float cost = ( float )vectorDistance( &triangleA->center, &triangleB->center );
assert( cost > 0 );
return cost;
}
double vectorDistance(const GPoint & pt) {
return vectorDistance(pt.getX(), pt.getY());
}
double dtw_c(double *s, double *t, int w, int ns, int nt, int k)
{
double d=0;
int sizediff=ns-nt>0 ? ns-nt : nt-ns;
double ** D;
int i,j;
int j1,j2;
double cost,temp;
// printf("ns=%d, nt=%d, w=%d, s[0]=%f, t[0]=%f\n",ns,nt,w,s[0],t[0]);
if(w!=-1 && w<sizediff) w=sizediff; // adapt window size
// create D
D=(double **)malloc((ns+1)*sizeof(double *));
for(i=0;i<ns+1;i++)
{
D[i]=(double *)malloc((nt+1)*sizeof(double));
}
// initialization
for(i=0;i<ns+1;i++)
{
for(j=0;j<nt+1;j++)
{
D[i][j]=-1;
}
}
D[0][0]=0;
// dynamic programming
for(i=1;i<=ns;i++)
{
if(w==-1)
{
j1=1;
j2=nt;
}
else
{
j1= i-w>1 ? i-w : 1;
j2= i+w<nt ? i+w : nt;
}
for(j=j1;j<=j2;j++)
{
cost=vectorDistance(s,t,ns,nt,k,i-1,j-1);
temp=D[i-1][j];
if(D[i][j-1]!=-1)
{
if(temp==-1 || D[i][j-1]<temp) temp=D[i][j-1];
}
if(D[i-1][j-1]!=-1)
{
//if(temp==-1 || D[i-1][j-1]<temp) temp=D[i-1][j-1];
if(temp==-1 || (D[i-1][j-1] + cost) < temp) temp=D[i-1][j-1]+cost;
}
D[i][j]=cost+temp;
}
}
d=D[ns][nt];
/* view matrix D */
/*
for(i=0;i<ns+1;i++)
{
for(j=0;j<nt+1;j++)
{
printf("%f ",D[i][j]);
}
printf("\n");
}
*/
// free D
for(i=0;i<ns+1;i++)
{
free(D[i]);
}
free(D);
//return d;
return d/(ns+nt);
}
