First part of save bvh file implemented.
Second part of save bvh file
Added comments, litle code refactoring