Fixed translation matrix, added sphere plot and garbage plot.

Support to generate orthonormal bases from a vector.
Added some testing code