順運動学
延々4時間デバッグに使って、やっと図2.23の順運動学まで実装できました。エラーの原因は引数の使い方にあったようです。私の勉強不足かな?今回、行列の計算があるので、自分で行列の計算の関数を作っていたのですが、void mtr_scl(float e2[3][3],float e1[3][3],float s) // e2 = e1 * s上記のように、引数の中に、配列と変数が混在していると、コンパイルが上手く出来ず、変数がちゃんと渡せない状況になっていました。で、とりあえずの解決策は、void mtr_scl(float e2[3][3],float e1[3][3],float s[1]) // e2 = e1 * s(呼び出し側および関数内は、s[0]を使う)上記のように、変数も無理やり配列表記にしたら、ちゃんと動きました。何故?それ以外は、Rodriguesの式が上手く計算できるか不安だったのですが、計算方法は考えた通りで良かったみたいで、ちゃんと座標変換してくれました。さ~って、次は、問題のヤコビアンです。逆行列の計算があるのだけど、上手く行くかな~。(--;