C++ Vector3s类代码示例

void StaticPlaneSphereConstraint::computeContactBasis( const VectorXs& q, const VectorXs& v, MatrixXXsc& basis ) const
{
  const Vector3s n{ m_plane.n() };
  assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );

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

  // Create a second orthogonal sample in the tangent plane
  const Vector3s t{ n.cross( s ).normalized() }; // Don't need to normalize but it won't hurt

  assert( MathUtilities::isRightHandedOrthoNormal( n, s, t, 1.0e-6 ) );
  basis.resize( 3, 3 );
  basis.col( 0 ) = n;
  basis.col( 1 ) = s;
  basis.col( 2 ) = t;
}

void FrictionUtilities::generateOrthogonalVectors( const Vector3s& n, std::vector<Vector3s>& vectors, const Vector3s& suggested_tangent )
{
  if( vectors.empty() )
  {
    return;
  }
  assert( ( suggested_tangent.cross( n ) ).squaredNorm() != 0.0 );

  // Make sure the first vector is orthogonal to n and unit length
  vectors[0] = ( suggested_tangent - n.dot( suggested_tangent ) * n ).normalized();
  assert( fabs( vectors[0].norm() - 1.0 ) <= 1.0e-10 );
  assert( fabs( n.dot( vectors[0] ) ) <= 1.0e-10 );

  // Generate the remaining vectors by rotating the first vector about n
  const scalar dtheta{ 2.0 * MathDefines::PI<scalar>() / scalar( vectors.size() ) };
  for( std::vector<Vector3s>::size_type i = 1; i < vectors.size(); ++i )
  {
    vectors[i] = Eigen::AngleAxis<scalar>{ i * dtheta, n } * vectors[0];
    assert( fabs( vectors[i].norm() - 1.0 ) <= 1.0e-10 );
    assert( fabs( n.dot( vectors[i] ) ) <= 1.0e-10 );
  }

  #ifndef NDEBUG
  if( vectors.size() == 1 )
  {
    return;
  }
  // Check that the angle between each vector is the one we expect
  for( std::vector<Vector3s>::size_type i = 0; i < vectors.size(); ++i )
  {
    assert( fabs( angleBetweenVectors( vectors[i], vectors[( i + 1 ) % vectors.size()] ) - dtheta ) < 1.0e-6 );
  }
  #endif
}

void TwoDSceneXMLParser::loadDragDampingForces( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
  assert( node != NULL );
  
  int forcenum = 0;
  for( rapidxml::xml_node<>* nd = node->first_node("dragdamping"); nd; nd = nd->next_sibling("dragdamping") )
  {
    Vector3s constforce;
    constforce.setConstant(std::numeric_limits<scalar>::signaling_NaN());
    
    // Extract the linear damping coefficient
    scalar b = std::numeric_limits<scalar>::signaling_NaN();
    if( nd->first_attribute("b") )
    {
      std::string attribute(nd->first_attribute("b")->value());
      if( !stringutils::extractFromString(attribute,b) )
      {
        std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of b attribute for dragdamping " << forcenum << ". Value must be numeric. Exiting." << std::endl;
        exit(1);
      }
    }
    else 
    {
      std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse b attribute for dragdamping " << forcenum << ". Exiting." << std::endl;
      exit(1);
    }
    
    //std::cout << "x: " << constforce.transpose() << std::endl;
    
    twodscene.insertForce(new DragDampingForce(b));
    
    ++forcenum;
  }  
}

Vector3s StaticPlaneSphereConstraint::computePlaneCollisionPointVelocity( const VectorXs& q ) const
{
  const Vector3s n{ m_plane.n() };

  // Compute the collision point on the plane relative to x
  const Vector3s plane_point{ ( q.segment<3>( 3 * m_sphere_idx ) - m_plane.x() ) - n.dot( q.segment<3>( 3 * m_sphere_idx ) - m_plane.x() ) * n };

  return m_plane.v() + m_plane.omega().cross( plane_point );
}

