Created Position3D class
Modified the code to use it, eliminated old MatrixImpl and Matrix classes