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

void BodyBodyConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
  assert( q.size() % 12 == 0 );
  assert( col >= 0 );
  assert( col < G.cols() );

  const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };

  // MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
  {
    assert( 3 * nbodies + 3 * m_idx0 + 2 < unsigned( G.rows() ) );
    G.insert( 3 * m_idx0 + 0, col ) = m_n.x();
    G.insert( 3 * m_idx0 + 1, col ) = m_n.y();
    G.insert( 3 * m_idx0 + 2, col ) = m_n.z();
    const Vector3s ntilde_0{ m_r0.cross( m_n ) };
    G.insert( 3 * ( m_idx0 + nbodies ) + 0, col ) = ntilde_0.x();
    G.insert( 3 * ( m_idx0 + nbodies ) + 1, col ) = ntilde_0.y();
    G.insert( 3 * ( m_idx0 + nbodies ) + 2, col ) = ntilde_0.z();
  }

  {
    assert( 3 * nbodies + 3 * m_idx1 + 2 < unsigned( G.rows() ) );
    G.insert( 3 * m_idx1 + 0, col ) = - m_n.x();
    G.insert( 3 * m_idx1 + 1, col ) = - m_n.y();
    G.insert( 3 * m_idx1 + 2, col ) = - m_n.z();
    const Vector3s ntilde_1{ m_r1.cross( m_n ) };
    G.insert( 3 * ( m_idx1 + nbodies ) + 0, col ) = - ntilde_1.x();
    G.insert( 3 * ( m_idx1 + nbodies ) + 1, col ) = - ntilde_1.y();
    G.insert( 3 * ( m_idx1 + nbodies ) + 2, col ) = - ntilde_1.z();
  }
}

void BodyBodyConstraint::computeGeneralizedFrictionGivenTangentSample( const VectorXs& q, const VectorXs& t, const unsigned column, SparseMatrixsc& D ) const
{
  assert( column < unsigned( D.cols() ) );
  assert( q.size() % 12 == 0 );
  assert( t.size() == 3 );
  assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 );
  assert( fabs( m_n.dot( t ) ) <= 1.0e-6 );
  assert( m_idx0 < m_idx1 );

  const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };

  // Effect on center of mass of body i
  D.insert( 3 * m_idx0 + 0, column ) = t.x();
  D.insert( 3 * m_idx0 + 1, column ) = t.y();
  D.insert( 3 * m_idx0 + 2, column ) = t.z();
  // Effect on orientation of body i
  {
    const Vector3s ntilde0{ m_r0.cross( Eigen::Map<const Vector3s>{ t.data() } ) };
    D.insert( 3 * ( m_idx0 + nbodies ) + 0, column ) = ntilde0.x();
    D.insert( 3 * ( m_idx0 + nbodies ) + 1, column ) = ntilde0.y();
    D.insert( 3 * ( m_idx0 + nbodies ) + 2, column ) = ntilde0.z();
  }

  // Effect on center of mass of body j
  D.insert( 3 * m_idx1 + 0, column ) = - t.x();
  D.insert( 3 * m_idx1 + 1, column ) = - t.y();
  D.insert( 3 * m_idx1 + 2, column ) = - t.z();
  // Effect on orientation of body j
  {
    const Vector3s ntilde1{ m_r1.cross( Eigen::Map<const Vector3s>{ t.data() } ) };
    D.insert( 3 * ( m_idx1 + nbodies ) + 0, column ) = - ntilde1.x();
    D.insert( 3 * ( m_idx1 + nbodies ) + 1, column ) = - ntilde1.y();
    D.insert( 3 * ( m_idx1 + nbodies ) + 2, column ) = - ntilde1.z();
  }
}

