Fixed matrix calculation errors in defining ground plane. Fix coordinate conversions between OptiTrack and OSG. General bug fixes and refactoring.