重叠部分的计算我没看懂
核心部分是:
// 向量标准化函数
/* norm_vector() calculates normalized vector form two points */
void NormalizeVector(double y0, double x0, double y1, double x1, double *x, double *y)
{
double dx, dy, len;
dy = y1 - y0;
dx = x1 - x0;
if ((dx == 0) && (dy == 0))
{
*x = 0;
*y = 0;
return;
}
len = LENGTH(dx, dy);
*x = dx / len;
*y = dy / len;
return;
}
void Test_NormalizeVector0(double x1, double y1, double x2, double y2)
{
double x, y;
NormalizeVector(y1,x1,y2,x2, &x, &y);
LogStr("Normalize %f,%f, %f,%f to %f, %f", x1, y1, x2, y2, x, y);
}
void Test_NormalizeVector()
{
Test_NormalizeVector0(0,0,1,0);
Test_NormalizeVector0(0,0,2,0);
Test_NormalizeVector0(0,0,1,1);
Test_NormalizeVector0(0,0,2,2);
Test_NormalizeVector0(0,0,0,2);
Test_NormalizeVector0(0,0,0,-2);
Test_NormalizeVector0(0,0,-3,-2);
Test_NormalizeVector0(0,0,-3,2);
Test_NormalizeVector0(1,1,1,0);
Test_NormalizeVector0(1,1,2,0);
Test_NormalizeVector0(1,1,1,1);
Test_NormalizeVector0(1,1,2,2);
Test_NormalizeVector0(1,1,0,2);
Test_NormalizeVector0(1,1,0,-2);
Test_NormalizeVector0(1,1,-3,-2);
Test_NormalizeVector0(1,1,-3,2);
}
// 求切点向量
void EllipticTangent(double x, double y, double *px, double *py)
{
double u =0, v=0, len;
double da = GPS_MeteringData.daFactor;
double da2 = da*da;
double db = GPS_MeteringData.dbFactor;
double db2 = db*db;
if(da != 0 || db != 0)
{
u = da2 * y;
v = -db2 * x;
len = 1/sqrt(db2*x*x+da2*y*y);
u *= len;
v *= len;
*px = u;
*py = v;
}
*px = u;
*py = v;
}
void Test_EllipticTangent0(double x, double y)
{
double tx, ty;
EllipticTangent(x, y, &tx, &ty);
LogStr("Test_EllipticTangent0 %f,%f to %f, %f", x, y, tx, ty);
}
void Test_EllipticTangent()
{
Test_EllipticTangent0(1,0);
Test_EllipticTangent0(2,0);
Test_EllipticTangent0(1,1);
Test_EllipticTangent0(2,2);
Test_EllipticTangent0(0,2);
Test_EllipticTangent0(0,-2);
Test_EllipticTangent0(-3,-2);
Test_EllipticTangent0(-3,2);
}