CostModelForce can be updated to 3d case

In the construction of this class, we define ncost as 6 and its reference as a np.zeros(6). Here we could add an extra parameter for 3d case. Another solution would be to duplicate code and internally defines 3 as dimension.

@nmansard which solution do you like more?