// This method and the smooth version share the second half of code. Abstract that out.
void StaticPlaneSphereConstraint::computeGeneralizedFrictionDisk( const VectorXs& q, const VectorXs& v, const int start_column, const int num_samples, SparseMatrixsc& D, VectorXs& drel ) const
{
  assert( start_column >= 0 );
  assert( start_column < D.cols() );
  assert( num_samples > 0 );
  assert( start_column + num_samples - 1 < D.cols() );
  assert( q.size() % 12 == 0 );
  assert( q.size() == 2 * v.size() );

  const Vector3s n{ m_plane.n() };
  assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );

  std::vector<Vector3s> friction_disk( static_cast<std::vector<Vector3s>::size_type>( num_samples ) );
  assert( friction_disk.size() == std::vector<Vector3s>::size_type( num_samples ) );
  {
    // Compute the relative velocity
    Vector3s tangent_suggestion{ computeRelativeVelocity( q, v ) };
    if( tangent_suggestion.cross( n ).squaredNorm() < 1.0e-9 )
    {
      tangent_suggestion = FrictionUtilities::orthogonalVector( n );
    }
    tangent_suggestion *= -1.0;

    // Sample the friction disk
    FrictionUtilities::generateOrthogonalVectors( n, friction_disk, tangent_suggestion );
  }
  assert( unsigned( num_samples ) == friction_disk.size() );

  // Compute the displacement from the center of mass to the point of contact
  assert( fabs( n.norm() - 1.0 ) <= 1.0e-10 ); assert( m_r >= 0.0 );
  const Vector3s r_world{ - m_r * n };

  // Cache the velocity of the collision point on the plane
  const Vector3s plane_collision_point_vel{ computePlaneCollisionPointVelocity( q ) };

  // For each sample of the friction disk
  const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
  for( unsigned friction_sample = 0; friction_sample < unsigned( num_samples ); ++friction_sample )
  {
    const unsigned cur_col{ start_column + friction_sample };
    assert( cur_col < unsigned( D.cols() ) );

    // Effect on center of mass
    D.insert( 3 * m_sphere_idx + 0, cur_col ) = friction_disk[friction_sample].x();
    D.insert( 3 * m_sphere_idx + 1, cur_col ) = friction_disk[friction_sample].y();
    D.insert( 3 * m_sphere_idx + 2, cur_col ) = friction_disk[friction_sample].z();

    // Effect on orientation
    {
      const Vector3s ntilde{ r_world.cross( friction_disk[friction_sample] ) };
      D.insert( 3 * ( nbodies + m_sphere_idx ) + 0, cur_col ) = ntilde.x();
      D.insert( 3 * ( nbodies + m_sphere_idx ) + 1, cur_col ) = ntilde.y();
      D.insert( 3 * ( nbodies + m_sphere_idx ) + 2, cur_col ) = ntilde.z();
    }

    // Relative velocity contribution from kinematic scripting
    assert( cur_col < drel.size() );
    drel( cur_col ) = - friction_disk[friction_sample].dot( plane_collision_point_vel );
  }
}

void StaticPlaneSphereConstraint::computeGeneralizedFrictionGivenTangentSample( const VectorXs& q, const VectorXs& t, const unsigned column, SparseMatrixsc& D ) const
{
  assert( t.size() == 3 );
  assert( column < unsigned( D.cols() ) );
  assert( q.size() % 12 == 0 );
  assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 );
  assert( fabs( m_plane.n().dot( t ) ) <= 1.0e-6 );

  // Effect on center of mass
  D.insert( 3 * m_sphere_idx + 0, column ) = t.x();
  D.insert( 3 * m_sphere_idx + 1, column ) = t.y();
  D.insert( 3 * m_sphere_idx + 2, column ) = t.z();

  // Effect on orientation
  {
    const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
    // Compute the displacement from the center of mass to the point of contact
    assert( fabs( m_plane.n().norm() - 1.0 ) <= 1.0e-10 );
    assert( m_r >= 0.0 );
    const Vector3s r_world{ - m_r * m_plane.n() };
    const Vector3s ntilde{ r_world.cross( Eigen::Map<const Vector3s>( t.data() ) ) };
    D.insert( 3 * ( nbodies + m_sphere_idx ) + 0, column ) = ntilde.x();
    D.insert( 3 * ( nbodies + m_sphere_idx ) + 1, column ) = ntilde.y();
    D.insert( 3 * ( nbodies + m_sphere_idx ) + 2, column ) = ntilde.z();
  }
}

// TODO: This doesn't handle <0,0,0>
Vector3s FrictionUtilities::orthogonalVector( const Vector3s& n )
{
  assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 ); // TODO: Remove this
  // Chose the most orthogonal direction among x, y, z
  Vector3s orthog{ fabs(n.x()) <= fabs(n.y()) && fabs(n.x()) <= fabs(n.z()) ? Vector3s::UnitX() : fabs(n.y()) <= fabs(n.z()) ? Vector3s::UnitY() : Vector3s::UnitZ() };
  assert( orthog.cross(n).squaredNorm() != 0.0 ); // New vector shouldn't be parallel to the input
  // Project out any non-orthogonal component
  orthog -= n.dot( orthog ) * n;
  assert( orthog.norm() != 0.0 );
  return orthog.normalized();
}

void StaticPlaneSphereConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
  assert( col >= 0 );
  assert( col < G.cols() );
  assert( 3 * m_sphere_idx + 2 < unsigned( G.rows() ) );

  const Vector3s n{ m_plane.n() };
  assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );

  // MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
  G.insert( 3 * m_sphere_idx + 0, col ) = n.x();
  G.insert( 3 * m_sphere_idx + 1, col ) = n.y();
  G.insert( 3 * m_sphere_idx + 2, col ) = n.z();
}

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

bool XMLExporter::saveToXMLFile( const std::string& file_name, const RigidBody3DState& state )
{
  std::ofstream xml_file( file_name );

  if( !xml_file.is_open() )
  {
    std::cerr << "Error, failed to open file " << file_name << " for output." << std::endl;
    return false;
  }

  xml_file << "<rigidbody3d_scene>" << std::endl;

  // Save the geometry out
  for( const std::unique_ptr<RigidBodyGeometry>& geo : state.geometry() )
  {
    switch( geo->getType() )
    {
      case RigidBodyGeometryType::SPHERE:
      {
        const RigidBodySphere& sphere{ static_cast<RigidBodySphere&>( *geo.get() ) };
        xml_file << "  <geometry type=\"sphere\" r=\"" << sphere.r() << "\"/>" << std::endl;
        break;
      }
      case RigidBodyGeometryType::BOX:
      {
        std::cerr << "Code up box geometry xml output." << std::endl;
        std::exit( EXIT_FAILURE );
      }
      case RigidBodyGeometryType::STAPLE:
      {
        std::cerr << "Code up staple geometry xml output." << std::endl;
        std::exit( EXIT_FAILURE );
      }
      case RigidBodyGeometryType::TRIANGLE_MESH:
      {
        const RigidBodyTriangleMesh& tri_mesh{ static_cast<RigidBodyTriangleMesh&>( *geo.get() ) };
        xml_file << "  <geometry type=\"mesh\" filename=\"" << tri_mesh.inputFileName() << "\"/>" << std::endl;
        break;
      }
    }
  }

  // Save the bodies out
  for( unsigned bdy_idx = 0; bdy_idx < state.nbodies(); bdy_idx++ )
  {
    const Vector3s x{ state.q().segment<3>( 3 * bdy_idx ) };

    const Matrix33sr R{ Eigen::Map<const Matrix33sr>{ state.q().segment<9>( 3 * state.nbodies() + 9 * bdy_idx ).data() } };
    using std::fabs;
    assert( fabs( R.determinant() - 1.0 ) <= 1.0e-9 );
    assert( ( R.transpose() * R - Matrix33sr::Identity() ).lpNorm<Eigen::Infinity>() <= 1.0e-9 );
    const Vector3s R_aa{ matrixToAngleAxis( R ) };

    const Vector3s v{ state.v().segment<3>( 3 * bdy_idx ) };

    const Vector3s omega{ state.v().segment<3>( 3 * state.nbodies() + 3 * bdy_idx ) };

    const scalar density{ state.getTotalMass(bdy_idx) / state.getGeometryOfBody(bdy_idx).volume() };

    const bool is_fixed{ state.isKinematicallyScripted(bdy_idx) };

    const unsigned geo_idx{ state.getGeometryIndexOfBody(bdy_idx) };

    xml_file << "  <rigid_body_with_density x=\"" << x.x() << " " << x.y() << " " << x.z() << "\" R=\"" << R_aa.x() << " " << R_aa.y() << " " << R_aa.z() << "\" v=\"" << v.x() << " " << v.y() << " " << v.z() << "\" omega=\"" << omega.x() << " " << omega.y() << " " << omega.z() << "\" rho=\"" << density << "\" fixed=\"" << is_fixed << "\" geo_idx=\"" << geo_idx << "\"/>" << std::endl;
  }

  xml_file << "</rigidbody3d_scene>" << std::endl;

  return true;
}

