Учась в универе приходилось писать самому классы матриц векторов и функций с ними связанных. Жаль я тогда не знал о bullet-е www.bulletphysics.org/wordpress. Такие мелочи там используются на раз, два, три. А вот с кватернионами придется повозится ибо в универе нам про них не рассказывали.