0

以下のコードでは、必要な出力、つまり x2、y2、z2 が得られません。ここで、角度 t は度単位で、(x2,y2,z2) は回転後のベクトル面のパイントであり、(a2,b2,c2) は回転したベクトルの方向余弦です。回転ベクトルを視覚化するために 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;
    }
4

1 に答える 1

0

投稿したコードには 2 つの問題があります。

  1. (a1,b1,c1)およびの設定方法を指定していません(a2,b2,c2)。これらは次のとおりである必要があります。
    • 直交する(xgp,ygp,zgp)
    • 互いに直交する
    • 単位ベクトル
  2. ループでは(a2,b2,c2)、回転した単位ベクトルにリセットします。それを行うことはできませんが、ループの次のステップとの(a1,b1,c1)間で直交性を維持します。これがおそらく、あなたの印刷物がすべての(a2,b2,c2)ものではない主な理由です。i1

次のコードは、意図したことを行います

#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 のアドバイスに従い、意図している単位を自分で確認する必要があります。

お役に立てれば。

于 2016-07-16T20:32:31.640 に答える