C++ centroid函数代码示例

void GeospatialBoundingBox::exportInformationFile()	{

	FILE *file_ptr = fopen(_format("%s.info",file_name.c_str()).c_str(), "w");
	fprintf(file_ptr, "centroid: %f %f %f\n", centroid(0), centroid(1), centroid(2));
	fprintf(file_ptr, "resolution: %f %f %f\n",resolution(0), resolution(1), resolution(2));
	fprintf(file_ptr, "min_pt: %f %f %f\n", min_pt(0), min_pt(1), min_pt(2));
	fprintf(file_ptr, "max_pt: %f %f %f\n", max_pt(0), max_pt(1), max_pt(2));
	fprintf(file_ptr, "points: %d\n", number_of_points);
	fclose(file_ptr);
	return;
}

void particleScene::draw(float x, float y, float scale){
// moved here for convenience
    if (bGenerate) {
        for (int i = 0; i < flow->features.size(); i++) {
            if (flow->motion[i + flow->motion.size()/2].lengthSquared() > threshold) {
                ofVec2f featMapped;
                featMapped.x = ((flow->features[i].x - 160) * scale + 160 + x);
                featMapped.y = ((flow->features[i].y - 120) * scale + 120 + y);
                
                ofVec2f featVel;
                featVel.x = flow->motion[i + flow->motion.size()/2].x * scale;
                featVel.y = flow->motion[i + flow->motion.size()/2].y * scale;
                
                particles.add(featMapped, featVel * velMult);
                
            }
        }
        
    }
    else {
        if (flow->motion.size() > 0) {
            for (int i = 0; i < particles.particles.size(); i++) {
                ofVec2f featVel;
                featVel.x = flow->motion[i + flow->motion.size()/2].x * scale;
                featVel.y = flow->motion[i + flow->motion.size()/2].y * scale;
                
                
                if (bReverse) featVel *= -1;
                particles.particles[i]->vel+= featVel * velMult;
            }
        }
    }
    
    ofVec2f centroid(0,0);
    for (int i = 0; i < flow->features.size(); i++) {
        centroid+=flow->features[i];
    }
    centroid/=flow->features.size();
    
    particles.update(centroid.x, centroid.y);
    particles.draw(particleColor);
///draw
    
//    
//    ofPushMatrix();
//    ofTranslate(x, y);
//    ofTranslate(160, 120);
//    ofScale(scale, scale);
//    ofTranslate(-160, -120);
  
//    ofPopMatrix();

//    if (bDebug) {
//        for (int i = 0; i < particles.particles.size(); i++) {
//            cout << "particle[" << i << "].pos = " << particles.particles[i]->pos << endl;
//        }
//        bDebug = false;
//    }
//    bDebug = false;
}

tetra* step(const tetra *t, const  point *p, std::vector<tetra*> &branches) {

    tetra *step = nullptr;

    point centroid((t->p[0]->x + t->p[1]->x + t->p[2]->x + t->p[3]->x)/4,
                   (t->p[0]->y + t->p[1]->y + t->p[2]->y + t->p[3]->y)/4,
                   (t->p[0]->z + t->p[1]->z + t->p[2]->z + t->p[3]->z)/4
                  );

    for (unsigned int i = 0; i < 4; ++i) {

        if (!t->ngb[i])
            continue;

        unsigned int included = fast_triangle_ray_inclusion(t->f[i], &centroid, p);

        if (included != 0) {

            if (!step)
                step = t->ngb[i];
            else
                branches.push_back(t->ngb[i]);
        }
    }

    return step;
}

