C++ btMatrix3x3类代码示例

btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
{
	// use full newton-euler equations.  common practice to drop the wxIw term. want it for better tumbling behavior.
	// calculate using implicit euler step so it's stable.

	const btVector3 inertiaLocal = getLocalInertia();
	const btVector3 w0 = getAngularVelocity();

	btMatrix3x3 I;

	I = m_worldTransform.getBasis().scaled(inertiaLocal) *
		m_worldTransform.getBasis().transpose();

	// use newtons method to find implicit solution for new angular velocity (w')
	// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 
	// df/dw' = I + 1xIw'*step + w'xI*step

	btVector3 w1 = w0;

	// one step of newton's method
	{
		const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
		const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);

		btVector3 dw;
		dw = dfw.solve33(fw);
		//const btMatrix3x3 dfw_inv = dfw.inverse();
		//dw = dfw_inv*fw;

		w1 -= dw;
	}

	btVector3 gf = (w1 - w0);
	return gf;
}

static btMatrix3x3 Transpose(btMatrix3x3 &in)
{
	btVector3 row0 = in.getRow(0);
	btVector3 row1 = in.getRow(1);
	btVector3 row2 = in.getRow(2);
	btVector3 col0 = btAssign128(row0.x(), row1.x(), row2.x(), 0);
	btVector3 col1 = btAssign128(row0.y(), row1.y(), row2.y(), 0);
	btVector3 col2 = btAssign128(row0.z(), row1.z(), row2.z(), 0);
	return btMatrix3x3(col0, col1, col2);
}

Matrix3d toMatrix3d(const btMatrix3x3& basis)
{
	Matrix3d rotation;
	btVector3 col0 = basis.getColumn(0);
	btVector3 col1 = basis.getColumn(1);
	btVector3 col2 = basis.getColumn(2);
	rotation.col(0) = toVector3d(col0);
	rotation.col(1) = toVector3d(col1);
	rotation.col(2) = toVector3d(col2);
	return rotation;
}

void virtuose::Matrix3ToArray(btMatrix3x3 m, float *to) {
    to[0] = m.getRow(0).getX();
    to[1] = m.getRow(0).getZ();
    to[2] = m.getRow(0).getY();
    to[3] = m.getRow(2).getX();
    to[4] = m.getRow(2).getZ();
    to[5] = m.getRow(2).getY();
    to[6] = m.getRow(1).getZ();
    to[7] = m.getRow(1).getX();
    to[8] = m.getRow(1).getY();
}

/** Set the orientation (forward and up vectors) to the
 * provided rotation matrix
 * @param matrix matrix to use to set new orientation
 * @note this method is provided to ease performance interactions with 
 * the physics library
 */
void Position::setRowMajorRotationMatrix(const btMatrix3x3& matrix)
{
	btVector3 upV = matrix.getRow(1);
	btVector3 forwardV = matrix.getRow(2);
		
	// fill in y axis, second row
	up.set(upV.getX(), upV.getY(), upV.getZ());
							
	// fill in z axis, thrid row
	forward.set(forwardV.getX(), forwardV.getY(), forwardV.getZ());
	
}

void ConvertRotationToHL(const btMatrix3x3& matrix, QAngle& hl) {
	btQuaternion quat;
	matrix.getRotation(quat);
	Quaternion q(quat.getX(), -quat.getZ(), quat.getY(), quat.getW());
	RadianEuler radian(q);
	hl = radian.ToQAngle();
}

/** Get the rotation matrix into a 3x3 matrix
 * @param matrix out parameter to place matrix data into
 * @note this method is provided to ease performance interactions with 
 * the physics library
 */
void Position::getRowMajorRotationMatrix(btMatrix3x3& matrix)
{
	// get local x axis
	Vector3f xAxis;
	xAxis.crossProduct(up, forward);

	matrix.setValue (
	
		// fill in x axis, first column
		xAxis.getX(),
		xAxis.getY(),
		xAxis.getZ(),
		
		// fill in y axis, second column
		up.getX(),
		up.getY(),
		up.getZ(),
								
		// fill in z axis, thrid column
		forward.getX(),
		forward.getY(),
		forward.getZ()
	
	);
}

