Optimize a little MotionConstrCommon computeMatrix method.