void BodyBodyConstraint::computeGeneralizedFrictionDisk( const VectorXs& q, const VectorXs& v, const int start_column, const int num_samples, SparseMatrixsc& D, VectorXs& drel ) const
{
  assert( start_column >= 0 );
  assert( start_column < D.cols() );
  assert( num_samples > 0 );
  assert( start_column + num_samples - 1 < D.cols() );
  assert( q.size() % 12 == 0 );
  assert( q.size() == 2 * v.size() );

  const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };

  assert( fabs( m_n.norm() - 1.0 ) <= 1.0e-6 );
  std::vector<Vector3s> friction_disk;
  {
    // Compute the relative velocity
    Vector3s tangent_suggestion{ computeRelativeVelocity( q, v ) };
    if( tangent_suggestion.cross( m_n ).squaredNorm() < 1.0e-9 )
    {
      tangent_suggestion = FrictionUtilities::orthogonalVector( m_n );
    }
    tangent_suggestion *= -1.0;

    // Sample the friction disk
    friction_disk.resize( num_samples );
    FrictionUtilities::generateOrthogonalVectors( m_n, friction_disk, tangent_suggestion );
  }
  assert( unsigned( num_samples ) == friction_disk.size() );

  // For each sample of the friction disk
  assert( m_idx0 < m_idx1 );
  for( int i = 0; i < num_samples; ++i )
  {
    const int cur_col{ start_column + i };
    assert( cur_col >= 0 );
    assert( cur_col < D.cols() );

    // Effect on center of mass of body i
    D.insert( 3 * m_idx0 + 0, cur_col ) = friction_disk[i].x();
    D.insert( 3 * m_idx0 + 1, cur_col ) = friction_disk[i].y();
    D.insert( 3 * m_idx0 + 2, cur_col ) = friction_disk[i].z();
    // Effect on orientation of body i
    {
      const Vector3s ttilde0{ m_r0.cross( friction_disk[i] ) };
      D.insert( 3 * ( m_idx0 + nbodies ) + 0, cur_col ) = ttilde0.x();
      D.insert( 3 * ( m_idx0 + nbodies ) + 1, cur_col ) = ttilde0.y();
      D.insert( 3 * ( m_idx0 + nbodies ) + 2, cur_col ) = ttilde0.z();
    }

    // Effect on center of mass of body j
    D.insert( 3 * m_idx1 + 0, cur_col ) = - friction_disk[i].x();
    D.insert( 3 * m_idx1 + 1, cur_col ) = - friction_disk[i].y();
    D.insert( 3 * m_idx1 + 2, cur_col ) = - friction_disk[i].z();
    // Effect on orientation of body j
    {
      const Vector3s ttilde1{ m_r1.cross( friction_disk[i] ) };
      D.insert( 3 * ( m_idx1 + nbodies ) + 0, cur_col ) = - ttilde1.x();
      D.insert( 3 * ( m_idx1 + nbodies ) + 1, cur_col ) = - ttilde1.y();
      D.insert( 3 * ( m_idx1 + nbodies ) + 2, cur_col ) = - ttilde1.z();
    }

    // Relative velocity contribution from kinematic scripting
    assert( cur_col < drel.size() );
    // Zero for now
    drel( cur_col ) = 0.0;
  }
}

void BodyBodyConstraint::computeSmoothGeneralizedFrictionDisk( const VectorXs& q, const VectorXs& v, const int start_column, SparseMatrixsc& D ) const
{
  assert( start_column >= 0 );
  assert( start_column < D.cols() );
  assert( start_column+1 < D.cols() );
  assert( q.size() % 12 == 0 );
  assert( q.size() == 2 * v.size() );

  std::vector<Vector3s> friction_disk{ 2 };

  // Compute the relative velocity to use as a direction for the tangent sample
  friction_disk[0] = computeRelativeVelocity( q, v );
  // If the relative velocity is zero, any vector will do
  if( friction_disk[0].cross( m_n ).squaredNorm() < 1.0e-9 )
  {
    friction_disk[0] = FrictionUtilities::orthogonalVector( m_n );
  }
  // Otherwise project out the component along the normal and normalize the relative velocity
  else
  {
    friction_disk[0] = ( friction_disk[0] - friction_disk[0].dot( m_n ) * m_n ).normalized();
  }
  // Invert the tangent vector in order to oppose
  friction_disk[0] *= -1.0;

  // Create a second orthogonal sample in the tangent plane
  friction_disk[1] = m_n.cross( friction_disk[0] ).normalized(); // Don't need to normalize but it won't hurt
  assert( MathUtilities::isRightHandedOrthoNormal( m_n, friction_disk[0], friction_disk[1], 1.0e-6 ) );

  // For each sample of the friction disk
  assert( m_idx0 < m_idx1 );
  const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
  for( int i = 0; i < 2; ++i )
  {
    const int cur_col = start_column + i;
    assert( cur_col >= 0 );
    assert( cur_col < D.cols() );

    // Effect on center of mass of body i
    D.insert( 3 * m_idx0 + 0, cur_col ) = friction_disk[i].x();
    D.insert( 3 * m_idx0 + 1, cur_col ) = friction_disk[i].y();
    D.insert( 3 * m_idx0 + 2, cur_col ) = friction_disk[i].z();
    // Effect on orientation of body i
    {
      const Vector3s ntilde0{ m_r0.cross( friction_disk[i] ) };
      D.insert( 3 * ( m_idx0 + nbodies ) + 0, cur_col ) = ntilde0.x();
      D.insert( 3 * ( m_idx0 + nbodies ) + 1, cur_col ) = ntilde0.y();
      D.insert( 3 * ( m_idx0 + nbodies ) + 2, cur_col ) = ntilde0.z();
    }

    // Effect on center of mass of body j
    D.insert( 3 * m_idx1 + 0, cur_col ) = - friction_disk[i].x();
    D.insert( 3 * m_idx1 + 1, cur_col ) = - friction_disk[i].y();
    D.insert( 3 * m_idx1 + 2, cur_col ) = - friction_disk[i].z();
    // Effect on orientation of body j
    {
      const Vector3s ntilde1{ m_r1.cross( friction_disk[i] ) };
      D.insert( 3 * ( m_idx1 + nbodies ) + 0, cur_col ) = - ntilde1.x();
      D.insert( 3 * ( m_idx1 + nbodies ) + 1, cur_col ) = - ntilde1.y();
      D.insert( 3 * ( m_idx1 + nbodies ) + 2, cur_col ) = - ntilde1.z();
    }
  }
}

