C++ btMatrix3x3::inverse方法代码示例

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

btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Cooper(btScalar step) const
{
#if 0
	dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size
	dVector3 L; // Compute angular momentum
	dMultiply0_331(L, I, b->avel);
#endif

	btVector3 inertiaLocal = getLocalInertia();
	btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
	btVector3 L = inertiaTensorWorld*getAngularVelocity();

	btMatrix3x3 Itild(0, 0, 0, 0, 0, 0, 0, 0, 0);

#if 0
	for (int ii = 0; ii<12; ++ii) {
		Itild[ii] = Itild[ii] * h + I[ii];
	}
#endif

	btSetCrossMatrixMinus(Itild, L*step);
	Itild += inertiaTensorWorld;
	

#if 0
// Compute a new effective 'inertia tensor'
// for the implicit step: the cross-product 
// matrix of the angular momentum plus the
// old tensor scaled by the timestep.  
// Itild may not be symmetric pos-definite, 
// but we can still use it to compute implicit
// gyroscopic torques.
dMatrix3 Itild = { 0 };
dSetCrossMatrixMinus(Itild, L, 4);
for (int ii = 0; ii<12; ++ii) {
	Itild[ii] = Itild[ii] * h + I[ii];
}
#endif

	L *= step;
	//Itild may not be symmetric pos-definite
	btMatrix3x3 itInv = Itild.inverse();
	Itild =  inertiaTensorWorld * itInv;
	btMatrix3x3 ident(1,0,0,0,1,0,0,0,1);
	Itild -= ident;

	


#if 0
// Scale momentum by inverse time to get 
// a sort of "torque"
dScaleVector3(L, dRecip(h));
// Invert the pseudo-tensor
dMatrix3 itInv;
// This is a closed-form inversion.
// It's probably not numerically stable
// when dealing with small masses with
// a large asymmetry.
// An LU decomposition might be better.
if (dInvertMatrix3(itInv, Itild) != 0) {
	// "Divide" the original tensor
	// by the pseudo-tensor (on the right)
	dMultiply0_333(Itild, I, itInv);
	// Subtract an identity matrix
	Itild[0] -= 1; Itild[5] -= 1; Itild[10] -= 1;

	// This new inertia matrix rotates the 
	// momentum to get a new set of torques
	// that will work correctly when applied
	// to the old inertia matrix as explicit
	// torques with a semi-implicit update
	// step.
	dVector3 tau0;
	dMultiply0_331(tau0, Itild, L);

	// Add the gyro torques to the torque 
	// accumulator
	for (int ii = 0; ii<3; ++ii) {
		b->tacc[ii] += tau0[ii];
	}
#endif
	btVector3 tau0 = Itild * L;
//	printf("tau0 = %f,%f,%f\n",tau0.x(),tau0.y(),tau0.z());
	return tau0;
}

btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(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();

//.........这里部分代码省略.........