void RigidBodySphere::computeMassAndInertia( const scalar& density, scalar& M, Vector3s& CM, Vector3s& I, Matrix33sr& R ) const
{
  M = 4.0 * MathDefines::PI<scalar>() * m_r * m_r * m_r / 3.0;
  I.setConstant( 2.0 * M * m_r * m_r / 5.0 );
  M *= density;
  I *= density;
  CM.setZero();
  R.setIdentity();
}

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

scalar CollisionUtilities::closestPointPointSegment( const Vector3s& c, const Vector3s& a, const Vector3s& b, scalar& t )
{
  const Vector3s ab{ b - a };
  // Project c onto ab, computing parameterized position d(t) = a + t*(b – a)
  t = (c - a).dot( ab ) / ab.dot( ab );
  // If outside segment, clamp t (and therefore d) to the closest endpoint
  if( t < 0.0 )
  {
    t = 0.0;
  }
  if( t > 1.0 )
  {
    t = 1.0;
  }
  // Compute projected position from the clamped t
  const Vector3s& d{ a + t * ab };
  
  return ( c - d ).squaredNorm();
}

StaticCylinder::StaticCylinder( const Vector3s& x, const Vector3s& axis, const scalar& radius )
: m_x( x )
, m_R( Quaternions::FromTwoVectors( Vector3s::UnitY(), axis.normalized() ) )
, m_v( Vector3s::Zero() )
, m_omega( Vector3s::Zero() )
, m_r( radius )
{
  assert( fabs ( m_R.norm() - 1.0 ) < 1.0e-6 );
  assert( m_r > 0.0 );
}

void VortexForce::addGradEToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, VectorXs& gradE )
{
  assert( x.size() == v.size() );
  assert( x.size() == m.size() );
  assert( x.size() == gradE.size() );
  assert( x.size()%3 == 0 );
  assert( m_particles.first >= 0 );  assert( m_particles.first < x.size()/3 );
  assert( m_particles.second >= 0 ); assert( m_particles.second < x.size()/3 );

  Vector3s rhat = x.segment<3>(3*m_particles.second)-x.segment<3>(3*m_particles.first); 
  scalar r = rhat.norm(); 
  assert( r != 0.0 ); 
  rhat /= r;
  rhat *= m_kbs;
  // Rotate rhat 90 degrees clockwise
  scalar temp = rhat.x();
  rhat.x() = -rhat.y();
  rhat.y() = temp;
  
  rhat -= v.segment<3>(3*m_particles.second)-v.segment<3>(3*m_particles.first);
  
  rhat /= r*r;
  rhat *= m_kvc;

  gradE.segment<3>(3*m_particles.first)  -= -rhat;
  gradE.segment<3>(3*m_particles.second) += -rhat;
}

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 StapleStapleUtilities::computeStapleHalfPlaneActiveSet( const Vector3s& cm, const Matrix33sr& R, const RigidBodyStaple& staple,
                                                             const Vector3s& x0, const Vector3s& n, std::vector<int>& points )
{
  // For each vertex of the staple
  for( int i = 0; i < 4; ++i )
  {
    const Vector3s v{ R * staple.points()[ i ] + cm };
    // Compute the distance from the vertex to the halfplane
    const scalar d{ n.dot( v - x0 ) - staple.r() };
    // If the distance is not positive, we have a collision!
    if( d <= 0.0 )
    {
      points.emplace_back( i );
    }
  }
}

bool MathUtilities::isRightHandedOrthoNormal( const Vector3s& a, const Vector3s& b, const Vector3s& c, const scalar& tol )
{
  // All basis vectors should be unit
  if( fabs( a.norm() - 1.0 ) > tol ) { return false; }
  if( fabs( b.norm() - 1.0 ) > tol ) { return false; }
  if( fabs( c.norm() - 1.0 ) > tol ) { return false; }
  // All basis vectors should be mutually orthogonal
  if( fabs( a.dot( b ) ) > tol ) { return false; }
  if( fabs( a.dot( c ) ) > tol ) { return false; }
  if( fabs( b.dot( c ) ) > tol ) { return false; }
  // Coordinate system should be right handed
  if( ( a.cross( b ) - c ).lpNorm<Eigen::Infinity>() > tol ) { return false; }
  return true;
}

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( "" );
}

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( "" );
}