C++ pts函数代码示例

/*
 *  returns a roboop spline given the joint space via points and total duration
 */
Spl_path* TrajectoryController::createSpline(vector<ColumnVector>& jsPoints,
		double duration) {

	int size = jsPoints.size();
	if (size < 5) {
		return NULL;
	}

	//One row for time + 6 for each joint
	//TODO: ?? + 2 for adding the start and stop positions extra to keep the spline smooth
	Matrix pts(7, size);

	for (int i = 0; i < size; i++) {
		double time = 0.999 * (duration * i) / (size - 1);
		ColumnVector p = jsPoints.at(i);

		int index = i + 1;
		pts(1, index) = time;
		pts.SubMatrix(2, 7, index, index) = p;
	}
	cout << pts << endl;

	Spl_path* sp = new Spl_path(pts);
	return sp;
}

std::unique_ptr<SceneGraph::Node> AddChain::synchronize(
    std::unique_ptr<SceneGraph::Node> n) {
  if (pts().size() == 0) {
    return nullptr;
  }

  if (m_state & DirtyState::Points) {
    n = std::make_unique<Node>(pts());
    m_state ^= DirtyState::Points;
  }

  Node* node = static_cast<Node*>(n.get());
  if (m_state & DirtyState::MousePos) {
    node->setLastPoint(m_mousePos);
    m_state ^= DirtyState::MousePos;
  }

  if (m_state & DirtyState::Finished) {
    n = nullptr;

    m_pts.clear();
    m_state ^= DirtyState::Finished;
  }

  return n;
}

XC::MidDistanceBeamIntegration::MidDistanceBeamIntegration(int nIP,const Vector &pt)
  : ParameterIDBeamIntegration(BEAM_INTEGRATION_TAG_MidDistance,pt)
  {

    for(int i = 0; i < nIP; i++)
      {
        int key = i;
        for(int j = i+1; j < nIP; j++)
          {
            if(pts(j) < pts(key))
              {
	        key = j;
	        std::cerr << "MidDistanceBeamIntegration::MidDistanceBeamIntegration -- point are not sorted; sort before calling constructor" << std::endl;
              }
          }
        //double temp = pts(i);
        //pts(i) = pts(key);
        //pts(key) = temp;
      }

    Vector mids(nIP-1);

    for(int i = 0; i < nIP-1; i++)
      {
        mids(i) = 0.5*(pts(i)+pts(i+1));
      }

    wts(0) = mids(0);
    wts(nIP-1) = 1.0-mids(nIP-2);
    for(int i = 1; i < nIP-1; i++)
      { wts(i) = mids(i)-mids(i-1); }
  }

int Dmc_method::allocateIntermediateVariables(System * sys,
                                              Wavefunction_data * wfdata) {
  if(wf) delete wf;
  wf=NULL;
  if(sample) delete sample;
  sample=NULL;
  nelectrons=sys->nelectrons(0)+sys->nelectrons(1);
  wfdata->generateWavefunction(wf);
  sys->generateSample(sample);
  sample->attachObserver(wf);
  nwf=wf->nfunc();

  if(wf->nfunc() >1) 
    error("DMC doesn't support more than one guiding wave function");

  guidingwf=new Primary;

  pts.Resize(nconfig);
  for(int i=0; i < nconfig; i++) {
    pts(i).age.Resize(nelectrons);
    pts(i).age=0;
  }
  
  average_var.Resize(avg_words.size());
  average_var=NULL;
  for(int i=0; i< average_var.GetDim(0); i++) { 
    allocate(avg_words[i], sys, wfdata, average_var(i));
  }
  

  return 1;
}

void SkIntersectionHelper::dump() const {
    SkDPoint::Dump(pts()[0]);
    SkDPoint::Dump(pts()[1]);
    if (verb() >= SkPath::kQuad_Verb) {
        SkDPoint::Dump(pts()[2]);
    }
    if (verb() >= SkPath::kCubic_Verb) {
        SkDPoint::Dump(pts()[3]);
    }
}

