我计算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
}