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

void Enumerator::create_partition(std::vector<Plane>& partition, Plane& left) {
	if (left.size() == 0) {
		Plane cen = centroid(partition);
		double costs = evaluate_partition(partition, cen, fix_costs);
		if (costs < best_costs) {
			best_costs = costs;
			best_sites = cen;
			best_partition = partition;
			//std::cout << "current best value = " << costs << std::endl;
		}
		return;
	}

	Point current = left.back();
	left.pop_back();

	// insert into each existing subset
	for (unsigned int i = 0; i < partition.size(); ++i) {
		if ((int) partition[i].size() < capacity) { //only if <capacity
			partition[i].push_back(current);
			create_partition(partition, left);
			partition[i].pop_back();
		}
	}

	// deal with case: new subset for current element
	partition.push_back(Plane());
	partition.back().push_back(current);
	create_partition(partition, left);
	partition.pop_back();

	// restore
	left.push_back(current);
}

void Polygon::normalize() {
    // Assume centroid at point 0,0
    Vector2d centroid(0,0);
    double area = 0;
    const double three_inverse = 1/3;

    // For each triangle
    for(int i = 0;i<vertex_count;i++)
    {
        Vector2d p1(vertices[i]);
        int j = i + 1 <vertex_count ? i+1 :0;
        Vector2d p2(vertices[j]);
        double d = cross(p1,p2);
        // Calculate triangle area
        double triangle_area = 0.5 * d;
        area += triangle_area;

        // http://www.seas.upenn.edu/~sys502/extra_materials/Polygon%20Area%20and%20Centroid.pdf
        centroid += triangle_area * three_inverse * (p1+p2);
    }
    centroid *= 1 / area;

    // Correct vertices
    for(int i = 0;i<vertex_count;i++)
        vertices[i]-=centroid;
}

int main(int argc, char ** argv)
{
#ifdef QUESO_HAS_MPI
  MPI_Init(&argc, &argv);

  QUESO::FullEnvironment env(MPI_COMM_WORLD, "", "", NULL);
#else
  QUESO::FullEnvironment env("", "", NULL);
#endif

  QUESO::VectorSpace<> paramSpace1(env, "param1_", 4, NULL);
  QUESO::VectorSpace<> paramSpace2(env, "param2_", 2, NULL);

  QUESO::GslVector paramMins1(paramSpace1.zeroVector());
  paramMins1[0] = 1e2;
  paramMins1[1] = -1e5;
  paramMins1[2] = 4e-3;
  paramMins1[3] = 1;

  QUESO::GslVector paramMaxs1(paramSpace1.zeroVector());
  paramMaxs1[0] = 2e2;
  paramMaxs1[1] = 1e5;
  paramMaxs1[2] = 6e-3;
  paramMaxs1[3] = 11;

  QUESO::BoxSubset<> paramDomain1("", paramSpace1, paramMins1, paramMaxs1);

  QUESO::GslVector paramMins2(paramSpace2.zeroVector());
  paramMins2[0] = -1e5;
  paramMins2[1] = 2e-3;

  QUESO::GslVector paramMaxs2(paramSpace2.zeroVector());
  paramMaxs2[0] = 1e5;
  paramMaxs2[1] = 4e-3;

  QUESO::BoxSubset<> paramDomain2("", paramSpace2, paramMins2, paramMaxs2);

  QUESO::VectorSpace<> paramSpace(env, "param_", 6, NULL);

  QUESO::ConcatenationSubset<QUESO::GslVector,QUESO::GslMatrix>
    paramDomain("",paramSpace,paramDomain1,paramDomain2);

  QUESO::GslVector centroid(paramSpace.zeroVector());
  paramDomain.centroid(centroid);

  const char *msg = "ConcatenationSubset centroid is incorrect";
  queso_require_less_equal_msg(std::abs(centroid[0]-1.5e2), TOL, msg);
  queso_require_less_equal_msg(std::abs(centroid[1]), TOL, msg);
  queso_require_less_equal_msg(std::abs(centroid[2]-5e-3), TOL, msg);
  queso_require_less_equal_msg(std::abs(centroid[3]-6), TOL, msg);
  queso_require_less_equal_msg(std::abs(centroid[4]), TOL, msg);
  queso_require_less_equal_msg(std::abs(centroid[5]-3e-3), TOL, msg);

#ifdef QUESO_HAS_MPI
  MPI_Finalize();
#endif

  return 0;
}

	float Correlator::crossesyxoutliers() {
	  float g=bestgradxoutliers();
	  // y=Ax+b b=y-Ax x=-b/A
	  V2d cen=centroid();
	  float b=cen.y-g*cen.x;
	  float x=-b/g;
	  return x;
	}

void Face::update(){
	if(upToDate){
		return;
	}

	double x0, y0, x1, y1;
	x0 = (*vertexA)(0);
	y0 = (*vertexA)(1);
	x1 = (*vertexB)(0);
	y1 = (*vertexB)(1);
	area = sqrt(pow2(x1 - x0) + pow2(y1 - y0));

	centroid(0) = (x0 + x1)/2;
	centroid(1) = (y0 + y1)/2;

	//upToDate = true;
}