bool TrainExample::isEqual(const ParseTree & pt) const
{    
    assert(pts().size() >= 2);

    const ParseInfo * p = pts()[0].rootNode()->parseInfo(&pts()[0]);
    const ParseInfo * p1 = pt.rootNode()->parseInfo(&pt);

    assert(p);
    assert(p1);

    return (pts()[0].dataId() == pt.dataId()) &&
           (p->c_ == p1->c_) && (p->l_ == p1->l_) &&
            (p->x_ == p1->x_) && (p->y_ == p1->y_ );
}

    void hex_5elem_inline()
    {
      libMesh::Point origin = libMesh::Point(0,0.5,0.5);

      // end points
      std::vector<libMesh::Point> pts(5);
      pts[0] = libMesh::Point(5.0,0.5,0.5);    // right
      pts[1] = libMesh::Point(3.75,0.0,0.25);  // front
      pts[2] = libMesh::Point(1.25,0.77,1.0);  // top
      pts[3] = libMesh::Point(2.22,1.0,0.667); // back
      pts[4] = libMesh::Point(0.38,0.57,0.0);  // bottom

      // exit_elem IDs
      std::vector<unsigned int> exit_ids(5);
      exit_ids[0] = 4;
      exit_ids[1] = 3;
      exit_ids[2] = 1;
      exit_ids[3] = 2;
      exit_ids[4] = 0;

      std::string hex8_string = this->mesh_3D("HEX8",5.0,1.0,1.0,5,1,1);
      std::string hex27_string = this->mesh_3D("HEX27",5.0,1.0,1.0,5,1,1);

      this->run_test_with_mesh_from_file(origin,pts,exit_ids,hex8_string);
      this->run_test_with_mesh_from_file(origin,pts,exit_ids,hex27_string);
    }

void MapCarObject::DrawObject(const QRect & rBox, QPainter * pDC, MapObjectState eState) const
{
	short iHeading;
	float cos0, sin0;
	QPoint ptCenter;
	QPointArray pts(3);

	if (m_pCar == NULL)
		return;

	m_pCar->m_mutexUpdate.lock();
	iHeading = m_pCar->GetCurrentDirection();
	m_pCar->m_mutexUpdate.unlock();

	ptCenter = rBox.center();
	cos0 = cosf(iHeading * RADIANSPERCENTIDEGREE);
	sin0 = sinf(iHeading * RADIANSPERCENTIDEGREE);

	pts[0] = ptCenter + QPoint((int)round(MAPCAROBJECT_CIRCLE_RADIUS*sin0), (int)round(-MAPCAROBJECT_CIRCLE_RADIUS*cos0));
	pts[1] = ptCenter + QPoint((int)round((MAPCAROBJECT_ARROW_WIDTH/2)*cos0-(MAPCAROBJECT_ARROW_LENGTH-MAPCAROBJECT_CIRCLE_RADIUS)*sin0), (int)round((MAPCAROBJECT_ARROW_LENGTH-MAPCAROBJECT_CIRCLE_RADIUS)*cos0+(MAPCAROBJECT_ARROW_WIDTH/2)*sin0));
	pts[2] = ptCenter + QPoint((int)round(-(MAPCAROBJECT_ARROW_WIDTH/2)*cos0-(MAPCAROBJECT_ARROW_LENGTH-MAPCAROBJECT_CIRCLE_RADIUS)*sin0), (int)round((MAPCAROBJECT_ARROW_LENGTH-MAPCAROBJECT_CIRCLE_RADIUS)*cos0-(MAPCAROBJECT_ARROW_WIDTH/2)*sin0));

	pDC->setPen(QPen(m_clrDraw, eState & MapObjectStateCurrent ? 2 : 0));
	pDC->drawLine(pts[0], pts[1]);
	pDC->drawLine(pts[0], pts[2]);
	if (!(eState & MapObjectStateInactive))
		pDC->drawEllipse((int)round(ptCenter.x() - MAPCAROBJECT_CIRCLE_RADIUS), (int)round(ptCenter.y() - MAPCAROBJECT_CIRCLE_RADIUS), (int)round(2*MAPCAROBJECT_CIRCLE_RADIUS), (int)round(2*MAPCAROBJECT_CIRCLE_RADIUS));
	if (hasReceivedCurrentMsg())
	{
		QBrush brushOld = pDC->brush();
		pDC->setBrush(m_clrDraw);
		pDC->drawPolygon(pts);
		pDC->setBrush(brushOld);
	}
}

void ArrowLine::drawShape(QPainter &p)
{
    p.setPen(darkGray);
    QCanvasLine::drawShape(p);

    double angle = computeAngle(startPoint().x(),
                                startPoint().y(),
                                endPoint().x(),
                                endPoint().y());
    QPointArray pts(3);

    QWMatrix m;
    int x, y;
    m.rotate(angle);
    m.map(-5, -2, &x, &y);
    pts.setPoint(0, x, y);
    m.map(-5, 2, &x, &y);
    pts.setPoint(1, x, y);
    m.map(0, 0, &x, &y);
    pts.setPoint(2, x, y);

    pts.translate(endPoint().x(), endPoint().y());

    p.setBrush(QColor(darkGray));
    p.drawPolygon(pts);
}

