// 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 );
}