计算3D中大矢量之间的小角度

时间:2016-02-16 16:53:39

标签: c++ geolocation precision trigonometry

我计算3D中大矢量之间的小角度。

我使用通用公式:

double angle = atan2(norm(cross_product), dot_product);

一般情况下,我试图在地球上的矢量和地球上的椭球(引力矢量)之间找到角度,使我的矢量与引力矢量对齐。

我将我的矢量存储在ECEF坐标中。

价值观是关于:

normal_ecef[2853169.5286430484, 2139463.4659523461, 5254245.1411450412]
v_ecef[11.4893763275, 9.3040978122, 16.5129716340]
cross_product [13557111.1969175711, -13253692.2498887852, -1965067.4699216224]
dot_product 139450116.7675085962
Angle : 7.7832983103

理论上角度应该很小,所以我想在这里我们可能有一些精确损失的情况?

更新

我尝试了常规载体,但它没有帮助:

未正常化:

v1[11.4893763275, 9.3040978122, 16.5129716340]
v2[2853169.5286430484, 2139463.4659523461, 5254245.1411450412]
cross_product [13557111.1969175711, -13253692.2498887852, -1965067.4699216224]
dot_product 139450116.7675085962
angle 7.7832983103

归一化:

v1[11.4893763275, 9.3040978122, 16.5129716340]
v2[2853169.5286430484, 2139463.4659523461, 5254245.1411450412]
nv1[0.5183762175, 0.4197810998, 0.7450301506]
nv2[0.4493042622, 0.3369130521, 0.8274148145]
cross_product [0.0963227189, -0.0941669398, -0.0139617238]
dot_product 0.9907873594
angle 7.7832983103

完整代码:

double ComputeAngle(const Vec3d &v1, const Vec3d &v2)
{
    using namespace boost::math::double_constants;

    cout<< "v1" << v1 <<endl;
    cout<< "v2" << v2 <<endl;

    //ver1
    double dot_product= DotProduct(v1, v2);

    if(CloseEnough(dot_product, 0.0))
        return 90.0;

    Vec3d cross_product= CrossProduct(v1, v2);

    double angle = atan2(norm(cross_product), dot_product);

    cout<< "cross_product " << cross_product <<endl;
    cout<< "dot_product " << dot_product <<endl;

    cout<<"angle "<<angle*(180/pi)<<endl;//

    //ver2
    double angle;
    Vec3d nv1= v1/norm(v1);
    Vec3d nv2= v2/norm(v2);
    cout<< "nv1" << nv1<<endl;//
    cout<< "nv2" << nv2<<endl;//
    double dot_product= DotProduct(nv1, nv2);
    if(CloseEnough(dot_product, 0.0))
        angle= 90.0;
    Vec3d cross_product= CrossProduct(nv1, nv2);
    cout<< "cross_product " << cross_product <<endl;//
    cout<< "dot_product " << dot_product <<endl;//
    angle = atan2(norm(cross_product), dot_product);
    cout<<"angle "<<angle*(180/pi)<<endl;//del

    return angle*(180/pi); //return in degrees
}

0 个答案:

没有答案