void PolyMolecule::createPolyData(vtkPolyData *poly_data)
{
  int N = m_Nodes.size();
  EG_VTKSP(vtkPoints, points);
  points->SetNumberOfPoints(N);
  for (int i = 0; i < N; ++i) {
    vec3_t x = getXNode(i);
    points->SetPoint(i, x.data());
  }
  poly_data->Allocate(m_Faces.size());
  poly_data->SetPoints(points);
  for (int i = 0; i < m_Faces.size(); ++i) {
    QVector<vtkIdType> pts(m_Faces[i].size());
    for (int j = 0; j < m_Faces[i].size(); ++j) {
      pts[j] = m_Faces[i][j];
    }
    if (pts.size() == 3) {
      poly_data->InsertNextCell(VTK_TRIANGLE, pts.size(), pts.data());
    } else if (pts.size() == 4) {
      poly_data->InsertNextCell(VTK_QUAD, pts.size(), pts.data());
    } else {
      poly_data->InsertNextCell(VTK_POLYGON, pts.size(), pts.data());
    }
  }
}

/**
 * @function fit_BB
 * @brief 
 */
void fit_BB( std::vector<pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > _clusters ) {
    
    double dim[3]; Eigen::Isometry3d Tf;

    for( int i = 0; i < _clusters.size(); ++i ) {
	printf("\t * Fitting box for cluster %d \n", i );

	pcl::PointCloud<pcl::PointXYZ>::Ptr p( new pcl::PointCloud<pcl::PointXYZ>() );
	p->points.resize( _clusters[i]->points.size() );
	for(int j = 0; j < _clusters[i]->points.size(); ++j ) {
	    p->points[j].x = _clusters[i]->points[j].x;
	    p->points[j].y = _clusters[i]->points[j].y;
	    p->points[j].z = _clusters[i]->points[j].z;
	}
	p->width = 1; p->height = p->points.size();

	// Get Bounding Box
	pointcloudToBB( p, dim, Tf );
	// Store (cube)
	pcl::PointCloud<pcl::PointXYZ>::Ptr pts( new pcl::PointCloud<pcl::PointXYZ>() );
	pcl::PointCloud<pcl::PointXYZ>::Ptr final( new pcl::PointCloud<pcl::PointXYZ>() );

	pts = sampleSQ_uniform( dim[0]/2, dim[1]/2, dim[2]/2, 0.1, 0.1 );
	pcl::transformPointCloud( *pts, *final, Tf.matrix() );

	char name[50];
	sprintf( name, "bb_%d.pcd", i);
	pcl::io::savePCDFileASCII(name, *final );
    }


}

    void hex_27elem_3x3x3()
    {
      libMesh::Point origin = libMesh::Point(0.0,1.5,1.5);

       // end points
      std::vector<libMesh::Point> pts(5);
      pts[0] = libMesh::Point(3.0,1.5,1.5);     // right
      pts[1] = libMesh::Point(1.875,0.0,2.584); // front
      pts[2] = libMesh::Point(1.1,2.67,3.0);    // top
      pts[3] = libMesh::Point(2.75,3.0,0.25);   // back
      pts[4] = libMesh::Point(2.23,2.59,0.0);   // bottom

      // exit_elem IDs
      std::vector<unsigned int> exit_ids(5);
      exit_ids[0] = 14;
      exit_ids[1] = 19;
      exit_ids[2] = 25;
      exit_ids[3] = 8;
      exit_ids[4] = 8;

      std::string hex8_string = this->mesh_3D("HEX8",3.0,3.0,3.0,3,3,3);
      std::string hex27_string = this->mesh_3D("HEX27",3.0,3.0,3.0,3,3,3);

      this->run_test_with_mesh_from_file(origin,pts,exit_ids,hex8_string);
      this->run_test_with_mesh_from_file(origin,pts,exit_ids,hex27_string);
    }

