Change modelmatrix to use a vector p