C++ Vector3s::array方法代码示例

void CollisionUtilities::computeBoxSphereActiveSet( const Vector3s& xb, const Matrix33sc& Rb, const Vector3s& wb, const Vector3s& xs, const scalar& rs, std::vector<Vector3s>& p, std::vector<Vector3s>& n )
{
  // Rotation matrix should be orthonormal and orientation preserving
  assert( ( Rb * Rb.transpose() - Matrix33sc::Identity() ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
  assert( fabs( Rb.determinant() - 1.0 ) <= 1.0e-6 );
  // All half-widths should be positive
  assert( ( wb.array() > 0.0 ).all() );

  Vector3s closest_point;
  computeClosestPointToBox( xb, Rb, wb, xs, closest_point );
  const scalar dist_squared{ ( closest_point - xs ).squaredNorm() };
  
  // If the closest point is outside the sphere's radius, there is no collision
  if( dist_squared > rs * rs )
  {
    return;
  }

  // If we are inside the box, throw an error
  // TODO: Handle this case
  if( dist_squared <= 1.0e-9 )
  {
    std::cerr << "Error, degenerate case in Box-Sphere collision detection. Implement degenerate CD code or decrease the time step! Exiting." << std::endl;
    std::exit( EXIT_FAILURE );
  }
  
  // Compute the collision normal pointing from the sphere to the box
  const Vector3s normal{ ( closest_point - xs ) / sqrt( dist_squared ) };
  
  // Return the collision
  p.push_back( closest_point );
  n.push_back( normal );
}

static void diagonalizeInertiaTensor( const Matrix3s& I, Matrix3s& R0, Vector3s& I0 )
{
  // Inertia tensor should by symmetric
  assert( ( I - I.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
  // Inertia tensor should have positive determinant
  assert( I.determinant() > 0.0 );

  // Compute the eigenvectors and eigenvalues of the input matrix
  const Eigen::SelfAdjointEigenSolver<Matrix3s> es{ I };

  // Check for errors
  if( es.info() == Eigen::NumericalIssue )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NumericalIssue" << std::endl;
  }
  else if( es.info() == Eigen::NoConvergence )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NoConvergence" << std::endl;
  }
  else if( es.info() == Eigen::InvalidInput )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::InvalidInput" << std::endl;
  }
  assert( es.info() == Eigen::Success );

  // Save the eigenvectors and eigenvalues
  I0 = es.eigenvalues();
  assert( ( I0.array() > 0.0 ).all() );
  assert( I0.x() <= I0.y() );
  assert( I0.y() <= I0.z() );
  R0 = es.eigenvectors();
  assert( fabs( fabs( R0.determinant() ) - 1.0 ) <= 1.0e-6 );

  // Ensure that we have an orientation preserving transform
  if( R0.determinant() < 0.0 )
  {
    R0.col( 0 ) *= -1.0;
  }
}

void RigidBodySphere::computeAABB( const Vector3s& cm, const Matrix33sr& R, Array3s& min, Array3s& max ) const
{
  min = cm.array() - m_r;
  max = cm.array() + m_r;
}

// TODO: most of this function can be vectorized
void computeMoments( const Matrix3Xsc& vertices, const Matrix3Xuc& indices, scalar& mass, Vector3s& I, Vector3s& center, Matrix3s& R )
{
  assert( ( indices.array() < unsigned( vertices.cols() ) ).all() );

  const scalar oneDiv6{ 1.0 / 6.0 };
  const scalar oneDiv24{ 1.0 / 24.0 };
  const scalar oneDiv60{ 1.0 / 60.0 };
  const scalar oneDiv120{ 1.0 / 120.0 };

  // order:  1, x, y, z, x^2, y^2, z^2, xy, yz, zx
  VectorXs integral{ VectorXs::Zero( 10 ) };

  for( int i = 0; i < indices.cols(); ++i )
  {
    // Copy the vertices of triangle i
    const Vector3s v0{ vertices.col( indices( 0, i ) ) };
    const Vector3s v1{ vertices.col( indices( 1, i ) ) };
    const Vector3s v2{ vertices.col( indices( 2, i ) ) };
    
    // Compute a normal for the current triangle
    const Vector3s N{ ( v1 - v0 ).cross( v2 - v0 ) };

    // Compute the integral terms
    scalar tmp0{ v0.x() + v1.x() };
    scalar tmp1{ v0.x() * v0.x() };
    scalar tmp2{ tmp1 + v1.x() * tmp0 };
    const scalar f1x{ tmp0 + v2.x() };
    const scalar f2x{ tmp2 + v2.x() * f1x };
    const scalar f3x{ v0.x() * tmp1 + v1.x() * tmp2 + v2.x() * f2x };
    const scalar g0x{ f2x + v0.x() * ( f1x + v0.x() ) };
    const scalar g1x{ f2x + v1.x() * ( f1x + v1.x() ) };
    const scalar g2x{ f2x + v2.x() * ( f1x + v2.x() ) };
    
    tmp0 = v0.y() + v1.y();
    tmp1 = v0.y() * v0.y();
    tmp2 = tmp1 + v1.y() * tmp0;
    const scalar f1y{ tmp0 + v2.y() };
    const scalar f2y{ tmp2 + v2.y() * f1y };
    const scalar f3y{ v0.y() * tmp1 + v1.y() * tmp2 + v2.y() * f2y };
    const scalar g0y{ f2y + v0.y() * ( f1y + v0.y() ) };
    const scalar g1y{ f2y + v1.y() * ( f1y + v1.y() ) };
    const scalar g2y{ f2y + v2.y() * ( f1y + v2.y() ) };
    
    tmp0 = v0.z() + v1.z();
    tmp1 = v0.z()*v0.z();
    tmp2 = tmp1 + v1.z()*tmp0;
    const scalar f1z{ tmp0 + v2.z() };
    const scalar f2z{ tmp2 + v2.z() * f1z };
    const scalar f3z{ v0.z() * tmp1 + v1.z() * tmp2 + v2.z() * f2z };
    const scalar g0z{ f2z + v0.z() * ( f1z + v0.z() ) };
    const scalar g1z{ f2z + v1.z() * ( f1z + v1.z() ) };
    const scalar g2z{ f2z + v2.z() * ( f1z + v2.z() ) };
    
    // Update integrals
    integral(0) += N.x() * f1x;
    integral(1) += N.x() * f2x;
    integral(2) += N.y() * f2y;
    integral(3) += N.z() * f2z;
    integral(4) += N.x() * f3x;
    integral(5) += N.y() * f3y;
    integral(6) += N.z() * f3z;
    integral(7) += N.x() * ( v0.y() * g0x + v1.y() * g1x + v2.y() * g2x );
    integral(8) += N.y() * ( v0.z() * g0y + v1.z() * g1y + v2.z() * g2y );
    integral(9) += N.z() * ( v0.x() * g0z + v1.x() * g1z + v2.x() * g2z );
  }
  
  integral(0) *= oneDiv6;
  integral(1) *= oneDiv24;
  integral(2) *= oneDiv24;
  integral(3) *= oneDiv24;
  integral(4) *= oneDiv60;
  integral(5) *= oneDiv60;
  integral(6) *= oneDiv60;
  integral(7) *= oneDiv120;
  integral(8) *= oneDiv120;
  integral(9) *= oneDiv120;
  
  // Mass
  mass = integral(0);
  
  // Center of mass
  center = Vector3s( integral(1), integral(2), integral(3) )/mass;
  
  // Inertia relative to world origin
  R(0,0) = integral(5) + integral(6);
  R(0,1) = -integral(7);
  R(0,2) = -integral(9);
  R(1,0) = R(0,1);
  R(1,1) = integral(4) + integral(6);
  R(1,2) = -integral(8);
  R(2,0) = R(0,2);
  R(2,1) = R(1,2);
  R(2,2) = integral(4) + integral(5);
  
  // Comptue the inertia relative to the center of mass
  R(0,0) -= mass * ( center.y() * center.y() + center.z() * center.z() );
  R(0,1) += mass * center.x() * center.y();
  R(0,2) += mass * center.z() * center.x();
  R(1,0) = R(0,1);
//.........这里部分代码省略.........