public double p1_norm( btMatrix3x3 a )
{
double sum0 = abs_column_sum( ref a, 0 );
double sum1 = abs_column_sum( ref a, 1 );
double sum2 = abs_column_sum( ref a, 2 );
return btScalar.btMax( btScalar.btMax( sum0, sum1 ), sum2 );
}
public btTransform(btMatrix3x3 b) : this(BulletPINVOKE.new_btTransform__SWIG_4(btMatrix3x3.getCPtr(b)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
double pinf_norm( btMatrix3x3 a )
{
double sum0 = abs_row_sum( a, 0 );
double sum1 = abs_row_sum( a, 1 );
double sum2 = abs_row_sum( a, 2 );
return btScalar.btMax( btScalar.btMax( sum0, sum1 ), sum2 );
}
public btJacobianEntry(btMatrix3x3 world2A, btVector3 rel_pos1, btVector3 rel_pos2, btVector3 jointAxis, btVector3 inertiaInvA, float massInvA) : this(BulletPINVOKE.new_btJacobianEntry__SWIG_4(btMatrix3x3.getCPtr(world2A), btVector3.getCPtr(rel_pos1), btVector3.getCPtr(rel_pos2), btVector3.getCPtr(jointAxis), btVector3.getCPtr(inertiaInvA), massInvA), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btJacobianEntry(btVector3 jointAxis, btMatrix3x3 world2A, btMatrix3x3 world2B, btVector3 inertiaInvA, btVector3 inertiaInvB) : this(BulletPINVOKE.new_btJacobianEntry__SWIG_2(btVector3.getCPtr(jointAxis), btMatrix3x3.getCPtr(world2A), btMatrix3x3.getCPtr(world2B), btVector3.getCPtr(inertiaInvA), btVector3.getCPtr(inertiaInvB)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btMatrix3x3(btMatrix3x3 other) : this(BulletPINVOKE.new_btMatrix3x3__SWIG_3(btMatrix3x3.getCPtr(other)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public void calcAngleInfo2(btTransform transA, btTransform transB, btMatrix3x3 invInertiaWorldA, btMatrix3x3 invInertiaWorldB)
{
BulletPINVOKE.btConeTwistConstraint_calcAngleInfo2(swigCPtr, btTransform.getCPtr(transA), btTransform.getCPtr(transB), btMatrix3x3.getCPtr(invInertiaWorldA), btMatrix3x3.getCPtr(invInertiaWorldB));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public void diagonalize(btMatrix3x3 rot, float threshold, int maxSteps)
{
BulletPINVOKE.btMatrix3x3_diagonalize(swigCPtr, btMatrix3x3.getCPtr(rot), threshold, maxSteps);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public void setBasis(btMatrix3x3 basis)
{
BulletPINVOKE.btTransform_setBasis(swigCPtr, btMatrix3x3.getCPtr(basis));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btMatrix3x3 scaled(btVector3 s)
{
btMatrix3x3 ret = new btMatrix3x3(BulletPINVOKE.btMatrix3x3_scaled(swigCPtr, btVector3.getCPtr(s)), true);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public btMatrix3x3 timesTranspose(btMatrix3x3 m)
{
btMatrix3x3 ret = new btMatrix3x3(BulletPINVOKE.btMatrix3x3_timesTranspose(swigCPtr, btMatrix3x3.getCPtr(m)), true);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
// // solving 2x2 lcp problem (inequality, direct solution ) // void resolveBilateralPairConstraint( btRigidBody body0, btRigidBody body1, btMatrix3x3& world2A, btMatrix3x3& world2B, ref btVector3 invInertiaADiag, double invMassA, ref btVector3 linvelA, ref btVector3 angvelA, ref btVector3 rel_posA1, ref btVector3 invInertiaBDiag, double invMassB, ref btVector3 linvelB, ref btVector3 angvelB, ref btVector3 rel_posA2, double depthA, ref btVector3 normalA, ref btVector3 rel_posB1, ref btVector3 rel_posB2, double depthB, ref btVector3 normalB, double imp0, double imp1 );
public btMatrix3x3 transpose()
{
btMatrix3x3 ret = new btMatrix3x3(BulletPINVOKE.btMatrix3x3_transpose(swigCPtr), true);
return(ret);
}
public static btMatrix3x3 getIdentity()
{
btMatrix3x3 ret = new btMatrix3x3(BulletPINVOKE.btMatrix3x3_getIdentity(), false);
return(ret);
}
public btMatrix3x3 adjoint()
{
btMatrix3x3 ret = new btMatrix3x3(BulletPINVOKE.btMatrix3x3_adjoint(swigCPtr), true);
return(ret);
}
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 ) );
}
// method 2 - found in 6dof constraint
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
static bool matrixToEulerXYZ( btMatrix3x3& mat, ref btVector3 xyz )
{
// // rot = cy*cz -cy*sz sy
// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
//
double fi = btGetMatrixElem( mat, 2 );
if( fi < (double)( 1.0f ) )
{
if( fi > (double)( -1.0f ) )
{
xyz[0] = btAtan2( -btGetMatrixElem( mat, 5 ), btGetMatrixElem( mat, 8 ) );
xyz[1] = btAsin( btGetMatrixElem( mat, 2 ) );
xyz[2] = btAtan2( -btGetMatrixElem( mat, 1 ), btGetMatrixElem( mat, 0 ) );
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
xyz[0] = -btAtan2( btGetMatrixElem( mat, 3 ), btGetMatrixElem( mat, 4 ) );
xyz[1] = -SIMD_HALF_PI;
xyz[2] = (double)( 0.0 );
return false;
}
}
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
xyz[0] = btAtan2( btGetMatrixElem( mat, 3 ), btGetMatrixElem( mat, 4 ) );
xyz[1] = SIMD_HALF_PI;
xyz[2] = 0.0;
}
return false;
}
public btMatrix3x3 getInvInertiaTensorWorld()
{
btMatrix3x3 ret = new btMatrix3x3(BulletPINVOKE.btRigidBody_getInvInertiaTensorWorld(swigCPtr), false);
return(ret);
}
public static void setValue( out btMatrix3x3 m, ref btVector3 col1, ref btVector3 col2, ref btVector3 col3 )
{
m.m_el0.x = col1.x; m.m_el0.y = col2.x; m.m_el0.z = col3.x; m.m_el0.w = 0;
m.m_el1.x = col1.y; m.m_el1.y = col2.y; m.m_el1.z = col3.y; m.m_el1.w = 0;
m.m_el2.x = col1.z; m.m_el2.y = col2.z; m.m_el2.z = col3.z; m.m_el2.w = 0;
}
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 );
#if !DISABLE_ROW4
m_el3.Sub( ref m2.m_el3, out result.m_el3 );
#endif
}
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, 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 );
#if !DISABLE_ROW4
result.w = m.m_el3.dot( ref v );
#else
result.w = 0;
#endif
}
public static void Add( ref btMatrix3x3 m1, ref btMatrix3x3 m2, out btMatrix3x3 result )
{
m1.m_el0.Add( ref m2.m_el0, out result.m_el0 );
m1.m_el1.Add( ref m2.m_el1, out result.m_el1 );
m1.m_el2.Add( ref m2.m_el2, out result.m_el2 );
#if !DISABLE_ROW4
m1.m_el3.Add( ref m2.m_el3, out result.m_el3 );
#endif
}
public void Mult( double 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 );
#if !DISABLE_ROW4
m_el3.Mult( k, out result.m_el3 );
#endif
}
public static void Mult( ref btMatrix3x3 m, double 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 );
#if !DISABLE_ROW4
m.m_el3.Mult( k, out result.m_el3 );
#endif
}
/*@brief diagonalizes this matrix by the Jacobi method.
@param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
coordinate system, i.e., old_this = rot * new_this * rot^T.
@param threshold See iteration
@param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
Note that this matrix is assumed to be symmetric.
*/
public void diagonalize( out btMatrix3x3 rot, double threshold, int maxSteps )
{
rot = Identity;
for( int step = maxSteps; step > 0; step-- )
{
// find off-diagonal element [p,q] with largest magnitude
int p = 0;
int q = 1;
int r = 2;
double max = btScalar.btFabs( m_el0.y );
double v = btScalar.btFabs( m_el0.z );
if( v > max )
{
q = 2;
r = 1;
max = v;
}
v = btScalar.btFabs( m_el1.z );
if( v > max )
{
p = 1;
q = 2;
r = 0;
max = v;
}
double t = threshold * ( btScalar.btFabs( m_el0.x ) + btScalar.btFabs( m_el1.y ) + btScalar.btFabs( m_el2.z ) );
if( max <= t )
{
if( max <= btScalar.SIMD_EPSILON * t )
{
return;
}
step = 1;
}
// compute Jacobi rotation J which leads to a zero for element [p,q]
double mpq = this[p, q];
double theta = ( this[q, q] - this[p, p] ) / ( 2 * mpq );
double theta2 = theta * theta;
double cos;
double sin;
if( theta2 * theta2 < 10 / btScalar.SIMD_EPSILON )
{
t = ( theta >= 0 ) ? 1 / ( theta + btScalar.btSqrt( 1 + theta2 ) )
: 1 / ( theta - btScalar.btSqrt( 1 + theta2 ) );
cos = 1 / btScalar.btSqrt( 1 + t * t );
sin = cos * t;
}
else
{
// approximation for large theta-value, i.e., a nearly diagonal matrix
t = 1 / ( theta * ( 2 + 0.5 / theta2 ) );
cos = 1 - 0.5 * t * t;
sin = cos * t;
}
// apply rotation to matrix (this = J^T * this * J)
this[p, q] = this[q, p] = 0;
this[p, p] -= t * mpq;
this[q, q] += t * mpq;
double mrp = this[r, p];
double mrq = this[r, q];
this[r, p] = this[p, r] = cos * mrp - sin * mrq;
this[r, q] = this[q, r] = cos * mrq + sin * mrp;
// apply rotation to rot (rot = rot * J)
for( int i = 0; i < 3; i++ )
{
btVector3 row = rot[i];
mrp = row[p];
mrq = row[q];
row[p] = cos * mrp - sin * mrq;
row[q] = cos * mrq + sin * mrp;
}
}
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(btMatrix3x3 obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
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_el1.x = m_el0.y;
result.m_el1.y = m_el1.y;
result.m_el1.z = m_el2.y;
result.m_el2.x = m_el0.z;
result.m_el2.y = m_el1.z;
result.m_el2.z = m_el2.z;
#if !DISABLE_ROW4
result.m_el0.w = m_el3.x;
result.m_el1.w = m_el3.y;
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;
#else
result.m_el0.w = 0;
result.m_el1.w = 0;
result.m_el2.w = 0;
#endif
}
public void getInfo2NonVirtual(btTypedConstraint.btConstraintInfo2 info, btTransform transA, btTransform transB, btMatrix3x3 invInertiaWorldA, btMatrix3x3 invInertiaWorldB)
{
BulletPINVOKE.btConeTwistConstraint_getInfo2NonVirtual(swigCPtr, btTypedConstraint.btConstraintInfo2.getCPtr(info), btTransform.getCPtr(transA), btTransform.getCPtr(transB), btMatrix3x3.getCPtr(invInertiaWorldA), btMatrix3x3.getCPtr(invInertiaWorldB));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
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 static double getValue( ref btMatrix3x3 m, int i, int j)
{
{
switch( i )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( i > 3 || i < 0 );
#endif
case 0:
switch( j )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( j > 3 || j < 0 );
#endif
case 0: return m.m_el0.x;
case 1: return m.m_el0.y;
case 2: return m.m_el0.z;
case 3: return m.m_el0.w;
}
case 1:
switch( j )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( j > 3 || j < 0 );
#endif
case 0: return m.m_el1.x;
case 1: return m.m_el1.y;
case 2: return m.m_el1.z;
case 3: return m.m_el1.w;
}
case 2:
switch( j )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( j > 3 || j < 0 );
#endif
case 0: return m.m_el2.x;
case 1: return m.m_el2.y;
case 2: return m.m_el2.z;
case 3: return m.m_el2.w;
}
#if !DISABLE_ROW4
case 3:
switch( j )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( j > 3 || j < 0 );
#endif
case 0: return m_el3.x;
case 1: return m_el3.y;
case 2: return m_el3.z;
case 3: return m_el3.w;
}
#endif
}
}
}
/*
public btMatrix3x3 inverse()
{
btVector3 co = new btVector3( cofac( 1, 1, 2, 2 ), cofac( 1, 2, 2, 0 ), cofac( 1, 0, 2, 1 ) );
double det = m_el0.dot( ref co );
#if PARANOID_ASSERTS
Debug.Assert( det != 0 );
#endif
double 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 ) );
double det = m_el0.dot( ref co );
#if PARANOID_ASSERTS
Debug.Assert( det != 0 );
#endif
double s = 1.0 / 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;
#if !DISABLE_ROW4
result.m_el3.x = 0;
result.m_el3.y = 0;
result.m_el3.z = 0;
result.m_el3.w = 0;
#endif
}
public static void setValue( out btMatrix3x3 m, double xx, double xy, double xz,
double yx, double yy, double yz,
double zx, double zy, double zz )
{
m.m_el0.x = xx; m.m_el0.y = xy; m.m_el0.z = xz;
m.m_el1.x = yx; m.m_el1.y = yy; m.m_el1.z = yz;
m.m_el2.x = zx; m.m_el2.y = zy; m.m_el2.z = zz;
m.m_el0.w = 0; m.m_el1.w = 0; m.m_el2.w = 0;
#if !DISABLE_ROW4
m.m_el3.x = 0;
m.m_el3.y = 0;
m.m_el3.z = 0;
m.m_el3.w = 1;
#endif
}
/*
* Decomposes a matrix into orthogonal and symmetric, positive-definite
* parts. If the number of iterations returned by this function is equal to
* the maximum number of iterations, the algorithm has failed to converge.
*
* @param a - the original matrix
* @param u - the resulting orthogonal matrix
* @param h - the resulting symmetric matrix
*
* @return the number of iterations performed by the algorithm.
*/
public uint decompose( ref btMatrix3x3 a, out btMatrix3x3 u, out btMatrix3x3 h )
{
// Use the 'u' and 'h' matrices for intermediate calculations
u = a;
a.inverse( out h );
btMatrix3x3 tmp;
btMatrix3x3 tmp2;
btMatrix3x3 tmp3;
for( uint i = 0; i < m_maxIterations; ++i )
{
double h_1 = p1_norm( h );
double h_inf = pinf_norm( h );
double u_1 = p1_norm( u );
double u_inf = pinf_norm( u );
double h_norm = h_1 * h_inf;
double u_norm = u_1 * u_inf;
// The matrix is effectively singular so we cannot invert it
if( btScalar.btFuzzyZero( h_norm ) || btScalar.btFuzzyZero( u_norm ) )
break;
double gamma = btScalar.btPow( h_norm / u_norm, 0.25f );
double inv_gamma = (double)( 1.0 ) / gamma;
// Determine the delta to 'u'
u.Mult( gamma - (double)( 2.0 ), out tmp );
h.transpose( out tmp2 );
tmp2.Mult( inv_gamma, out tmp3 );
tmp.Add( ref tmp3, out tmp2 );
btMatrix3x3 delta;
tmp2.Mult( 0.5, out delta );
//const btMatrix3x3 delta = ( u * ( gamma - btScalar( 2.0 ) ) + h.transpose() * inv_gamma ) * btScalar( 0.5 );
// Update the matrices
u.Add( ref delta, out u );
//u += delta;
u.inverse( out h );
// Check for convergence
if( p1_norm( delta ) <= m_tolerance * u_1 )
{
u.transpose( out tmp );
tmp.Mult( ref a, out h );
//h = u.transpose() * a;
h.transpose( out tmp );
h.Add( ref tmp, out tmp2 );
tmp2.Mult( 0.5, out h );
//h = ( h + h.transpose() ) * 0.5;
return i;
}
}
// The algorithm has failed to converge to the specified tolerance, but we
// want to make sure that the matrices returned are in the right form.
u.transpose( out tmp );
tmp.Mult( ref a, out h );
//h = u.transpose() * a;
h.transpose( out tmp );
h.Add( ref tmp, out tmp2 );
tmp2.Mult( 0.5, out h );
//h = ( h + h.transpose() ) * 0.5;
return m_maxIterations;
}
static double btGetMatrixElem( ref btMatrix3x3 mat, int index )
{
int i = index % 3;
int j = index / 3;
return mat[i][j];
}
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;
#if !DISABLE_ROW4
result.m_el3.x = 0;
result.m_el3.y = 0;
result.m_el3.z = 0;
result.m_el3.w = 1;
#endif
}
/*@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 );
}
public static void Apply( 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 = 0;
}
public double abs_row_sum( btMatrix3x3 a, int i )
{
return btScalar.btFabs( btMatrix3x3.getValue( ref a,i, 0 ) )
+ btScalar.btFabs( btMatrix3x3.getValue( ref a,i, 1 ) )
+ btScalar.btFabs( btMatrix3x3.getValue( ref a,i, 2) );
}
public void Apply( btIMatrix3x3 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;
#if !DISABLE_ROW4
result.m_el3.x = 0;
result.m_el3.y = 0;
result.m_el3.z = 0;
result.m_el3.w = 1;
#endif
}
public btMatrix3x3 absolute()
{
btMatrix3x3 ret = new btMatrix3x3(BulletPINVOKE.btMatrix3x3_absolute(swigCPtr), true);
return(ret);
}
/*
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 btMatrix3x3 inverse()
{
btMatrix3x3 ret = new btMatrix3x3(BulletPINVOKE.btMatrix3x3_inverse(swigCPtr), true);
return(ret);
}
// 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;
#if !DISABLE_ROW4
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;
#endif
}
public uint polarDecompose( ref btMatrix3x3 a, out btMatrix3x3 u, out btMatrix3x3 h )
{
return decompose( ref a, out u, out h );
}
public static double setValue( ref btMatrix3x3 m, int i, int j, double value )
{
{
switch( i )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( i > 3 || i < 0 );
#endif
case 0:
switch( j )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( j > 3 || j < 0 );
#endif
case 0: return m.m_el0.x = value;
case 1: return m.m_el0.y = value;
case 2: return m.m_el0.z = value;
case 3: return m.m_el0.w = value;
}
break;
case 1:
switch( j )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( j > 3 || j < 0 );
#endif
case 0: return m.m_el1.x = value;
case 1: return m.m_el1.y = value;
case 2: return m.m_el1.z = value;
case 3: return m.m_el1.w = value;
}
case 2:
switch( j )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( j > 3 || j < 0 );
#endif
case 0: return m.m_el2.x = value;
case 1: return m.m_el2.y = value;
case 2: return m.m_el2.z = value;
case 3: return m.m_el2.w = value;
}
#if !DISABLE_ROW4
case 3:
switch( j )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( j > 3 || j < 0 );
#endif
case 0: m_el3.x = value; break;
case 1: m_el3.y = value; break;
case 2: m_el3.z = value; break;
case 3: m_el3.w = value; break;
}
break;
#endif
}
}
}
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;
#if !DISABLE_ROW4
result.m_el3.x = 0;
result.m_el3.y = 0;
result.m_el3.z = 0;
result.m_el3.w = 1;
#endif
}
public btMatrix3x3 getBasis()
{
btMatrix3x3 ret = new btMatrix3x3(BulletPINVOKE.btTransform_getBasis__SWIG_0(swigCPtr), false);
return(ret);
}