Fix issues with JOS3, migrate most functions to use math.Matrix instead of plain arrays

Fix issues with JOS3, migrate most functions to use math.Matrix instead of plain arrays #417