Numerical Algorithms for General Body Dynamics