/*! 
  moves and rotates the quad such that it enables us to 
  use components of ef's
*/
void localize_quad_for_ef( VerdictVector node_pos[4])
{

  VerdictVector centroid(node_pos[0]);
  centroid += node_pos[1];
  centroid += node_pos[2];
  centroid += node_pos[3];
  
  centroid /= 4.0;

  node_pos[0] -= centroid;
  node_pos[1] -= centroid;
  node_pos[2] -= centroid;
  node_pos[3] -= centroid;

  VerdictVector rotate = node_pos[1] + node_pos[2] - node_pos[3] - node_pos[0];
  rotate.normalize();

  double cosine = rotate.x();
  double   sine = rotate.y();
 
  double xnew;
 
  for (int i=0; i < 4; i++) 
  {
    xnew =  cosine * node_pos[i].x() +   sine * node_pos[i].y();
    node_pos[i].y( -sine * node_pos[i].x() + cosine * node_pos[i].y() );
    node_pos[i].x(xnew);
  }
}

   double_xy calculateCentroid( tBlob& bo ) {
      struct centroid {
         uint32_t icount;
         uint32_t xsum;
         uint32_t ysum;
         centroid(): icount( 0 ), xsum( 0 ), ysum( 0 ) {}
         void operator()( tStreak const& s ) {
            int l = s.length();

            // -- add y coordinates
            ysum += s.y() * l;

            // -- add x coordinates
            int ix    = s.x();
            icount += l;

            while( l-- ) {
               xsum +=  ix;
               ix++;
            }

         }

      };
      centroid const& s = for_each( bo.streaks().begin(), bo.streaks().end(), centroid() );
      double x = static_cast<double>(s.xsum) / static_cast<double>(s.icount);
      double y = static_cast<double>(s.ysum) / static_cast<double>(s.icount);
      bo._centerOfMass = double_xy( x, y );
      return bo._centerOfMass;

   }

/*!
 * \brief BestFitPlane::setUpResult
 * \param plane
 * \return
 */
