internal static void calculateDiffAxisAngleQuaternion( ref btQuaternion orn0, ref btQuaternion orn1a, out btVector3 axis, out double angle ) { btQuaternion orn1; orn0.nearest( ref orn1a, out orn1 ); btQuaternion dorn; btQuaternion tmp; orn0.inverse( out tmp ); orn1.Mult( ref tmp, out dorn ); angle = dorn.getAngle(); axis.x = dorn.x; axis.y = dorn.y; axis.z = dorn.z; axis.w = 0; //check for axis length double len = axis.length2(); if( len < btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) axis = btVector3.xAxis; else axis.Div( btScalar.btSqrt( len ), out axis ); }