static PyObject* setStaticCylinderAngularVelocity( PyObject* self, PyObject* args )
{
  using std::is_same;
  static_assert( is_same<scalar,double>::value || is_same<scalar,float>::value, "Error, scalar type must be double or float for Python interface." );
  unsigned cylinder_idx;
  Vector3s omega;
  assert( args != nullptr );
  if( !PyArg_ParseTuple( args, is_same<scalar,double>::value ? "Iddd" : "Ifff", &cylinder_idx, &omega.x(), &omega.y(), &omega.z() ) )
  {
    PyErr_Print();
    std::cerr << "Failed to read parameters for setStaticCylinderAngularVelocity, parameters are: cylinder_idx, omegax, omegay, omegaz. Exiting." << std::endl;
    std::exit( EXIT_FAILURE );
  }
  assert( s_sim_state != nullptr );
  if( cylinder_idx > s_sim_state->staticCylinders().size() )
  {
    std::cerr << "Invalid cylinder_idx parameter of " << cylinder_idx << " in setStaticCylinderAngularVelocity, cylinder_idx must be less than " << s_sim_state->staticCylinders().size() << ". Exiting." << std::endl;
    std::exit( EXIT_FAILURE );
  }
  s_sim_state->staticCylinder(cylinder_idx).omega() = omega;
  return Py_BuildValue( "" );
}

static PyObject* setStaticPlaneVelocity( PyObject* self, PyObject* args )
{
  using std::is_same;
  static_assert( is_same<scalar,double>::value || is_same<scalar,float>::value, "Error, scalar type must be double or float for Python interface." );
  unsigned plane_idx;
  Vector3s velocity;
  assert( args != nullptr );
  if( !PyArg_ParseTuple( args, is_same<scalar,double>::value ? "Iddd" : "Ifff", &plane_idx, &velocity.x(), &velocity.y(), &velocity.z() ) )
  {
    PyErr_Print();
    std::cerr << "Failed to read parameters for setStaticPlaneVelocity, parameters are: plane_idx, vx, vy, vz. Exiting." << std::endl;
    std::exit( EXIT_FAILURE );
  }
  assert( s_sim_state != nullptr );
  if( plane_idx > s_sim_state->staticPlanes().size() )
  {
    std::cerr << "Invalid plane_idx parameter of " << plane_idx << " in setStaticPlaneVelocity, plane_idx must be less than " << s_sim_state->staticPlanes().size() << ". Exiting." << std::endl;
    std::exit( EXIT_FAILURE );
  }
  s_sim_state->staticPlane(plane_idx).v() = velocity;
  return Py_BuildValue( "" );
}

