簡單的FK(Forward Kinematics)Algorithm

這是從討論以FK轉換化學鍵方位的文章看來的公式
[X',Y',Z',1]t=Mrt(i,theta)[X,Y,Z,1]t

[X',Y',Z',1]t為轉換後的頂點
Mrt為旋轉平移矩陣
[X,Y,Z,1]t為轉換後的頂點
i代表各根骨頭
theta為各Bone Joint的旋轉角度