bool BestFitPlane::setUpResult(Plane &plane){

    //get and check input observations
    if(!this->inputElements.contains(0) || this->inputElements[0].size() < 3){
        emit this->sendMessage(QString("Not enough valid observations to fit the plane %1").arg(plane.getFeatureName()), eWarningMessage);
        return false;
    }
    QList<QPointer<Observation> > inputObservations;
    foreach(const InputElement &element, this->inputElements[0]){
        if(!element.observation.isNull() && element.observation->getIsSolved() && element.observation->getIsValid()
                && element.shouldBeUsed){
            inputObservations.append(element.observation);
            this->setIsUsed(0, element.id, true);
            continue;
        }
        this->setIsUsed(0, element.id, false);
    }
    if(inputObservations.size() < 3){
        emit this->sendMessage(QString("Not enough valid observations to fit the plane %1").arg(plane.getFeatureName()), eWarningMessage);
        return false;
    }

    //centroid
    OiVec centroid(4);
    foreach(const QPointer<Observation> &obs, inputObservations){
        centroid = centroid + obs->getXYZ();
    }

Vector3 Winding::centroid(const Plane3& plane) const
{
	Vector3 centroid(0,0,0);

	float area2 = 0, x_sum = 0, y_sum = 0;
	const ProjectionAxis axis = projectionaxis_for_normal(plane.normal());
	const indexremap_t remap = indexremap_for_projectionaxis(axis);

	for (std::size_t i = size() - 1, j = 0; j < size(); i = j, ++j)
	{
		const float ai = (*this)[i].vertex[remap.x]
				* (*this)[j].vertex[remap.y] - (*this)[j].vertex[remap.x]
				* (*this)[i].vertex[remap.y];

		area2 += ai;

		x_sum += ((*this)[j].vertex[remap.x] + (*this)[i].vertex[remap.x]) * ai;
		y_sum += ((*this)[j].vertex[remap.y] + (*this)[i].vertex[remap.y]) * ai;
	}

	centroid[remap.x] = x_sum / (3 * area2);
	centroid[remap.y] = y_sum / (3 * area2);
	{
		Ray ray(Vector3(0, 0, 0), Vector3(0, 0, 0));
		ray.origin[remap.x] = centroid[remap.x];
		ray.origin[remap.y] = centroid[remap.y];
		ray.direction[remap.z] = 1;
		centroid[remap.z] = ray.getDistance(plane);
	}

	return centroid;
}

	float Correlator::yforx(float x) {
//	  float a=angle();
	  float g=grad();
	  V2d cen=centroid();
	  float b=cen.y-g*cen.x;
	  return g*x+b;
	}

//-------------------------------------------------------------------------
// Purpose       : 
//
// Special Notes : 
//
// Creator       : Jason Kraftcheck
//
// Creation Date : 05/10/04
//-------------------------------------------------------------------------
CubitStatus PartitionBody::mass_properties( CubitVector& result, double& volume )
{
  DLIList<Lump*> lump_list;
  lumps( lump_list );
  
  DLIList<PartitionLump*> part_list;
  CAST_LIST( lump_list, part_list, PartitionLump );
  if (part_list.size() < lump_list.size())
    return real_body()->mass_properties( result, volume );
  
  CubitVector centroid(0.0, 0.0, 0.0), tmp_centroid;
  volume = 0.0;
  double tmp_volume;
  for (int i = part_list.size(); i--; )
  {
    if (CUBIT_FAILURE == 
        part_list.get_and_step()->mass_properties( tmp_centroid, tmp_volume ))
      return CUBIT_FAILURE;
    
    centroid += tmp_volume * tmp_centroid;
    volume += tmp_volume;
  }
  
  if (volume > CUBIT_RESABS)
  {
    result = centroid / volume;
  }
  else
  {
    result.set( 0.0, 0.0, 0.0 );
    volume = 0.0;
  }
  return CUBIT_SUCCESS;
}

// Computes the centroid of a given cloud
Eigen::Vector4f compute_centroid(pcl::PointCloud<pcl::PointXYZ> cloud)
{
  Eigen::Vector4f centroid(4);
  centroid.setZero ();
  // For each point in the cloud
  int cp = 0;

  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < cloud.points.size (); ++i)
      centroid += cloud.points[i].getVector4fMap ();
    centroid[3] = 0;
    centroid /= cloud.points.size ();
  }
  // NaN or Inf values could exist => check for them
  else
  {
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
      // Check if the point is invalid
      if (!pcl_isfinite (cloud.points[i].x) ||
          !pcl_isfinite (cloud.points[i].y) ||
          !pcl_isfinite (cloud.points[i].z))
        continue;

      centroid += cloud.points[i].getVector4fMap ();
      cp++;
    }
    centroid[3] = 0;
    centroid /= cp;
  }

  return centroid;
}

	Garbage * GarbageHistoric::guessPosition(){
		//assumes linear movement using last known delta/position
		std::vector<int> newCentroid (2,0);
		std::vector<int> centroid (this->garbage->getCentroid());
		std::vector<int> delta (this->deltaPos);
		int factor=(this->age - this->lastAppeareance);
		
		if(factor<=1){
			return this->garbage;
		}

		int deltax=(factor-1)*delta[0];
		int deltay=(factor-1)*delta[1];
		newCentroid[0]=centroid[0] +deltax;
		newCentroid[1]=centroid[1] +deltay;
	
		MinimalBoundingRectangle * oldMbr=this->garbage->boundingBox();
		int x=oldMbr->x;
		int y=oldMbr->y;
		int h=oldMbr->getHeight();
		int w=oldMbr->getWidth();
		
		MinimalBoundingRectangle * mbr= new MinimalBoundingRectangle(x+deltax,y+deltay,w,h);
		Garbage * newGarbage=new  Garbage(mbr,newCentroid);
	
		//benchmark purposes
		newGarbage->isFocused=this->garbage->isFocused;
		newGarbage->isPredicted=this->garbage->isPredicted;
		newGarbage->isVisualized=this->garbage->isVisualized;
		
		
		return newGarbage;
	}

void
MoGES::missing::sortCircular
(
  std::vector<Point>& pointsA
)
{
  // Compute the center of gravity.
  Point centroidL = centroid(pointsA);

  // Copy points to the more suitable list container and make them mean free.
  std::list<Point> pointsL;
  for
  ( std::vector<Point>::const_iterator pointItL = pointsA.begin();
    pointItL < pointsA.end();
    ++pointItL )
  {
    pointsL.push_back(*pointItL - centroidL);
  }

  // Sort points counter-clockwise.
  pointsL.sort(smallerAngleToXAxis<Point>);

  // Copy points to vector and restore their original position in space.
  std::list<Point>::const_iterator pointItL = pointsL.begin();
  std::vector<Point>::iterator resultItL = pointsA.begin();
  while (pointItL != pointsL.end())
  {
    *resultItL = *pointItL + centroidL;

    ++resultItL;  ++pointItL;
  }
}

