Fix calcDerivative() to work for any ordering of 2nd and 3rd components, organize it to allow computing only the needed derivatives, and simplify it.
Fix calcDerivative() to work for any ordering of 2nd and 3rd components, organize it to allow computing only the needed derivatives, and simplify it.

elkingtonmcb/simbody,chrisdembia/simbody,simbody/simbody,chrisdembia/simbody,scamille/simbody,Gjacquenot/simbody,elkingtonmcb/simbody,scamille/simbody,elkingtonmcb/simbody,Gjacquenot/simbody,robojukie/simbody,chrisdembia/simbody,Gjacquenot/simbody,robojukie/simbody,scamille/simbody,elen4/simbody,elen4/simbody,robojukie/simbody,simbody/simbody