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

static int operator!= ( const btMatrix3x3 &a, const btMatrix3x3 &b )
{
    if( a.getRow(0) != b.getRow(0) )
        return 1;
    if( a.getRow(1) != b.getRow(1) )
        return 1;
    if( a.getRow(2) != b.getRow(2) )
        return 1;
    return 0;
}

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

static btMatrix3x3 M3x3setRot_ref( btMatrix3x3 &m, const btQuaternion &q )
{
    btScalar d = q.length2();
    btScalar s = btScalar(2.0) / d;

    btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
    
    btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
    btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
    btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
    m.setValue(
        btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
        xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
        xz - wy, yz + wx, btScalar(1.0) - (xx + yy));

    return m;
}

unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const
{
  // Use the 'u' and 'h' matrices for intermediate calculations
  u = a;
  h = a.inverse();

  for (unsigned int i = 0; i < m_maxIterations; ++i)
  {
    const btScalar h_1 = p1_norm(h);
    const btScalar h_inf = pinf_norm(h);
    const btScalar u_1 = p1_norm(u);
    const btScalar u_inf = pinf_norm(u);

    const btScalar h_norm = h_1 * h_inf;
    const btScalar u_norm = u_1 * u_inf;

    // The matrix is effectively singular so we cannot invert it
    if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
      break;

    const btScalar gamma = btPow(h_norm / u_norm, 0.25f);
    const btScalar inv_gamma = 1.0 / gamma;

    // Determine the delta to 'u'
    const btMatrix3x3 delta = (u * (gamma - 2.0) + h.transpose() * inv_gamma) * 0.5;

    // Update the matrices
    u += delta;
    h = u.inverse();

    // Check for convergence
    if (p1_norm(delta) <= m_tolerance * u_1)
    {
      h = u.transpose() * a;
      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.
  h = u.transpose() * a;
  h = (h + h.transpose()) * 0.5;

  return m_maxIterations;
}

static int operator!= ( const btMatrix3x3 &a, const btMatrix3x3 &b )
{
    if( a.getRow(0) != b.getRow(0) )
	{
		if (!fuzzyEqualSlow(a.getRow(0),b.getRow(0)))
		{
			return 1;
		}
	}
    if( a.getRow(1) != b.getRow(1) )
	{
	    if( !fuzzyEqualSlow(a.getRow(1),b.getRow(1)) )
	        return 1;
	}
    if( a.getRow(2) != b.getRow(2) )
	{
		if( !fuzzyEqualSlow(a.getRow(2),b.getRow(2)) )
		{
			return 1;
		}
	}
    return 0;
}