Add a bench to ForwardDynamics::compute_C.
Optimize ForwardDynamics::computeC.