.-- --

スポンサーサイト

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。
スポンサー広告 comment(-) trackback(-)
.25 2013

Eigenでクォータニオン理解

こんにちわ。Uedaです。

今回はEigenを使ってクォータニオンを作ってみました。
前回、回転行列を作ったので、いけるだろーと思っていました...
が、
符号が合わないやら、行列の要素逆に格納してしまうわで、てこずってしまいました。。

以下コードです。


#ifdef _quaternion_

double p = 0.5 -[self random] / 10.0;
double y = 0.5 - [self random] / 10.0;
double r = 0.5 - [self random] / 10.0;
Matrix3d R_pitch;
R_pitch(0, 0) = 1.0; R_pitch(0, 1) = 0.0; R_pitch(0, 2) = 0.0;
R_pitch(1, 0) = 0.0; R_pitch(1, 1) = cos(p); R_pitch(1, 2) = -sin(p);
R_pitch(2, 0) = 0.0; R_pitch(2, 1) = sin(p); R_pitch(2, 2) = cos(p);
Matrix3d R_yaw;
R_yaw(0, 0) = cos(y); R_yaw(0, 1) = 0.0; R_yaw(0, 2) = sin(y);
R_yaw(1, 0) = 0.0; R_yaw(1, 1) = 1.0; R_yaw(1, 2) = 0.0;
R_yaw(2, 0) = -sin(y); R_yaw(2, 1) = 0.0; R_yaw(2, 2) = cos(y);
Matrix3d R_roll;
R_roll(0, 0) = cos(r); R_roll(0, 1) = -sin(r); R_roll(0, 2) = 0.0;
R_roll(1, 0) = sin(r); R_roll(1, 1) = cos(r); R_roll(1, 2) = 0.0;
R_roll(2, 0) = 0.0; R_roll(2, 1) = 0.0; R_roll(2, 2) = 1.0;
MatrixXd R = R_roll * R_pitch * R_yaw;

double q0, q1, q2, q3;
double r11 = R(0, 0); double r12 = R(0, 1); double r13 = R(0, 2);
double r21 = R(1, 0); double r22 = R(1, 1); double r23 = R(1, 2);
double r31 = R(2, 0); double r32 = R(2, 1); double r33 = R(2, 2);

q0 = ( r11 + r22 + r33 + 1.0f) / 4.0f;
q1 = ( r11 - r22 - r33 + 1.0f) / 4.0f;
q2 = (-r11 + r22 - r33 + 1.0f) / 4.0f;
q3 = (-r11 - r22 + r33 + 1.0f) / 4.0f;
if(q0 < 0.0f) q0 = 0.0f;
if(q1 < 0.0f) q1 = 0.0f;
if(q2 < 0.0f) q2 = 0.0f;
if(q3 < 0.0f) q3 = 0.0f;
q0 = sqrt(q0);
q1 = sqrt(q1);
q2 = sqrt(q2);
q3 = sqrt(q3);
if(q0 >= q1 && q0 >= q2 && q0 >= q3) {
q0 *= +1.0f;
q1 *= 1.0 - 2.0 * signbit(r32 - r23);
q2 *= 1.0 - 2.0 * signbit(r13 - r31);
q3 *= 1.0 - 2.0 * signbit(r21 - r12);
} else if(q1 >= q0 && q1 >= q2 && q1 >= q3) {
q0 *= 1.0 - 2.0 * signbit(r32 - r23);
q1 *= +1.0f;
q2 *= 1.0 - 2.0 * signbit(r21 + r12);
q3 *= 1.0 - 2.0 * signbit(r13 + r31);
} else if(q2 >= q0 && q2 >= q1 && q2 >= q3) {
q0 *= 1.0 - 2.0 * signbit(r13 - r31);
q1 *= 1.0 - 2.0 * signbit(r21 + r12);
q2 *= +1.0f;
q3 *= 1.0 - 2.0 * signbit(r32 + r23);
} else if(q3 >= q0 && q3 >= q1 && q3 >= q2) {
q0 *= 1.0 - 2.0 * signbit(r21 - r12);
q1 *= 1.0 - 2.0 * signbit(r31 + r13);
q2 *= 1.0 - 2.0 * signbit(r32 + r23);
q3 *= +1.0f;
} else {
}
double normalize = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= normalize;
q1 /= normalize;
q2 /= normalize;
q3 /= normalize;

double sx = q0 * q0;
double sy = q1 * q1;
double sz = q2 * q2;
double cx = q1 * q2;
double cy = q0 * q2;
double cz = q0 * q1;
double wx = q3 * q0;
double wy = q3 * q1;
double wz = q3 * q2;

Matrix3d Rq(3, 3);

Rq(2, 2) = 1.0 - 2.0 * (sy + sz);
Rq(2, 1) = 2.0 * (cz + wz);
Rq(2, 0) = -(2.0 * (cy - wy));
Rq(1, 2) = -(2.0 * (cz - wz));
Rq(1, 1) = -(1.0 - 2.0 * (sx + sz));
Rq(1, 0) = 2.0 * (cx + wx);
Rq(0, 2) = 2.0 * (cy + wy);
Rq(0, 1) = 2.0 * (cx - wx);
Rq(0, 0) = -(1.0 - 2.0 * (sx + sy));

for (int i = 0; i < 3; i++)
{
NSLog(@"%f,%f,%f", R(i, 0), R(i, 1), R(i, 2));
}
NSLog(@"\n");
for (int i = 0; i < 3; i++)
{
NSLog(@"%f,%f,%f", Rq(i, 0), Rq(i, 1), Rq(i, 2));
}

#endif


出力結果
2013-04-25 17:25:01.295 TestEigen[5495:c07] 0.823784,-0.309281,0.475105
2013-04-25 17:25:01.297 TestEigen[5495:c07] 0.417921,0.897580,-0.140332
2013-04-25 17:25:01.297 TestEigen[5495:c07] -0.383043,0.314159,0.868667
2013-04-25 17:25:01.298 TestEigen[5495:c07]
2013-04-25 17:25:01.299 TestEigen[5495:c07] 0.823784,-0.309281,0.475105
2013-04-25 17:25:01.300 TestEigen[5495:c07] 0.417921,0.897580,-0.140332
2013-04-25 17:25:01.300 TestEigen[5495:c07] -0.383043,0.314159,0.868667

値は合ってますが、すみません、プログラムは自信ありません。。
関連記事
スポンサーサイト

Comment

Post comment

  • comment
  • secret
  • 管理者にだけ表示を許可する

Trackback

trackbackURL:http://appteam.blog114.fc2.com/tb.php/251-548e4bf3

ブログ内検索

関連リンク

製品情報

最新記事

カテゴリ

プロフィール

neoxneo



NEXT-SYSTEM iOS Developers Blog


  • UTO:
    カナダ版iPhone4Sは、マナーモードでシャッター音がならない…


  • Ehara:
    ...


  • Hayate:
    ...


  • Tasaki:
    Developer登録完了...したのはいいけど


  • Ueda:
    ...



リンク

上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。