Added some dimensionality check. Make work for matrices with 3 dimensions