Add dist3dsqrd to matrix
do depth sorting