Implement of Rigid Bone Solver