使用 3d 圆的参数方程旋转矢量
rotating a vector using parametric equation of 3d circle
我没有得到所需的输出,即下面代码中的 x2、y2、z2。这里的角度t是度数,(x2,y2,z2)是向量面旋转后的pint,(a2,b2,c2)是旋转后vector.I的方向余弦,用rviz可视化旋转矢量,但它没有像想象的那样出现,即它没有旋转。
double xgp[ipgp] = 23, ygp[ipgp] = 45, zgp[ipgp] = 345; int ipgp = 0;
double a2, b2 , c2, a1, b1, c1 = ...
double la = 0.0032;
for(double t=0;t<360;t+=la)
{
double x2 = xgp[ipgp]+a2*cos(t)+a1*sin(t);
double y2 = ygp[ipgp]+b2*cos(t)+b1*sin(t);
double z2 = zgp[ipgp]+c2*cos(t)+c1*sin(t);
a2 = x2-xgp[ipgp]; // vector (xgp,ygp,zgp)->(x2,y2,z2)
b2 = y2-ygp[ipgp];
c2 = z2-zgp[ipgp];
int i = round(sqrt(pow(a2,2)+pow(b2,2)+pow(c2,2)));
std::cout<<i<<std::endl;
}
您发布的代码有两个问题:
- 您没有指定如何设置
(a1,b1,c1)
和 (a2,b2,c2)
。这些必须是:
- 正交于
(xgp,ygp,zgp)
- 彼此正交
- 单位向量
- 在循环中,您将
(a2,b2,c2)
重置为旋转的单位向量。您不能那样做,并且在循环的下一步中仍然保持 (a1,b1,c1)
和 (a2,b2,c2)
之间的正交性。这可能是为什么你打印出来的 i
不全是 1
的 的主要原因
以下代码可以满足您的需求
#include <iostream>
#include <math.h>
int main() {
double xgp = 23., ygp = 45., zgp = 345.;
// setting (a2,b2,c2) to be orthogonal to (xgp,ygp,zgp) with unit magnitude
double a2 = -45./sqrt(23.*23. + 45.*45.);
double b2 = 23./sqrt(23.*23. + 45.*45.);
double c2 = 0.;
// setting (a1,b1,c1) to be orthogonal to both (a2,b2,c2) and (xgp,ygp,zgp)
// using cross product (xgp,ygp,zgp) X (a2,b2,c2) with unit magnitude
double a1 = -23.*345./sqrt(23.*345.*23.*345. + 45.*345.*45.*345. + (23.*23. + 45.*45.)*(23.*23. + 45.*45.));
double b1 = -45.*345./sqrt(23.*345.*23.*345. + 45.*345.*45.*345. + (23.*23. + 45.*45.)*(23.*23. + 45.*45.));
double c1 = (23.*23. + 45.*45.)/sqrt(23.*345.*23.*345. + 45.*345.*45.*345. + (23.*23. + 45.*45.)*(23.*23. + 45.*45.));
double la = 0.0032;
for(double t=0;t<360;t+=la) {
double x2 = xgp+a2*cos(t)+a1*sin(t);
double y2 = ygp+b2*cos(t)+b1*sin(t);
double z2 = zgp+c2*cos(t)+c1*sin(t);
// cannot overwrite a2, b2, c2; otherwise (a2,b2,c2) is no longer orthogonal to (a1,b1,c1)!
double a3 = x2-xgp; // vector (xgp,ygp,zgp)->(x2,y2,z2)
double b3 = y2-ygp;
double c3 = z2-zgp;
// (a3,b3,c3) is a unit vector!
int i = round(sqrt(pow(a3,2)+pow(b3,2)+pow(c3,2)));
std::cout<<i<<std::endl;
}
return(0)
}
请注意,就您的问题而言,以度数或弧度为单位旋转并不重要。但是,您应该遵循@Bob 的建议,并确保您自己知道您打算使用什么单位。
希望这对您有所帮助。
我没有得到所需的输出,即下面代码中的 x2、y2、z2。这里的角度t是度数,(x2,y2,z2)是向量面旋转后的pint,(a2,b2,c2)是旋转后vector.I的方向余弦,用rviz可视化旋转矢量,但它没有像想象的那样出现,即它没有旋转。
double xgp[ipgp] = 23, ygp[ipgp] = 45, zgp[ipgp] = 345; int ipgp = 0;
double a2, b2 , c2, a1, b1, c1 = ...
double la = 0.0032;
for(double t=0;t<360;t+=la)
{
double x2 = xgp[ipgp]+a2*cos(t)+a1*sin(t);
double y2 = ygp[ipgp]+b2*cos(t)+b1*sin(t);
double z2 = zgp[ipgp]+c2*cos(t)+c1*sin(t);
a2 = x2-xgp[ipgp]; // vector (xgp,ygp,zgp)->(x2,y2,z2)
b2 = y2-ygp[ipgp];
c2 = z2-zgp[ipgp];
int i = round(sqrt(pow(a2,2)+pow(b2,2)+pow(c2,2)));
std::cout<<i<<std::endl;
}
您发布的代码有两个问题:
- 您没有指定如何设置
(a1,b1,c1)
和(a2,b2,c2)
。这些必须是:- 正交于
(xgp,ygp,zgp)
- 彼此正交
- 单位向量
- 正交于
- 在循环中,您将
(a2,b2,c2)
重置为旋转的单位向量。您不能那样做,并且在循环的下一步中仍然保持(a1,b1,c1)
和(a2,b2,c2)
之间的正交性。这可能是为什么你打印出来的i
不全是1
的 的主要原因
以下代码可以满足您的需求
#include <iostream>
#include <math.h>
int main() {
double xgp = 23., ygp = 45., zgp = 345.;
// setting (a2,b2,c2) to be orthogonal to (xgp,ygp,zgp) with unit magnitude
double a2 = -45./sqrt(23.*23. + 45.*45.);
double b2 = 23./sqrt(23.*23. + 45.*45.);
double c2 = 0.;
// setting (a1,b1,c1) to be orthogonal to both (a2,b2,c2) and (xgp,ygp,zgp)
// using cross product (xgp,ygp,zgp) X (a2,b2,c2) with unit magnitude
double a1 = -23.*345./sqrt(23.*345.*23.*345. + 45.*345.*45.*345. + (23.*23. + 45.*45.)*(23.*23. + 45.*45.));
double b1 = -45.*345./sqrt(23.*345.*23.*345. + 45.*345.*45.*345. + (23.*23. + 45.*45.)*(23.*23. + 45.*45.));
double c1 = (23.*23. + 45.*45.)/sqrt(23.*345.*23.*345. + 45.*345.*45.*345. + (23.*23. + 45.*45.)*(23.*23. + 45.*45.));
double la = 0.0032;
for(double t=0;t<360;t+=la) {
double x2 = xgp+a2*cos(t)+a1*sin(t);
double y2 = ygp+b2*cos(t)+b1*sin(t);
double z2 = zgp+c2*cos(t)+c1*sin(t);
// cannot overwrite a2, b2, c2; otherwise (a2,b2,c2) is no longer orthogonal to (a1,b1,c1)!
double a3 = x2-xgp; // vector (xgp,ygp,zgp)->(x2,y2,z2)
double b3 = y2-ygp;
double c3 = z2-zgp;
// (a3,b3,c3) is a unit vector!
int i = round(sqrt(pow(a3,2)+pow(b3,2)+pow(c3,2)));
std::cout<<i<<std::endl;
}
return(0)
}
请注意,就您的问题而言,以度数或弧度为单位旋转并不重要。但是,您应该遵循@Bob 的建议,并确保您自己知道您打算使用什么单位。
希望这对您有所帮助。