全て
| カテゴリ未分類
SH4
| AX-12
| シミュレータ
| 夏休み
| あまね
| onPC/sat.
| FPGA
| ロボットビジョン
| SEMB1200A
| 新しい機体
| SH7125
| onPC4th
| 蹴飛ばしても倒れないロボット
| フィールドウォーカー
| 新・新しい機体
| SH7786
| Linux
| Arduino
2020年10月11日
カテゴリ： Linux
仕事が忙しく、思うように進まない・・・
前回エクセルで確認した胴体の角速度ベクトルの計算から、空間速度の計算まで、neon化してみたのだけど、
どうも、外積の計算は、neonには向いていない気がする・・・。
一応、エクセルで確認計算までしてｏｋだったけど、今一、間違っていない自信がない。
空間速度って値が、こう、実感わかないのだよね～。
コードは、以下のようです。
前回エクセルで確認した胴体の角速度ベクトルの計算から、空間速度の計算まで、neon化してみたのだけど、
どうも、外積の計算は、neonには向いていない気がする・・・。
一応、エクセルで確認計算までしてｏｋだったけど、今一、間違っていない自信がない。
空間速度って値が、こう、実感わかないのだよね～。
コードは、以下のようです。
r0 = vld1q_f32( &link[1].Rp[0] ); // 古い
r1 = vld1q_f32( &link[1].Rp[4] );
r2 = vld1q_f32( &link[1].Rp[8] );
c0 = vld1q_f32( &body.Rp[0] ); // 新しい
c1 = vld1q_f32( &body.Rp[4] );
c2 = vld1q_f32( &body.Rp[8] );
vst1q_f32( &link[1].Rp[0], c0 ); // 更新
vst1q_f32( &link[1].Rp[4], c1 );
vst1q_f32( &link[1].Rp[8], c2 );
c0 = vsubq_f32( c0, r0 );
c1 = vsubq_f32( c1, r1 );
c2 = vsubq_f32( c2, r2 );
c0 = vmulq_n_f32(c0,Tra.inv_dt); // dR
c1 = vmulq_n_f32(c1,Tra.inv_dt);
c2 = vmulq_n_f32(c2,Tra.inv_dt);
r3 = vld1q_f32( &link[1].w[0] ); // 古い
r0 = vmulq_n_f32( c0, link[1].Rp[0] );
r1 = vmulq_n_f32( c1, link[1].Rp[4] );
r2 = vmulq_n_f32( c2, link[1].Rp[8] );
r0 = vaddq_f32( r0, r1 );
r0 = vaddq_f32( r0, r2 );
link[1].w[2] = vgetq_lane_f32(r0,1);
r0 = vmulq_n_f32( c0, link[1].Rp[1] );
r1 = vmulq_n_f32( c1, link[1].Rp[5] );
r2 = vmulq_n_f32( c2, link[1].Rp[9] );
r0 = vaddq_f32( r0, r1 );
r0 = vaddq_f32( r0, r2 );
link[1].w[0] = vgetq_lane_f32(r0,2);
r0 = vmulq_n_f32( c0, link[1].Rp[2] );
r1 = vmulq_n_f32( c1, link[1].Rp[6] );
r2 = vmulq_n_f32( c2, link[1].Rp[10] );
r0 = vaddq_f32( r0, r1 );
r0 = vaddq_f32( r0, r2 );
link[1].w[1] = vgetq_lane_f32(r0,0);
link[1].w[3] = 0.0;
r0 = vld1q_f32( &link[1].w[0] ); // 新しい
r0 = vsubq_f32( r0, r3 );
r0 = vmulq_n_f32(r0,Tra.inv_dt);
vst1q_f32( &link[1].dw[0], r0 ); // 更新
c0 = vld1q_f32( &link[1].Rp[12] ); // 古い
r0 = vld1q_f32( &body.Rp[12] ); // 新しい
vst1q_f32( &link[1].Rp[12], r0 ); // 更新
r3 = vsubq_f32( r0, c0 );
r3 = vmulq_n_f32(r3,Tra.inv_dt); // dp
// 外積
c0 = vdupq_n_f32(0.0);
c1 = vdupq_n_f32(0.0);
c2 = vdupq_n_f32(0.0);
r0 = vld1q_f32( &link[1].w[0] );
c0 = vsetq_lane_f32( vgetq_lane_f32(r0,2),c0,1);
c0 = vsetq_lane_f32(-vgetq_lane_f32(r0,1),c0,2);
c1 = vsetq_lane_f32(-vgetq_lane_f32(r0,2),c1,0);
c1 = vsetq_lane_f32( vgetq_lane_f32(r0,0),c1,2);
c2 = vsetq_lane_f32( vgetq_lane_f32(r0,1),c2,0);
c2 = vsetq_lane_f32(-vgetq_lane_f32(r0,0),c2,1);
r0 = vmulq_n_f32(c0,-link[1].Rp[12]); //
r1 = vmulq_n_f32(c1,-link[1].Rp[13]); //
r2 = vmulq_n_f32(c2,-link[1].Rp[14]); //
r0 = vaddq_f32(r0,r1);
r2 = vaddq_f32(r2,r3); //
r0 = vaddq_f32(r0,r2);
c0 = vld1q_f32( &link[1].v0[0] ); // 古い
vst1q_f32( &link[1].v0[0], r0 ); // 更新
r0 = vsubq_f32( r0, c0 );
r0 = vmulq_n_f32(r0,Tra.inv_dt);
vst1q_f32( &link[1].dv0[0], r0 ); // 更新
お気に入りの記事を「いいね！」で応援しよう
最終更新日
2020年10月11日 21時30分51秒
[Linux] カテゴリの最新記事
-
MPU-9250 2020年11月14日
-
上手く動かない理由（その２） 2020年11月08日
-
上手く動かない理由 2020年11月07日
【毎日開催】
15記事にいいね！で1ポイント
© Rakuten Group, Inc.
