java - 3D 変換行列の使用

Question

AI クラスには、7 つの関節を持つ腕を持つロボットがいます。各関節は異なる方向に回転できます。終わりがどこにあるのかを知る必要があります。私は 3D マトリックス乗算を実行しようとしましたが、1 つのジョイントで機能しますが、別のジョイントを追加すると、Java3D API を使用して作成したモデルと一致しません。これが、2番目の関節の位置を計算しようとしている方法です。

double[][] b = {{0, 0, 0, 1}};

// Joint 1
b = HercMatrix.MatMul(b, HercMatrix.getTranslation(0, 0, -110));
b = HercMatrix.MatMul(b, HercMatrix.getRotation(0, arm.getJointAngle(0), 0));

// Joint 2
b = HercMatrix.MatMul(b, HercMatrix.getTranslation(0, 0, -250));
b = HercMatrix.MatMul(b, HercMatrix.getRotation(arm.getJointAngle(1), 0, 0));

System.out.println("Matrix: " + b[0][0] + ", " + b[0][1] + ", " + b[0][2]);

乗算を適用するのは彼らのやり方だと思います。私はそれが私のmatmulまたは行列生成コードではないことを知っています。私はそれをすべて個別にテストしました。

2 番目の平行移動は、最初のジョイントの角度の相対的な x 軸上にある必要があります。でも、これが私のやり方かどうかはわかりません...

どんな助けやアイデアも大歓迎です。