public void GetGLMatrix( out btMatrix3x3 m )
{
#if !COLUMN_MAJOR_EXPECTED
m = m_basis;
m.m_el3 = m_origin;
m.m_el3.w = 1;
#else
m.m_el0.x = m_basis.m_el0.x;
m.m_el0.y = m_basis.m_el1.x;
m.m_el0.z = m_basis.m_el2.x;
m.m_el0.w = m_basis.m_el0.w;
m.m_el1.x = m_basis.m_el0.y;
m.m_el1.y = m_basis.m_el1.y;
m.m_el1.z = m_basis.m_el2.y;
m.m_el1.w = m_basis.m_el1.w;
m.m_el2.x = m_basis.m_el0.z;
m.m_el2.y = m_basis.m_el1.z;
m.m_el2.z = m_basis.m_el2.z;
m.m_el2.w = m_basis.m_el2.w;
m.m_el3 = m_origin;
m.m_el3.w = m_basis[3][3];
#endif
}
public void adjoint( out btMatrix3x3 result )
{
btMatrix3x3.setValue( out result, cofac( 1, 1, 2, 2 ), cofac( 0, 2, 2, 1 ), cofac( 0, 1, 1, 2 ),
cofac( 1, 2, 2, 0 ), cofac( 0, 0, 2, 2 ), cofac( 0, 2, 1, 0 ),
cofac( 1, 0, 2, 1 ), cofac( 0, 1, 2, 0 ), cofac( 0, 0, 1, 1 ) );
}
public void transposeTimes( ref btMatrix3x3 m, out btMatrix3x3 result )
{
result.m_el0.x = m_el0.x * m.m_el0.x + m_el1.x * m.m_el1.x + m_el2.x * m.m_el2.x;
result.m_el0.y = m_el0.x * m.m_el0.y + m_el1.x * m.m_el1.y + m_el2.x * m.m_el2.y;
result.m_el0.z = m_el0.x * m.m_el0.z + m_el1.x * m.m_el1.z + m_el2.x * m.m_el2.z;
result.m_el0.w = 0;
result.m_el1.x = m_el0.y * m.m_el0.x + m_el1.y * m.m_el1.x + m_el2.y * m.m_el2.x;
result.m_el1.y = m_el0.y * m.m_el0.y + m_el1.y * m.m_el1.y + m_el2.y * m.m_el2.y;
result.m_el1.z = m_el0.y * m.m_el0.z + m_el1.y * m.m_el1.z + m_el2.y * m.m_el2.z;
result.m_el1.w = 0;
result.m_el2.x = m_el0.z * m.m_el0.x + m_el1.z * m.m_el1.x + m_el2.z * m.m_el2.x;
result.m_el2.y = m_el0.z * m.m_el0.y + m_el1.z * m.m_el1.y + m_el2.z * m.m_el2.y;
result.m_el2.z = m_el0.z * m.m_el0.z + m_el1.z * m.m_el1.z + m_el2.z * m.m_el2.z;
result.m_el2.w = 0;
result.m_el3.x = 0;
result.m_el3.y = 0;
result.m_el3.z = 0;
result.m_el3.w = 1;
}
public void Add( ref btMatrix3x3 m2, out btMatrix3x3 result )
{
m_el0.Add( ref m2.m_el0, out result.m_el0 );
m_el1.Add( ref m2.m_el1, out result.m_el1 );
m_el2.Add( ref m2.m_el2, out result.m_el2 );
m_el3.Add( ref m2.m_el3, out result.m_el3 );
}
public void absolute( out btMatrix3x3 result )
{
btMatrix3x3.setValue( out result,
btScalar.btFabs( m_el0.x ), btScalar.btFabs( m_el0.y ), btScalar.btFabs( m_el0.z ),
btScalar.btFabs( m_el1.x ), btScalar.btFabs( m_el1.y ), btScalar.btFabs( m_el1.z ),
btScalar.btFabs( m_el2.x ), btScalar.btFabs( m_el2.y ), btScalar.btFabs( m_el2.z ) );
}
public static void Mult( ref btMatrix3x3 m, float k, out btMatrix3x3 result )
{
m.m_el0.Mult( k, out result.m_el0 );
m.m_el1.Mult( k, out result.m_el1 );
m.m_el2.Mult( k, out result.m_el2 );
m.m_el3.Mult( k, out result.m_el3 );
}
public static void Mult( ref btMatrix3x3 m, ref btVector3 v, out btVector3 result )
{
result.x = m.m_el0.dot( ref v );
result.y = m.m_el1.dot( ref v );
result.z = m.m_el2.dot( ref v );
result.w = m.m_el3.dot( ref v );
}
/*@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;
}
/*@brief Constructor from btMatrix3x3 (optional btVector3)
@param b Rotation from Matrix
@param c Translation from Vector default (0,0,0)*/
public btTransform( ref btMatrix3x3 b, ref btVector3 c )
{
m_basis = b;
m_origin = c;
}
/*@brief Constructor from btMatrix3x3 (optional btVector3)
@param b Rotation from Matrix
@param c Translation from Vector default (0,0,0)*/
public btTransform( ref btMatrix3x3 b )
{
m_basis = b;
m_origin = btVector3.Zero;
}
/*@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;
}
public btTransform( ref btVector3 q )
{
m_basis = btMatrix3x3.Identity;
m_origin = q;
}
/*@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;
}
public void GetGLCameraMatrix( out btMatrix3x3 m )
{
#if !COLUMN_MAJOR_EXPECTED
m.m_el0.x = m_basis.m_el0.x;
m.m_el0.y = m_basis.m_el1.x;
m.m_el0.z = -m_basis.m_el2.x;
m.m_el0.w = 0;
m.m_el1.x = m_basis.m_el0.y;
m.m_el1.y = m_basis.m_el1.y;
m.m_el1.z = -m_basis.m_el2.y;
m.m_el1.w = 0;// m_basis.m_el1.w;
m.m_el2.x = m_basis.m_el0.z;
m.m_el2.y = m_basis.m_el1.z;
m.m_el2.z = -m_basis.m_el2.z;
m.m_el2.w = 0;// m_basis.m_el2.w;
//m.m_el3 =
btVector3 negOrigin;
m_origin.Invert( out negOrigin );
m_basis.m_el2.Invert( out m_basis.m_el2 );
m_basis.ApplyInverseRotation( ref negOrigin, out m.m_el3 );
m_basis.m_el2.Invert( out m_basis.m_el2 );
m.m_el3.w = 1;
#else
m = m_basis;
btVector3 tmp;
btVector3 negOrigin;
m_origin.Invert( out negOrigin );
m.m_el0.z = -m.m_el0.z;
m.m_el1.z = -m.m_el1.z;
m.m_el2.z = -m.m_el2.z;
m_basis.m_el0.z = -m_basis.m_el0.z;
m_basis.m_el1.z = -m_basis.m_el1.z;
m_basis.m_el2.z = -m_basis.m_el2.z;
m_basis.ApplyInverseRotation( ref negOrigin, out tmp );
m_basis.m_el0.z = -m_basis.m_el0.z;
m_basis.m_el1.z = -m_basis.m_el1.z;
m_basis.m_el2.z = -m_basis.m_el2.z;
m.m_el3.x = tmp.x;
m.m_el3.y = tmp.y;
m.m_el3.z = tmp.z;
m.m_el3.w = m_basis[3][3];
#endif
}
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 btMatrix3x3 (optional btVector3)
@param b Rotation from Matrix
@param c Translation from Vector default (0,0,0)*/
public void setValue( ref btMatrix3x3 b, ref btVector3 c )
{
m_basis = b;
m_origin = c;
}
/*@brief Create a scaled copy of the matrix
@param s Scaling vector The elements of the vector will scale each column */
public void scaled( ref btVector3 s, out btMatrix3x3 result )
{
btMatrix3x3.setValue( out result,
m_el0.x * s.x, m_el0.y * s.y, m_el0.z * s.z,
m_el1.x * s.x, m_el1.y * s.y, m_el1.z * s.z,
m_el2.x * s.x, m_el2.y * s.y, m_el2.z * s.z );
}
/*@brief Copy constructor */
public btTransform( ref btTransform other )
{
m_basis = other.m_basis;
m_origin = other.m_origin;
}
public void Mult( float k, out btMatrix3x3 result )
{
m_el0.Mult( k, out result.m_el0 );
m_el1.Mult( k, out result.m_el1 );
m_el2.Mult( k, out result.m_el2 );
m_el3.Mult( k, out result.m_el3 );
}
public void dot3( ref btMatrix3x3 m, out btVector3 result )
{
result.x = dot( ref m.m_el0 );
result.y = dot( ref m.m_el1 );
result.z = dot( ref m.m_el2 );
result.w = 0;
}
public void Mult( ref btMatrix3x3 m2, out btMatrix3x3 result )
{
result.m_el0.x = m2.tdotx( ref m_el0 );
result.m_el0.y = m2.tdoty( ref m_el0 );
result.m_el0.z = m2.tdotz( ref m_el0 );
result.m_el0.w = 0;
result.m_el1.x = m2.tdotx( ref m_el1 );
result.m_el1.y = m2.tdoty( ref m_el1 );
result.m_el1.z = m2.tdotz( ref m_el1 );
result.m_el1.w = 0;
result.m_el2.x = m2.tdotx( ref m_el2 );
result.m_el2.y = m2.tdoty( ref m_el2 );
result.m_el2.z = m2.tdotz( ref m_el2 );
result.m_el2.w = 0;
result.m_el3.x = 0;
result.m_el3.y = 0;
result.m_el3.z = 0;
result.m_el3.w = 1;
}
/*
public btMatrix3x3 btMultTransposeLeft(btMatrix3x3 m1, btMatrix3x3 m2) {
return btMatrix3x3(
m1[0,0] * m2[0,0] + m1[1,0] * m2[1,0] + m1[2,0] * m2[2,0],
m1[0,0] * m2[0,1] + m1[1,0] * m2[1,1] + m1[2,0] * m2[2,1],
m1[0,0] * m2[0,2] + m1[1,0] * m2[1,2] + m1[2,0] * m2[2,2],
m1[0,1] * m2[0,0] + m1[1,1] * m2[1,0] + m1[2,1] * m2[2,0],
m1[0,1] * m2[0,1] + m1[1,1] * m2[1,1] + m1[2,1] * m2[2,1],
m1[0,1] * m2[0,2] + m1[1,1] * m2[1,2] + m1[2,1] * m2[2,2],
m1[0,2] * m2[0,0] + m1[1,2] * m2[1,0] + m1[2,2] * m2[2,0],
m1[0,2] * m2[0,1] + m1[1,2] * m2[1,1] + m1[2,2] * m2[2,1],
m1[0,2] * m2[0,2] + m1[1,2] * m2[1,2] + m1[2,2] * m2[2,2]);
}
*/
/*@brief Equality operator between two matrices
It will test all elements are equal. * */
public bool Equals( ref btMatrix3x3 m1 )
{
return ( m1.m_el0.x == m_el0.x && m1.m_el1.x == m_el1.x && m1.m_el2.x == m_el2.x &&
m1.m_el0.y == m_el0.y && m1.m_el1.y == m_el1.y && m1.m_el2.y == m_el2.y &&
m1.m_el0.z == m_el0.z && m1.m_el1.z == m_el1.z && m1.m_el2.z == m_el2.z );
}
public void Sub( ref btMatrix3x3 m2, out btMatrix3x3 result )
{
m_el0.Sub( ref m2.m_el0, out result.m_el0 );
m_el1.Sub( ref m2.m_el1, out result.m_el1 );
m_el2.Sub( ref m2.m_el2, out result.m_el2 );
m_el3.Sub( ref m2.m_el3, out result.m_el3 );
}
void GetGLMatrix( out btMatrix3x3 m )
{
m = this;
btVector3 tmp;
ApplyInverseRotation( ref m_el3, out tmp );
m.m_el3.x = tmp.x;
m.m_el3.y = tmp.y;
m.m_el3.z = tmp.z;
m.m_el3.w = this[3][3];
}
public void transpose( out btMatrix3x3 result )
{
result.m_el0.x = m_el0.x;
result.m_el0.y = m_el1.x;
result.m_el0.z = m_el2.x;
result.m_el0.w = m_el3.x;
result.m_el1.x = m_el0.y;
result.m_el1.y = m_el1.y;
result.m_el1.z = m_el2.y;
result.m_el1.w = m_el3.y;
result.m_el2.x = m_el0.z;
result.m_el2.y = m_el1.z;
result.m_el2.z = m_el2.z;
result.m_el2.w = m_el3.z;
result.m_el3.x = m_el0.w;
result.m_el3.y = m_el1.w;
result.m_el3.z = m_el2.w;
result.m_el3.w = m_el3.w;
}
// Copy constructor
public btMatrix3x3( ref btMatrix3x3 rhs )
{
m_el0.x = rhs.m_el0.x; m_el0.y = rhs.m_el0.y; m_el0.z = rhs.m_el0.z; m_el0.w = rhs.m_el0.w;
m_el1.x = rhs.m_el1.x; m_el1.y = rhs.m_el1.y; m_el1.z = rhs.m_el1.z; m_el1.w = rhs.m_el1.w;
m_el2.x = rhs.m_el2.x; m_el2.y = rhs.m_el2.y; m_el2.z = rhs.m_el2.z; m_el2.w = rhs.m_el2.w;
m_el3.x = rhs.m_el3.x; m_el3.y = rhs.m_el3.y; m_el3.z = rhs.m_el3.z; m_el3.w = rhs.m_el3.w;
}
/*
public btMatrix3x3 inverse()
{
btVector3 co = new btVector3( cofac( 1, 1, 2, 2 ), cofac( 1, 2, 2, 0 ), cofac( 1, 0, 2, 1 ) );
float det = m_el0.dot( ref co );
#if PARANOID_ASSERTS
Debug.Assert( det != 0 );
#endif
float s = 1.0 / det;
return new btMatrix3x3( co.x * s, cofac( 0, 2, 2, 1 ) * s, cofac( 0, 1, 1, 2 ) * s,
co.y * s, cofac( 0, 0, 2, 2 ) * s, cofac( 0, 2, 1, 0 ) * s,
co.z * s, cofac( 0, 1, 2, 0 ) * s, cofac( 0, 0, 1, 1 ) * s );
}
*/
public void inverse( out btMatrix3x3 result )
{
btVector3 co = new btVector3( cofac( 1, 1, 2, 2 ), cofac( 1, 2, 2, 0 ), cofac( 1, 0, 2, 1 ) );
float det = m_el0.dot( ref co );
#if PARANOID_ASSERTS
Debug.Assert( det != 0 );
#endif
float s = btScalar.BT_ONE / det;
result.m_el0.x = co.x * s;
result.m_el0.y = cofac( 0, 2, 2, 1 ) * s;
result.m_el0.z = cofac( 0, 1, 1, 2 ) * s;
result.m_el0.w = 0;
result.m_el1.x = co.y * s;
result.m_el1.y = cofac( 0, 0, 2, 2 ) * s;
result.m_el1.z = cofac( 0, 2, 1, 0 ) * s;
result.m_el1.w = 0;
result.m_el2.x = co.z * s;
result.m_el2.y = cofac( 0, 1, 2, 0 ) * s;
result.m_el2.z = cofac( 0, 0, 1, 1 ) * s;
result.m_el2.w = 0;
result.m_el3.x = 0;
result.m_el3.y = 0;
result.m_el3.z = 0;
result.m_el3.w = 0;
}
public static void setValue( out btMatrix3x3 m, float xx, float xy, float xz,
float yx, float yy, float yz,
float zx, float zy, float zz )
{
m.m_el0.x = xx; m.m_el1.x = xy; m.m_el2.x = xz; m.m_el3.x = 0;
m.m_el0.y = yx; m.m_el1.y = yy; m.m_el2.y = yz; m.m_el3.y = 0;
m.m_el0.z = zx; m.m_el1.z = zy; m.m_el2.z = zz; m.m_el3.z = 0;
m.m_el0.w = 0; m.m_el1.w = 0; m.m_el2.w = 0; m.m_el3.w = 1;
}
public void timesTranspose( ref btMatrix3x3 m, out btMatrix3x3 result )
{
result.m_el0.x = m_el0.dot( ref m.m_el0 );
result.m_el0.y = m_el0.dot( ref m.m_el1 );
result.m_el0.z = m_el0.dot( ref m.m_el2 );
result.m_el0.w = 0;
result.m_el1.x = m_el1.dot( ref m.m_el0 );
result.m_el1.y = m_el1.dot( ref m.m_el1 );
result.m_el1.z = m_el1.dot( ref m.m_el2 );
result.m_el1.w = 0;
result.m_el2.x = m_el2.dot( ref m.m_el0 );
result.m_el2.y = m_el2.dot( ref m.m_el1 );
result.m_el2.z = m_el2.dot( ref m.m_el2 );
result.m_el2.w = 0;
result.m_el3.x = 0;
result.m_el3.y = 0;
result.m_el3.z = 0;
result.m_el3.w = 1;
}
/*@brief Set the rotational element by btMatrix3x3 */
public void setBasis( ref btMatrix3x3 basis )
{
m_basis = basis;
}