// explicit btMatrix3x3(stringbtScalar m) { setFromOpenGLSubMatrix(m); } /*@brief Constructor from Quaternion */ public btMatrix3x3( ref btQuaternion q ) { //setRotation( q ); float d = q.length2(); //btFullAssert(d != (float)(0.0)); float s = btScalar.BT_TWO / d; float xs = q.x * s, ys = q.y * s, zs = q.z * s; float wx = q.w * xs, wy = q.w * ys, wz = q.w * zs; float xx = q.x * xs, xy = q.x * ys, xz = q.x * zs; float yy = q.y * ys, yz = q.y * zs, zz = q.z * zs; m_el0.x = btScalar.BT_ONE - ( yy + zz ); m_el0.y = xy - wz; m_el0.z = xz + wy; m_el0.w = 0; m_el1.x = xy + wz; m_el1.y = btScalar.BT_ONE - ( xx + zz ); m_el1.z = yz - wx; m_el1.w = 0; m_el2.x = xz - wy; m_el2.y = yz + wx; m_el2.z = btScalar.BT_ONE - ( xx + yy ); m_el2.w = 0; m_el3.x = 0; m_el3.y = 0; m_el3.z = 0; m_el3.w = 1; }
/*@brief Get the matrix represented as a quaternion @param q The quaternion which will be set */ public void getRotation( out btQuaternion result ) { float trace = m_el0.x + m_el1.y + m_el2.z; float[] temp = new float[4]; if( trace > 0 ) { float s = btScalar.btSqrt( trace + btScalar.BT_ONE ); temp[3] = ( s * btScalar.BT_HALF ); s = btScalar.BT_TWO / s; temp[0] = ( ( m_el2.y - m_el1.z ) * s ); temp[1] = ( ( m_el0.z - m_el2.x ) * s ); temp[2] = ( ( m_el1.x - m_el0.y ) * s ); } else { int i = m_el0.x < m_el1.y ? ( m_el1.y < m_el2.z ? 2 : 1 ) : ( m_el0.x < m_el2.z ? 2 : 0 ); int j = ( i + 1 ) % 3; int k = ( i + 2 ) % 3; float s = btScalar.btSqrt( this[i, i] - this[j, j] - this[k, k] + btScalar.BT_ONE ); temp[i] = s * btScalar.BT_HALF; s = btScalar.BT_HALF / s; temp[3] = ( this[k, j] - this[j, k] ) * s; temp[j] = ( this[j, i] + this[i, j] ) * s; temp[k] = ( this[k, i] + this[i, k] ) * s; } result.x = temp[0]; result.y = temp[1]; result.z = temp[2]; result.w = temp[3]; }
/*@brief Constructor from btQuaternion (optional btVector3 ) @param q Rotation from quaternion @param c Translation from Vector (default 0,0,0) */ public void setValue( ref btQuaternion q, ref btVector3 c ) { m_basis = new btMatrix3x3( ref q ); m_origin = c; }
public static void setRotation( out btMatrix3x3 result, ref btQuaternion q ) { float d = q.length2(); #if PARANOID_ASSERTS Debug.Assert( d != 0 ); #endif float s = btScalar.BT_TWO / d; float xs = q.x * s, ys = q.y * s, zs = q.z * s; float wx = q.w * xs, wy = q.w * ys, wz = q.w * zs; float xx = q.x * xs, xy = q.x * ys, xz = q.x * zs; float 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 ) ); }
/*@brief Constructor from btQuaternion (optional btVector3 ) @param q Rotation from quaternion @param c Translation from Vector (default 0,0,0) */ public btTransform( ref btQuaternion q, ref btVector3 c ) { m_basis = new btMatrix3x3( ref q ); m_origin = c; }
/*@brief Constructor from btQuaternion (optional btVector3 ) @param q Rotation from quaternion @param c Translation from Vector (default 0,0,0) */ public btTransform( ref btQuaternion q ) { m_basis = new btMatrix3x3( ref q ); m_origin = btVector3.Zero; }
/*@brief Set the rotational element by btQuaternion */ public void setRotation( ref btQuaternion q ) { m_basis.setRotation( ref q ); }
/*@brief Return a quaternion representing the rotation */ public void getRotation( out btQuaternion result ) { m_basis.getRotation( out result ); }
/*@brief Return the transform of the btQuaternion */ public static void Apply( ref btTransform t, ref btQuaternion q, out btQuaternion result ) { btQuaternion tmp; t.getRotation( out tmp ); tmp.Mult( ref q, out result ); }