Calculating small angle between large vectors in 3

2019-09-10 16:03发布

问题:

I'm calculating small angle between large vectors in 3D.

I use common formula:

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

In general I'm trying to get angle between my vector on Earth and Normal to Earth elipsoid(gravitational vector) to align my vector to gravitational vector.

I store my vector in ECEF coordinates.

And values are about:

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

Theoretically angle should be very small, so I wonder maybe here we have some case of precision loss?

UPDATE:

I tried normalie vectors, but it doesn't help:

not normalized:

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

normalized:

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

Full code:

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
}