/*@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion * @param q The other quaternion to interpolate with * @param t The ratio between this and q to interpolate. * If t = 0 the result is this, if t=1 the result is q. * Slerp interpolates assuming constant velocity. */ void slerp( ref btQuaternion q, double t, out btQuaternion result ) { double magnitude = btScalar.btSqrt( length2() * q.length2() ); //Debug.Assert( magnitude > (double)( 0 ) ); double product = dot( ref q ) / magnitude; if( btScalar.btFabs( product ) < 1 ) { // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp double sign = ( product < 0 ) ? -1 : 1; double theta = btScalar.btAcos( sign * product ); double s1 = btScalar.btSin( sign * t * theta ); double d = 1.0 / btScalar.btSin( theta ); double s0 = btScalar.btSin( ( 1.0 - t ) * theta ); result.x = ( x * s0 + q.x * s1 ) * d; result.y = ( y * s0 + q.y * s1 ) * d; result.z = ( z * s0 + q.z * s1 ) * d; result.w = ( w * s0 + q.w * s1 ) * d; } else { result = this; } }
/*@brief Return the angle between this quaternion and the other along the shortest path @param q The other quaternion */ public double angleShortestPath( ref btQuaternion q ) { double s = btScalar.btSqrt( length2() * q.length2() ); //Debug.Assert( s != 0.0 ); if( dot( ref q ) < 0 ) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp { btQuaternion b; q.inverse( out b ); return btScalar.btAcos( dot( ref b ) / s ) * 2.0; } else return btScalar.btAcos( dot( ref q ) / s ) * 2.0; }
/*@brief Return the []half[] angle between this quaternion and the other @param q The other quaternion */ double angle( ref btQuaternion q ) { double s = btScalar.btSqrt( length2() * q.length2() ); //Debug.Assert( s != 0.0 ); return btScalar.btAcos( dot( ref q ) / s ); }
public static void setRotation( out btMatrix3x3 result, ref btQuaternion q ) { double d = q.length2(); #if PARANOID_ASSERTS Debug.Assert( d != 0 ); #endif double s = btScalar.BT_TWO / d; double xs = q.x * s, ys = q.y * s, zs = q.z * s; double wx = q.w * xs, wy = q.w * ys, wz = q.w * zs; double xx = q.x * xs, xy = q.x * ys, xz = q.x * zs; double yy = q.y * ys, yz = q.y * zs, zz = q.z * zs; btMatrix3x3.setValue( out result, btScalar.BT_ONE - ( yy + zz ), xy - wz, xz + wy, xy + wz, btScalar.BT_ONE - ( xx + zz ), yz - wx, xz - wy, yz + wx, btScalar.BT_ONE - ( xx + yy ) ); }
//public btVector3 m_el3; // explicit btMatrix3x3(stringbtScalar m) { setFromOpenGLSubMatrix(m); } /*@brief Constructor from Quaternion */ public btMatrix3x3( ref btQuaternion q ) { //setRotation( q ); double d = q.length2(); //btFullAssert(d != (double)(0.0)); double s = 2.0 / d; double xs = q.x * s, ys = q.y * s, zs = q.z * s; double wx = q.w * xs, wy = q.w * ys, wz = q.w * zs; double xx = q.x * xs, xy = q.x * ys, xz = q.x * zs; double yy = q.y * ys, yz = q.y * zs, zz = q.z * zs; m_el0.x = 1.0 - ( yy + zz ); m_el0.y = xy - wz; m_el0.z = xz + wy; m_el0.w = 0; m_el1.x = xy + wz; m_el1.y = 1.0 - ( xx + zz ); m_el1.z = yz - wx; m_el1.w = 0; m_el2.x = xz - wy; m_el2.y = yz + wx; m_el2.z = 1.0 - ( xx + yy ); m_el2.w = 0; #if !DISABLE_ROW4 m_el3.x = 0; m_el3.y = 0; m_el3.z = 0; m_el3.w = 1; #endif }
/*@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion * @param q The other quaternion to interpolate with * @param t The ratio between this and q to interpolate. * If t = 0 the result is this, if t=1 the result is q. * Slerp interpolates assuming constant velocity. */ void slerp( ref btQuaternion q, float t, out btQuaternion result ) { float magnitude = btScalar.btSqrt( length2() * q.length2() ); //Debug.Assert( magnitude > (float)( 0 ) ); float product = dot( ref q ) / magnitude; if( btScalar.btFabs( product ) < 1 ) { // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp float sign = ( product < 0 ) ? -1 : 1; float theta = btScalar.btAcos( sign * product ); float s1 = btScalar.btSin( sign * t * theta ); float d = btScalar.BT_ONE / btScalar.btSin( theta ); float s0 = btScalar.btSin( ( btScalar.BT_ONE - t ) * theta ); result.x = ( x * s0 + q.x * s1 ) * d; result.y = ( y * s0 + q.y * s1 ) * d; result.z = ( z * s0 + q.z * s1 ) * d; result.w = ( w * s0 + q.w * s1 ) * d; } else { result = this; } }