Excellent, it helped me a lot.
You can speed this function up considerably by not using the Matlab functions cross and dot, these are unnecessarily slow. Make your own cross product function, and use "sum(a.*b)" instead of "dot(a,b), and it'll will be at least a factor 10 faster.
