corrected the matrix math

next commit will start the cleanup process.