Mark's vector and matrix code he posted is pretty good. After working with it a while, I realized complicated transformations can be solved in just a couple lines of code.
The problem I had was that Mark's quaternion extraction code was wrong. I replaced it with my own I figured out from a math book.
The problem I had was that Mark's quaternion extraction code was wrong. I replaced it with my own I figured out from a math book.