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

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

//.........这里部分代码省略.........
        // vhat_i += qidot * shat_i
        vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top;
        vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom;

        // calculate zhat_i^A
        zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force));
        zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];

        zero_acc_bottom_linear[i+1] =
            - (rot_from_world[i+1] * links[i].applied_torque);
		if (m_useGyroTerm)
		{
			zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] );
		}

        zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());

        // calculate Ihat_i^A
        inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
        inertia_top_right[i+1].setValue(links[i].mass, 0, 0,
                                  0, links[i].mass, 0,
                                  0, 0, links[i].mass);
        inertia_bottom_left[i+1].setValue(links[i].inertia[0], 0, 0,
                                    0, links[i].inertia[1], 0,
                                    0, 0, links[i].inertia[2]);
    }


    // 'Downward' loop.
    // (part of TreeForwardDynamics in Mirtich.)
    for (int i = num_links - 1; i >= 0; --i) {

        h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom;
        h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom;
		btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]);
        D[i] = val;
		Y[i] = links[i].joint_torque
            - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
            - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);

        const int parent = links[i].parent;

        
        // Ip += pXi * (Ii - hi hi' / Di) * iXp
        const btScalar one_over_di = 1.0f / D[i];

		


		const btMatrix3x3 TL = inertia_top_left[i+1]   - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
        const btMatrix3x3 TR = inertia_top_right[i+1]  - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
        const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);


        btMatrix3x3 r_cross;
        r_cross.setValue(
            0, -links[i].cached_r_vector[2], links[i].cached_r_vector[1],
            links[i].cached_r_vector[2], 0, -links[i].cached_r_vector[0],
            -links[i].cached_r_vector[1], links[i].cached_r_vector[0], 0);
        
        inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
        inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
        inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
            (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];