void btSetCrossMatrixMinus(btMatrix3x3& res, const btVector3& a)
{
	const btScalar a_0 = a[0], a_1 = a[1], a_2 = a[2];
	res.setValue(0, +a_2, -a_1,
		-a_2, 0, +a_0,
		+a_1, -a_0, 0);
}

QDomElement SimDomElement::basisToNode(QDomDocument &doc, const btMatrix3x3 mx)
{
	QDomElement matrix = doc.createElement( "matrix" );
	
	for(int i=0;i<3;i++)
		matrix.appendChild(vectorToNode(doc, mx.getRow(i)));
	return matrix;
}

static int operator!= ( const btMatrix3x3 &a, const btMatrix3x3 &b )
{
    int i; 
    btVector3 av3, bv3;

    for(i=0; i<3; i++)
    {
        av3 = a.getRow(i);
        bv3 = b.getRow(i);
        
        if( fabs(av3.m_floats[0] - bv3.m_floats[0]) + 
            fabs(av3.m_floats[1] - bv3.m_floats[1]) +
            fabs(av3.m_floats[2] - bv3.m_floats[2]) > FLT_EPSILON * 4)
            return 1;
    }
    
    return 0;
}

ork::CMatrix3 btbasistoorkmtx3( const btMatrix3x3& mtx )
{	ork::CMatrix3 rval;
	for( int i=0; i<3; i++ )
	{	const btVector3& vec = mtx.getColumn(i);
		rval.SetElemXY(i,0,float(vec.x()));
		rval.SetElemXY(i,1,float(vec.y()));
		rval.SetElemXY(i,2,float(vec.z()));
	}
	return rval;
}

void convertToBtTransform(const Vector3d& start_pos, const Vector3d& end_pos, btVector3& origin, btMatrix3x3& basis)
{
	Matrix3d rotation;
	rotation_from_tangent((start_pos - end_pos).normalized(), rotation);
	basis.setValue(rotation(0,0), rotation(0,1), rotation(0,2),
								 rotation(1,0), rotation(1,1), rotation(1,2),
								 rotation(2,0), rotation(2,1), rotation(2,2));
	Vector3d mid_point = (start_pos + end_pos)/2.0;
	origin.setValue(mid_point(0), mid_point(1), mid_point(2));
}

osg::Matrix
osgbCollision::asOsgMatrix( const btMatrix3x3& m )
{
    btScalar f[ 9 ];
    m.getOpenGLSubMatrix( f );
    return( osg::Matrix(
        f[0], f[1], f[2], 0.,
        f[3], f[4], f[5], 0.,
        f[6], f[7], f[8], 0.,
        0., 0., 0., 1. ) );
}

void btMatrix3_to_Matrix3(JNIEnv * const &jenv, jobject &target, const btMatrix3x3 &source)
{
	matrix3_ensurefields(jenv, target);
	
	jfloatArray valArray = (jfloatArray) jenv->GetObjectField(target, matrix3_val);
	jfloat * elements = jenv->GetFloatArrayElements(valArray, NULL);
	
	// Convert to column-major
	elements[0] = (jfloat) source.getColumn(0).getX();
	elements[1] = (jfloat) source.getColumn(0).getY();
	elements[2] = (jfloat) source.getColumn(0).getZ();
	elements[3] = (jfloat) source.getColumn(1).getX();
	elements[4] = (jfloat) source.getColumn(1).getY();
	elements[5] = (jfloat) source.getColumn(1).getZ();
	elements[6] = (jfloat) source.getColumn(2).getX();
	elements[7] = (jfloat) source.getColumn(2).getY();
	elements[8] = (jfloat) source.getColumn(2).getZ();
	
	jenv->ReleaseFloatArrayElements(valArray, elements, 0);  
	jenv->DeleteLocalRef(valArray);
}

static btMatrix3x3 M3x3mulM1M2_ref( const btMatrix3x3 &m1, const btMatrix3x3 &m2 )
{
	return btMatrix3x3(
        m2.tdotx(m1[0]), m2.tdoty(m1[0]), m2.tdotz(m1[0]),
		m2.tdotx(m1[1]), m2.tdoty(m1[1]), m2.tdotz(m1[1]),
		m2.tdotx(m1[2]), m2.tdoty(m1[2]), m2.tdotz(m1[2]));
}