Refactor Matrix, make Vertex classes POD for memcpy

extracted common operations in matrix transformation functions to
private applyTransformation function.