Moved matrix calculation of rigid bodies into seperate function.