Fixed Matrix/Quaternion code

Fixed matrix and quaternion code. Added some code to RigidBody class
(still working on it).