Sym3x3 ComputeWeightedCovariance( int n, Vec3 const* points, float const* weights, Vec3::Arg metric )
{
	// compute the centroid
	float total = 0.0f;
	Vec3 centroid( 0.0f );
	for( int i = 0; i < n; ++i )
	{
		total += weights[i];
		centroid += weights[i]*points[i];
	}
	centroid /= total;

	// accumulate the covariance matrix
	Sym3x3 covariance( 0.0f );
	for( int i = 0; i < n; ++i )
	{
		Vec3 a = (points[i] - centroid) * metric;
		Vec3 b = weights[i]*a;
		
		covariance[0] += a.X()*b.X();
		covariance[1] += a.X()*b.Y();
		covariance[2] += a.X()*b.Z();
		covariance[3] += a.Y()*b.Y();
		covariance[4] += a.Y()*b.Z();
		covariance[5] += a.Z()*b.Z();
	}
	
	// return it
	return covariance;
}

double YARPOrientation::Apply(const YARPImageOf<YarpPixelMono> &image)
{
    double
        x = 0.0,
        y = 0.0 ;

    centroid(image, x, y) ;
    
    double
        argsin = 0.0,
        argcos = 0.0 ;

    arguments(image, x, y, argsin, argcos) ;
	        
    double
        angle = 0.0 ;

    if (argsin > 0.0 && argcos > 0.0)
        angle = (asin(argsin)) / 2.0 ;
    else if (argsin > 0.0 && argcos < 0.0)
        angle = (acos(argcos)) / 2.0 ;
    else if (argsin < 0.0 && argcos < 0.0)
        angle = (-1.0) * (acos(argcos)) / 2.0;
    else if (argsin < 0.0 && argcos > 0.0)
        angle = (asin(argsin)) / 2.0 ;
    
#ifdef __WIN32__
    return angle == numeric_limits<double>::signaling_NaN() ? 0 : - angle ;
#else
	//should check for error using ERRNO kind of stuff.
	return -angle;
#endif
}

void
FeatureFloodCount::flood(const DofObject * dof_object, unsigned long current_idx, FeatureData * feature)
{
  if (dof_object == nullptr)
    return;

  // Retrieve the id of the current entity
  auto entity_id = dof_object->id();

  // Has this entity already been marked? - if so move along
  if (_entities_visited[current_idx].find(entity_id) != _entities_visited[current_idx].end())
    return;

  // Mark this entity as visited
  _entities_visited[current_idx][entity_id] = true;

  // Determine which threshold to use based on whether this is an established region
  auto threshold = feature ? _step_connecting_threshold : _step_threshold;

  // Get the value of the current variable for the current entity
  Real entity_value;
  if (_is_elemental)
  {
    const Elem * elem = static_cast<const Elem *>(dof_object);
    std::vector<Point> centroid(1, elem->centroid());
    _subproblem.reinitElemPhys(elem, centroid, 0);
    entity_value = _vars[current_idx]->sln()[0];
  }
  else
    entity_value = _vars[current_idx]->getNodalValue(*static_cast<const Node *>(dof_object));

  // This node hasn't been marked, is it in a feature?  We must respect
  // the user-selected value of _use_less_than_threshold_comparison.
  if (_use_less_than_threshold_comparison && (entity_value < threshold))
    return;

  if (!_use_less_than_threshold_comparison && (entity_value > threshold))
    return;

  /**
   * If we reach this point (i.e. we haven't returned early from this routine),
   * we've found a new mesh entity that's part of a feature.
   */
  auto map_num = _single_map_mode ? decltype(current_idx)(0) : current_idx;

  // New Feature (we need to create it and add it to our data structure)
  if (!feature)
    _partial_feature_sets[map_num].emplace_back(current_idx);

  // Get a handle to the feature we will update (always the last feature in the data structure)
  feature = &_partial_feature_sets[map_num].back();

  // Insert the current entity into the local ids map
  feature->_local_ids.insert(entity_id);

  if (_is_elemental)
    visitElementalNeighbors(static_cast<const Elem *>(dof_object), current_idx, feature, /*expand_halos_only =*/false);
  else
    visitNodalNeighbors(static_cast<const Node *>(dof_object), current_idx, feature, /*expand_halos_only =*/false);
}