I have built a nice little library of vector, matrix and quaternion functions. Some are even handassembled using sse. I can upload them later if you are interested