TEST(kdtree, DefaultConstructor)
{
    typedef double realScalarType;
    typedef point<realScalarType> pointType;
    typedef kdTree<pointType> kdTreeType;

    std::default_random_engine generator;

    std::normal_distribution<double> distribution(0.,1.0);

    const size_t numDims = 1;
    const size_t numPoints = 10;

    std::vector<pointType> pts(numPoints);


    for(size_t i=0;i<numPoints;++i)
    {
        std::vector<double> pv(numDims);
        for(size_t j=0;j<numDims;++j)
        {
            pv[j] = distribution(generator);
        }

        pointType pt(pv);

        pts[i] = pt;
    }

    kdTreeType kdtr(pts);
}

void NurbsCurveSP<T,N>::modOnlySurfCPby(int i, const HPoint_nD<T,N>& a) {
    Vector<T> u(2*deg_+3) ;
    Vector< Point_nD<T,N> > pts(2*deg_+3) ;

    int n=0;
    for(int j=i-deg_-1; j<=i+deg_+1; ++j) {
        if(j<0)
            continue ;
        if(j>=P.n())
            break ;
        u[n] = maxAt_[j] ;
        if( j == i) {
            pts[n].x() = a.x() ;
            pts[n].y() = a.y() ;
            pts[n].z() = a.z() ;
        }
        //else
        //  pts[n] = Point3D(0,0,0) ; pts is alredy set to 0,0,0
        ++n ;
    }

    u.resize(n) ;
    pts.resize(n) ;

    movePoint(u,pts) ;
}

visualization_msgs::MarkerArray OccupancyGrid::getVisualization(std::string type)
{
  visualization_msgs::MarkerArray ma;

  if(type.compare("bounds") == 0)
  {
    double dimx, dimy, dimz, originx, originy, originz;
    std::vector<geometry_msgs::Point> pts(10);
    getOrigin(originx, originy, originz);
    getWorldSize(dimx,dimy,dimz);
    pts[0].x = originx;      pts[0].y = originy;      pts[0].z = originz;
    pts[1].x = originx+dimx; pts[1].y = originy;      pts[1].z = originz;
    pts[2].x = originx+dimx; pts[2].y = originy+dimy; pts[2].z = originz;
    pts[3].x = originx;      pts[3].y = originy+dimy; pts[3].z = originz;
    pts[4].x = originx;      pts[4].y = originy;      pts[4].z = originz;
    pts[5].x = originx;      pts[5].y = originy;      pts[5].z = originz+dimz;
    pts[6].x = originx+dimx; pts[6].y = originy;      pts[6].z = originz+dimz;
    pts[7].x = originx+dimx; pts[7].y = originy+dimy; pts[7].z = originz+dimz;
    pts[8].x = originx;      pts[8].y = originy+dimy; pts[8].z = originz+dimz;
    pts[9].x = originx;      pts[9].y = originy;      pts[9].z = originz+dimz;

    ma.markers.resize(1);
    ma.markers[0] = viz::getLineMarker(pts, 0.05, 10, getReferenceFrame(), "collision_space_bounds", 0);
  }
  else if(type.compare("distance_field") == 0)
  {
    visualization_msgs::Marker m;
    // grid_->getIsoSurfaceMarkers(0.01, 0.03, getReferenceFrame(), ros::Time::now(),  Eigen::Affine3d::Identity(), m);
    //grid_->getIsoSurfaceMarkers(0.01, 0.08, getReferenceFrame(), ros::Time::now(), tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,0)), m);
    grid_->getIsoSurfaceMarkers(0.01, 0.02, getReferenceFrame(), ros::Time::now(), m);
    m.color.a +=0.2;
    ma.markers.push_back(m);
  }
  else if(type.compare("occupied_voxels") == 0)
  {
    visualization_msgs::Marker marker;
    std::vector<geometry_msgs::Point> voxels;
    getOccupiedVoxels(voxels);
    marker.header.seq = 0;
    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = getReferenceFrame();
    marker.ns = "occupied_voxels";
    marker.id = 1;
    marker.type = visualization_msgs::Marker::POINTS;
    marker.action = visualization_msgs::Marker::ADD;
    marker.lifetime = ros::Duration(0.0);
    marker.scale.x = grid_->getResolution() / 2.0;
    marker.scale.y = grid_->getResolution() / 2.0;
    marker.scale.z = grid_->getResolution() / 2.0;
    marker.color.r = 0.8;
    marker.color.g = 0.3;
    marker.color.b = 0.5;
    marker.color.a = 1;
    marker.points = voxels;
    ma.markers.push_back(marker);
  }
  else
    ROS_ERROR("No visualization found of type '%s'.", type.c_str());
  return ma;
}