void TwoDSceneXMLParser::loadParticles( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
  assert( node != NULL );
  
  // Count the number of particles
  int numparticles = 0;
  for( rapidxml::xml_node<>* nd = node->first_node("particle"); nd; nd = nd->next_sibling("particle") ) ++numparticles;
  
  twodscene.resizeSystem(numparticles);
  
  //std::cout << "Num particles " << numparticles << std::endl;
  
  std::vector<std::string>& tags = twodscene.getParticleTags();
  
  int particle = 0;
  for( rapidxml::xml_node<>* nd = node->first_node("particle"); nd; nd = nd->next_sibling("particle") )
  {
    // Extract the particle's initial position
    Vector3s pos;
    if( nd->first_attribute("px") ) 
    {
      std::string attribute(nd->first_attribute("px")->value());
      if( !stringutils::extractFromString(attribute,pos.x()) )
      {
        std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of px attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
        exit(1);
      }
    }
    else 
    {
      std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse px attribute for particle " << particle << ". Exiting." << std::endl;
      exit(1);
    }

    if( nd->first_attribute("py") ) 
    {
      std::string attribute(nd->first_attribute("py")->value());
      if( !stringutils::extractFromString(attribute,pos.y()) )
      {
        std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of py attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
        exit(1);
      }
    }
    else 
    {
      std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse py attribute for particle " << particle << ". Exiting." << std::endl;
      exit(1);
    }

    if( nd->first_attribute("pz") ) 
    {
      std::string attribute(nd->first_attribute("pz")->value());
      if( !stringutils::extractFromString(attribute,pos.z()) )
      {
        std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of pz attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
        exit(1);
      }
    }
    else 
    {
      std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse pz attribute for particle " << particle << ". Exiting." << std::endl;
      exit(1);
    }
    twodscene.setPosition( particle, pos );

    // Extract the particle's initial velocity
    Vector3s vel;
    if( nd->first_attribute("vx") ) 
    {
      std::string attribute(nd->first_attribute("vx")->value());
      if( !stringutils::extractFromString(attribute,vel.x()) )
      {
        std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of vx attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
        exit(1);
      }
    }
    else 
    {
      std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse vx attribute for particle " << particle << ". Exiting." << std::endl;
      exit(1);
    }
    
    if( nd->first_attribute("vy") ) 
    {
      std::string attribute(nd->first_attribute("vy")->value());
      if( !stringutils::extractFromString(attribute,vel.y()) )
      {
        std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of vy attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
        exit(1);
      }
    }
    else 
    {
      std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse vy attribute for particle " << particle << ". Exiting." << std::endl;
      exit(1);
    }

    if( nd->first_attribute("vz") ) 
    {
      std::string attribute(nd->first_attribute("vz")->value());
//.........这里部分代码省略.........

void TwoDSceneXMLParser::loadSimpleGravityForces( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
  assert( node != NULL );
  
  // Load each constant force
  int forcenum = 0;
  for( rapidxml::xml_node<>* nd = node->first_node("simplegravity"); nd; nd = nd->next_sibling("simplegravity") )
  {
    Vector3s constforce;
    constforce.setConstant(std::numeric_limits<scalar>::signaling_NaN());
    
    // Extract the x component of the force
    if( nd->first_attribute("fx") ) 
    {
      std::string attribute(nd->first_attribute("fx")->value());
      if( !stringutils::extractFromString(attribute,constforce.x()) )
      {
        std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of fx attribute for constantforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
        exit(1);
      }
    }
    else 
    {
      std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse fx attribute for constantforce " << forcenum << ". Exiting." << std::endl;
      exit(1);
    }
    
    // Extract the y component of the force
    if( nd->first_attribute("fy") ) 
    {
      std::string attribute(nd->first_attribute("fy")->value());
      if( !stringutils::extractFromString(attribute,constforce.y()) )
      {
        std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of fy attribute for constantforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
        exit(1);
      }
    }
    else 
    {
      std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse fy attribute for constantforce " << forcenum << ". Exiting." << std::endl;
      exit(1);
    }
    
    // Extract the y component of the force
    if( nd->first_attribute("fz") ) 
    {
      std::string attribute(nd->first_attribute("fz")->value());
      if( !stringutils::extractFromString(attribute,constforce.z()) )
      {
        std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of fz attribute for constantforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
        exit(1);
      }
    }
    else 
    {
      std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse fz attribute for constantforce " << forcenum << ". Exiting." << std::endl;
      exit(1);
    }
    
    //std::cout << "x: " << constforce.transpose() << std::endl;
    
    twodscene.insertForce(new SimpleGravityForce(constforce));
    
    ++forcenum;
  }  
}

// 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);
//.........这里部分代码省略.........