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

Eigen::Vector3d Circuit::GetFieldFromPoint(
  const Eigen::Vector3d& point,
  const Eigen::Vector3d& segmentCenter,
  const Eigen::Vector3d& flowDirection,
  double segmentCoeff
) {
  Eigen::Vector3d r = segmentCenter.array()-point.array();
  double rLengthSquared = r.squaredNorm(); // distance squared.
  Eigen::Vector3d rUnit = r.normalized();
  Eigen::Vector3d Vcrossr = flowDirection.cross(rUnit);

  Eigen::Vector3d B = segmentCoeff/rLengthSquared*Vcrossr.array();
  return B;
}

Eigen::Matrix3d Bd970::getOrientationUncertainty(void)
{
    Eigen::Matrix3d orientation_uncertainty;

    /** Get the yaw with respect to the north **/
    Eigen::Vector3d variance;
    variance << 0.00, 0.00, (m_current_nmea.data_gst.heading_sigma_error*m_current_nmea.data_gst.heading_sigma_error);
    orientation_uncertainty = variance.array().matrix().asDiagonal();

    return orientation_uncertainty;
}

void mesh_core::generateAABB(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d& min,
      Eigen::Vector3d& max)
{
  min = Eigen::Vector3d(std::numeric_limits<double>::max(),
                        std::numeric_limits<double>::max(),
                        std::numeric_limits<double>::max());
  max = Eigen::Vector3d(-std::numeric_limits<double>::max(),
                        -std::numeric_limits<double>::max(),
                        -std::numeric_limits<double>::max());

  EigenSTL::vector_Vector3d::const_iterator it = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; it != end ; ++it)
  {
    min = min.array().min(it->array());
    max = max.array().max(it->array());
  }
}

Eigen::Vector3d Atom::CalculateElectricalForce(const Eigen::Vector3d& chargePosition, Real charge, shared_ptr<ForceVectors> forceVectorStorage) {
  Eigen::Vector3d forceDirection = chargePosition.array()-m_position.array();
  Real rSquared = forceDirection.squaredNorm();
  
  Real forceMagnitude = K_VACUUM*charge*m_effectiveNuclearCharge*Square(ELECTRIC_CHARGE)/rSquared;
  
  // The force is in the direction of the charge.
  forceDirection.normalize();
  Eigen::Vector3d forceVector = forceDirection.array()*forceMagnitude;
  
  if(forceVectorStorage) {
    forceVectorStorage->AddForceVector(GetName(), forceVector);
  }
  
  // Add in forces due to sp orbitals.
  for(int i=0; i<m_spOrbitals.size(); i++) {
    shared_ptr<SpOrbital> orbital = m_spOrbitals[i];
    Eigen::Vector3d spForce = orbital->CalculateElectricalForce(chargePosition, charge, forceVectorStorage);
    forceVector.array() += spForce.array();
  }
  
  return forceVector;
}

Eigen::Vector3d Circuit::GetField(const Eigen::Vector3d& point) {
  double segmentLength = 2.0*NXGR_PI*m_radius/m_segments;
  double segmentCoeff = PERMIABILITY_OVER_4PI*segmentLength*m_current;

  Eigen::Vector3d B(0.0);
  Eigen::Vector3d pos(0.0, 0.0, 0.0);
  Eigen::Vector3d up(0.0, 0.0, 1.0);

  double angle = 0.0;
  double angleInc = 2.0*NXGR_PI/m_segments;
  for(int i=0; i<m_segments; i++) {
    pos[0] = m_radius*cos(angle);
    pos[1] = m_radius*sin(angle);
    Eigen::Vector3d flow = pos.cross(up);
    flow.normalize();
    Eigen::Vector3d segmentB = GetFieldFromPoint(point, pos, flow, segmentCoeff);
    B.array() += segmentB.array();
    angle += angleInc;
  }
  return B;
}

void igl::fit_plane(
    const Eigen::MatrixXd & V,
    Eigen::RowVector3d & N,
    Eigen::RowVector3d & C)
{
  assert(V.rows()>0);

  Eigen::Vector3d sum = V.colwise().sum();

  Eigen::Vector3d center = sum.array()/(double(V.rows()));

  C = center;

  double sumXX=0.0f,sumXY=0.0f,sumXZ=0.0f;
  double sumYY=0.0f,sumYZ=0.0f;
  double sumZZ=0.0f;

  for(int i=0;i<V.rows();i++)
  {
    double diffX=V(i,0)-center(0);
    double diffY=V(i,1)-center(1);
    double diffZ=V(i,2)-center(2);
    sumXX+=diffX*diffX;
    sumXY+=diffX*diffY;
    sumXZ+=diffX*diffZ;
    sumYY+=diffY*diffY;
    sumYZ+=diffY*diffZ;
    sumZZ+=diffZ*diffZ;
  }

  Eigen::MatrixXd m(3,3);
  m << sumXX,sumXY,sumXZ,
    sumXY,sumYY,sumYZ,
    sumXZ,sumYZ,sumZZ;

  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(m);
  
  N = es.eigenvectors().col(0);
}

void SceneCloudView::update_current_cloud(const sensor_msgs::PointCloud2ConstPtr& msg, const Eigen::Affine3d& cam_pose, const Eigen::Vector3d& world_size)
{
    if (!cur_view_added_)
        return;

    pcl17::PointCloud<pcl17::PointXYZ> cloud;
    pcl17::fromROSMsg(*msg, cloud);

    current_cloud_ptr_->points.clear();
    current_cloud_ptr_->width = (int)cloud.points.size();
    current_cloud_ptr_->height = 1;

    BOOST_FOREACH (const pcl17::PointXYZ& pt, cloud.points)
    {
        Eigen::Vector3d p(pt.x, pt.y, pt.z);
        Eigen::Vector3d g_p = cam_pose * p;// + world_size / 2;
        if ((g_p.array() < world_size.array()).all())
        {
            current_cloud_ptr_->points.push_back(pcl17::PointXYZ(g_p(0), g_p(1), g_p(2)));
        }
    }
}