...
if (joint->num >= 2) {
axes[0][0] += axes[1][0] * torque2;
axes[0][1] += axes[1][0] * torque2;
^ Should be 1
axes[0][2] += axes[1][0] * torque2;
^ Should be 2
if (joint->num >= 3) {
axes[0][0] += axes[2][0] * torque3;
axes[0][1] += axes[2][0] * torque3;
^ Should be 1
axes[0][2] += axes[2][0] * torque3;
^ Should be 2
}
}
...
Peng