Implement calculation of mass matrix, mass inverse matrix, Coriolis force vector, and external force vector. Still need bug fixes.