Sphere Sphere::calculateBoundingSphere( const vec3 *points, size_t numPoints )
{
	if( ! numPoints )
		return Sphere( vec3( 0 ), 0 );
	
	// compute minimal and maximal bounds
	vec3 min(points[0]), max(points[0]);
	for( size_t i = 1; i < numPoints; ++i ) {
		if( points[i].x < min.x )
			min.x = points[i].x;
		else if( points[i].x > max.x )
			max.x = points[i].x;
		if( points[i].y < min.y )
			min.y = points[i].y;
		else if( points[i].y > max.y )
			max.y = points[i].y;
		if( points[i].z < min.z )
			min.z = points[i].z;
		else if( points[i].z > max.z )
			max.z = points[i].z;
	}
	// compute center and radius
	vec3 center = 0.5f * ( min + max );
	float maxDistance = distance2( center, points[0] );
	for( size_t i = 1; i < numPoints; ++i ) {
		float dist = distance2( center, points[i] );
		if( dist > maxDistance )
			maxDistance = dist;
	}
	return Sphere( center, math<float>::sqrt( maxDistance ) );
}

/** Recherche les 3 premiers points non aligné
*/
bool Convexe2D::initNonAlignee(const QVector<Vector2D>& points, int& i0, int& i1, int& i2) const
{
    i0 = 0; i1 = 1; i2 = 2;
    Vector2D p0 = points[i0], p1 = points[i1];
    while(i2 < points.size())
    {
        Vector2D p2 = points[i2];
        if(inHalfSpaceDroit(p0,p1,p2) != ALIGNEE)
            return true;
        else
        {
            //mettre p2 entre p0 et p1
            if(distance2(p0, p1) < distance2(p0, p2)){  // p0 -- p1 -- p2   ou   p1 -- p0 ---- p2
                int swap = i2;
                i2 = i1;
                i1 = swap;
                p1 = points[i1];
                p2 = points[i2];                       // p0 -- p2 --- p1    ou   p2 -- p0 ---- p1
            }
            if(distance2(p0, p1) < distance2(p1, p2))   //p2 -- p0 ---- p1
            {
                int swap = i0;
                i0 = i2;
                i2 = swap;
                p0 = points[i0];                       //p0 -- p2 ---- p1
                //p2 = points[i2];
            }
            i2++;
        }
    }
    return false;   //on ne peut pas construire un triangle avec les points, ils sont tous aligné
}

void Quadtree::update(GLdouble x, GLdouble y, GLdouble z) {
  bool split = distance2(x, y, z) < (box[2] - box[0]) * (box[2] - box[0]) * 4.;

  if (split) {
    if (children[0] == NULL && level > 0) {
      divide();
    }
    if (children[0] != NULL) {
      children[0]->update(x, y, z);
      children[1]->update(x, y, z);
      children[2]->update(x, y, z);
      children[3]->update(x, y, z);
    }
  } else if (children[0] != NULL) {
    delete children[0];
    delete children[1];
    delete children[2];
    delete children[3];
    children[0] = NULL;
    children[1] = NULL;
    children[2] = NULL;
    children[3] = NULL;
  }

  if (distance2(x, y, z) < distance * distance) {
    distance = sqrt(distance2(x, y, z));
  }
}

void Features::makeBasicFeature(int offset,
                                const Node *first,
                                const Node *last) {
  // distance
  addFeature(offset + 1 , 10 * distance(first, last));

  // degree
  addFeature(offset + 2 ,
             std::atan2(last->y - first->y, last->x - first->x));

  // absolute position
  addFeature(offset + 3, 10 * (first->x - 0.5));
  addFeature(offset + 4, 10 * (first->y - 0.5));
  addFeature(offset + 5, 10 * (last->x - 0.5));
  addFeature(offset + 6, 10 * (last->y - 0.5));

  // absolute degree
  addFeature(offset + 7, std::atan2(first->y - 0.5, first->x - 0.5));
  addFeature(offset + 8, std::atan2(last->y - 0.5,  last->x - 0.5));

  // absolute distance
  addFeature(offset + 9,  10 * distance2(first));
  addFeature(offset + 10, 10 * distance2(last));

  // diff
  addFeature(offset + 11, 5 * (last->x - first->x));
  addFeature(offset + 12, 5 * (last->y - first->y));
}

real calc_geom(int isize, atom_id *index, rvec *x, rvec geom_center, rvec min,
               rvec max, gmx_bool bDiam)
{
    real diam2, d;
    char *grpnames;
    int ii, i, j;

    clear_rvec(geom_center);
    diam2 = 0;
    if (isize == 0)
    {
        clear_rvec(min);
        clear_rvec(max);
    }
    else
    {
        if (index)
            ii = index[0];
        else
            ii = 0;
        for (j = 0; j < DIM; j++)
            min[j] = max[j] = x[ii][j];
        for (i = 0; i < isize; i++)
        {
            if (index)
                ii = index[i];
            else
                ii = i;
            rvec_inc(geom_center, x[ii]);
            for (j = 0; j < DIM; j++)
            {
                if (x[ii][j] < min[j])
                    min[j] = x[ii][j];
                if (x[ii][j] > max[j])
                    max[j] = x[ii][j];
            }
            if (bDiam)
            {
                if (index)
                    for (j = i + 1; j < isize; j++)
                    {
                        d = distance2(x[ii], x[index[j]]);
                        diam2 = max(d,diam2);
                    }
                else
                    for (j = i + 1; j < isize; j++)
                    {
                        d = distance2(x[i], x[j]);
                        diam2 = max(d,diam2);
                    }
            }
        }
        svmul(1.0 / isize, geom_center, geom_center);
    }

    return sqrt(diam2);
}

void callback(JointsSet& collection, const brics_actuator::JointPositionsConstPtr& msg) {//Des nouvelles coordonnées ont été publiées
  Joints coord_recues = {
    msg->positions[0].value, 
    msg->positions[1].value, 
    msg->positions[2].value, 
    msg->positions[3].value
  };

 
  auto min_iter = std::min_element(collection.begin(), collection.end(),
				   boost::bind(distance2,std::ref(coord_recues),_1));
  
  // Si Ok on l'enregistre
  if(distance2(coord_recues,*min_iter) > MIN_DIST_THRESHOLD*MIN_DIST_THRESHOLD) {
      std::ofstream fichier; 
      fichier.exceptions(std::ios::failbit | std::ios::badbit);
      try {
	fichier.open(adresseTexte.c_str(), std::ios_base::app);
	fichier << coord_recues << std::endl;
	fichier.close();
	collection.push_back(coord_recues);
	ROS_INFO_STREAM("Coordonnee enregistree");
	std::cout << coord_recues<<std::endl;
      }
      catch (const std::exception& e) {
	ROS_INFO_STREAM("ERREUR Ouverture Fichier");
      }
  }
}

int mergecb(float *B,float **cb,unsigned char *P,int npt,int N,float thresh,int dim)
{
  int i,j,newN,*count,l,ei,debug=0;
  float **dist;

  if (N==1) return N;

  count=(int *)malloc(sizeof(int)*N);
  for (l=0;l<npt;l++) count[P[l]]++;

  dist=(float **)fmatrix(N,N);
  ei = N-1;
  for (i=0;i<ei;i++)
  {
    for (j=i+1;j<N;j++)
    {
      dist[i][j] = distance2(cb[i],cb[j],dim);
      dist[j][i] = dist[i][j];
    }
  }

  if (debug) printf("thresh %f ",thresh);
  newN=mergecb1(dist,B,cb,N,thresh,dim,count);

  free_fmatrix(dist,N);
  free(count);
  return newN;
}

   // implementation of the function to be minimized
   double operator() (const double * par) {

      assert(fGraph    != 0);
      double * x = fGraph->GetX();
      double * y = fGraph->GetY();
      double * z = fGraph->GetZ();
      int npoints = fGraph->GetN();
      double sum = 0;
      for (int i  = 0; i < npoints; ++i) {
         double d = distance2(x[i],y[i],z[i],par);
         sum += d;
#ifdef DEBUG
         if (first) std::cout << "point " << i << "\t"
            << x[i] << "\t"
            << y[i] << "\t"
            << z[i] << "\t"
            << std::sqrt(d) << std::endl;
#endif
      }
      
      // if (first)
      // 	std::cout << "Total distance square w/ initial params: " << sum << std::endl;
      // 	first = false;
      return sum;
   }

multimap<int,double>
Mesh<Ttype, Tpos>::getNearMeshMap(Tpos x, Tpos y, Tpos z, Tpos rmax, Tpos rmin) {
    std::multimap<int, double> nbr;   // the return list
    nbr.clear();          // clear list
    int ix,iy,iz,ii,p;
    Tpos dst2;
    ix = int((x - xmin)/dx);     // calculate index of point
    iy = int((y - ymin)/dy);
    iz = int((z - zmin)/dz);
    int srx = int(rmax/dx)+1;    // calculate radius in terms of mesh indicies
    int sry = int(rmax/dy)+1;
    int srz = int(rmax/dz)+1;
    std::vector<int> close = closemeshes(ix,iy,iz,srx,sry,srz);
    for (std::vector<int>::iterator ii=close.begin(); ii!=close.end(); ii++) {
        if ( (p=head[*ii])>=0 ) {
            do {
                dst2 = distance2((*dat)[p]->getX(), (*dat)[p]->getY(), (*dat)[p]->getZ(), x, y, z);
                if (dst2<rmax*rmax && dst2>=rmin*rmin) {
                    nbr.insert(std::pair<double,int>(p,dst2));
                }
            } while( (p=next[p])>=0 );
        }
    }
    return(nbr);
}

void solve(){
	memset(matches,0,sizeof(matches));
//	printf("%d\n",sizeof(matches));
	memset(links,0,sizeof(links));
	memset(ans,0,sizeof(ans));
	for(int i=1;i<n;i++){
		double distOfI = 2*distance1(i);
		for(int j = 1;j<=m;j++){
			if(distance2(i,j)<=distOfI){
				matches[i][j] = 1;
			}
		}
	}
	int s = n;
	for(int i=1;i<=n;i++){
		memset(visited,0,sizeof(visited));
		s+=dfs(i);
	}
	printf("%d\n",s);
	for(int i=1;i<n;i++){
		printf("%d %d ",routes[i][0],routes[i][1]);
		if(ans[i]){
			printf("%d %d ",places[ans[i]][0],places[ans[i]][1]);
		}
	}
	printf("%d %d",routes[n][0],routes[n][1]);
	printf("\n");
}

PyObject *wrap_distance2(PyObject *self,PyObject *args)
{
  PyObject *cs1, *cs2;
  PyObject *fast1, *fast2;

  if(!PyArg_ParseTuple(args,"OO",&cs1,&cs2))
    return NULL;

  if(!(fast1 = PySequence_Fast(cs1, "could not get fast sequence")))
    return NULL;
  if(!(fast2 = PySequence_Fast(cs2, "could not get fast sequence"))) {
    return NULL;
  }
  rvec r1,r2;
  int i;
  PyObject *d1,*d2;
  for(i=0;i<DIM;i++){
    d1=PySequence_Fast_GET_ITEM(fast1,i);
    d2=PySequence_Fast_GET_ITEM(fast2,i);
    r1[i] = PyFloat_AsDouble(d1);
    r2[i] = PyFloat_AsDouble(d2);
  }
  real d = distance2(r1,r2);
  PyObject *ret;
  ret=Py_BuildValue("d",d);
  return ret;
}

/*
  Compute the exact integration of the mipmap \a M within the ball of
  radius r and center (x0,y0,z0).
  
  The method is a simple scanning at the finest scale.
*/
Value computeExact( MipMap* M, int x0, int y0, int z0, Value r )
{
  Image* img = MipMap_get_image( M, M->max_lvl );
  int minx = x0 - (int) ceil( r );
  int miny = y0 - (int) ceil( r );
  int minz = z0 - (int) ceil( r );
  int maxx = x0 + (int) ceil( r );
  int maxy = y0 + (int) ceil( r );
  int maxz = z0 + (int) ceil( r );
  minx = minx <= 0 ? 0 : minx;
  miny = miny <= 0 ? 0 : miny;
  minz = minz <= 0 ? 0 : minz;
  maxx = maxx >= img->size ? img->size-1 : maxx;
  maxy = maxy >= img->size ? img->size-1 : maxy;
  maxz = maxz >= img->size ? img->size-1 : maxz;
  Value r2  = r*r;
  Value acc = (Value) 0;
  for ( int z = minz; z <= maxz; ++z )
    for ( int y = miny; y <= maxy; ++y )
      for ( int x = minx; x <= maxx; ++x )
        {
          if ( distance2( x0, y0, z0, x, y, z ) <= r2 )
            {
              acc += Image_get( img, x, y, z );
              nb_access_exact += 1;
            }
          nb_iteration_exact += 1;
        }
  return acc;
}

double Atomtype::CalcPE(int frame_i, const Trajectory &trj, const coordinates &rand_xyz, const cubicbox_m256 &box, double vol) const
{
    float pe = 0.0;
    int atom_i = 0;

    /* BEGIN SIMD SECTION */
    // This performs the exact same calculation after the SIMD section
    // but doing it on 8 atoms at a time using SIMD instructions.

    coordinates8 rand_xyz8(rand_xyz), atom_xyz;
    __m256 r2_8, mask, r6, ri6, pe_tmp;
    __m256 pe_sum = _mm256_setzero_ps();
    float result[n] __attribute__((aligned (16)));

    for (; atom_i < this->n-8; atom_i+=8)
    {
        atom_xyz = trj.GetXYZ8(frame_i, this->name, atom_i);
        r2_8 = distance2(atom_xyz, rand_xyz8, box);
        mask = _mm256_cmp_ps(r2_8, rcut2_8, _CMP_LT_OS);
        r6 = _mm256_and_ps(mask, _mm256_mul_ps(_mm256_mul_ps(r2_8, r2_8), r2_8));
        ri6 = _mm256_and_ps(mask, _mm256_rcp_ps(r6));
        pe_tmp = _mm256_and_ps(mask, _mm256_mul_ps(ri6, _mm256_sub_ps(_mm256_mul_ps(c12_8, ri6), c6_8)));
        pe_sum = _mm256_add_ps(pe_tmp, pe_sum);
    }
    _mm256_store_ps(result, pe_sum);
    for (int i = 0; i < 8; i++)
    {
        pe += result[i];
    }

    /* END SIMD SECTION */

    for (; atom_i < this->n; atom_i++)
    {
        coordinates atom_xyz = trj.GetXYZ(frame_i, this->name, atom_i);
        float r2 = distance2(atom_xyz, rand_xyz, cubicbox(box));
        if (r2 < this->rcut2)
        {
            float ri6 = 1.0/(pow(r2,3));
            pe += ri6*(this->c12*ri6 - this->c6);
        }
    }

    pe += this->n/vol * this->tail_factor;;

    return pe;
}

void getcmap(float *B,unsigned char *cmap,float **cb,int npt,int dim,int N)
{
  int i,j;
  float mindif,dif;

  for (i=0;i<npt;i++)
  {
    mindif = distance2(B,cb[0],dim);
    cmap[i]=0;
    for (j=1;j<N;j++)
    {
      dif = distance2(B,cb[j],dim);
      if (dif<mindif) { cmap[i]=j; mindif=dif; }
    }
    B += dim;
  }
}

//H-Client
// TODO: should be more general
int CGameClient::IntersectCharacter(vec2 Pos0, vec2 Pos1, vec2& NewPos, float Speed/*hook speed*/, int ownID)
{
	// Find other players
	const float ProximityRadius2 = (28.f + 2.f) * (28.f + 2.f);
	float ClosestLen2 = distance2(Pos0, Pos1) * 10000.0f;
	int ClosestID = -1;
	
	if (!m_Tuning.m_PlayerHooking)
		return ClosestID;

	for (int i=0; i<MAX_CLIENTS; i++)
	{
        const CClientData& cData = m_aClients[i];

        if (!cData.m_Active || cData.m_Team == TEAM_SPECTATORS || i == ownID || !m_Teams.SameTeam(i, ownID))
            continue;

        vec2 Position = cData.m_Predicted.m_Pos;
        vec2 Velocity = cData.m_Predicted.m_Vel;

		vec2 IntersectPos = closest_point_on_line(Pos0, Pos1, Position);
		
		if(Speed != 0.f)
		{
			const float DistFromTarget = distance(Pos0, IntersectPos);
			// predict
			Position += Velocity * (DistFromTarget / Speed);
			// can be deleted ?
			IntersectPos = closest_point_on_line(Pos0, Pos1, Position);
		}
		
		float Len2 = distance2(Position, IntersectPos);
		if(Len2 < ProximityRadius2)
		{
			Len2 = distance2(Pos0, IntersectPos);
			if(Len2 < ClosestLen2)
			{
				NewPos = IntersectPos;
				ClosestLen2 = Len2;
				ClosestID = i;
			}
		}
	}

	return ClosestID;
}

bool steepest_descent_step(void) {
  Cell *cell;
  Particle *p;
  int c, i, j, np;
  double f_max = -std::numeric_limits<double>::max();
  double f;
  /* Verlet list criterion */
  const double skin2 = SQR(0.5*skin);
  double f_max_global;
  double dx[3], dx2;
  const double max_dx2 = SQR(params->max_displacement);

  for (c = 0; c < local_cells.n; c++) {
    cell = local_cells.cell[c];
    p  = cell->part;
    np = cell->n;
    for(i = 0; i < np; i++) {
      f = 0.0;
      dx2 = 0.0;
#ifdef VIRTUAL_SITES
      if (ifParticleIsVirtual(&p[i])) continue;
#endif
      for(j=0; j < 3; j++){
#ifdef EXTERNAL_FORCES
        if (!(p[i].p.ext_flag & COORD_FIXED(j)))
#endif
          {
            f += SQR(p[i].f.f[j]);	    	    
	    dx[j] = params->gamma * p[i].f.f[j];	    
	    dx2 += SQR(dx[j]);
	    MINIMIZE_ENERGY_TRACE(printf("part %d dim %d dx %e gamma*f %e\n", i, j, dx[j], params->gamma * p[i].f.f[j]));
	  }
#ifdef EXTERNAL_FORCES
	else {
	  dx[j] = 0.0;	  
	}
#endif
      }

      if(dx2 <= max_dx2) {
	p[i].r.p[0] += dx[0];
	p[i].r.p[1] += dx[1];
	p[i].r.p[2] += dx[2];
      } else {
	const double c = params->max_displacement/std::sqrt(dx2);
	p[i].r.p[0] += c*dx[0];
	p[i].r.p[1] += c*dx[1];
	p[i].r.p[2] += c*dx[2];
      }
      if(distance2(p[i].r.p,p[i].l.p_old) > skin2 ) resort_particles = 1;
      f_max = std::max(f_max, f);
    }
  }
  MINIMIZE_ENERGY_TRACE(printf("f_max %e resort_particles %d\n", f_max, resort_particles));
  announce_resort_particles();
  MPI_Allreduce(&f_max, &f_max_global, 1, MPI_DOUBLE, MPI_MAX, comm_cart);
  return (sqrt(f_max_global) < params->f_max);
}

static void cellFunc(const R3 &p, float &n0, float &f0, float &f1)
{
    R3 q = p;
    int i = (int)floorf(q.x);
    int j = (int)floorf(q.y);
    int k = (int)floorf(q.z);
    q.x -= i;
    q.y -= j;
    q.z -= k;
    int index = fold(i,j,k);
    float minDist = distance2(CellSampleTable[index], q);
    float minDist2 = 2.0;
    int k0;
    k0 = index;
    
    // easy but slow way, check distance to point in each adjacent 
    // cell to find closest and second closest.
    R3 q1;
    for (int ii = -1; ii <= 1; ++ii) {
		q1.x = q.x - ii;
		int i1 = i + ii;
		for (int jj = -1; jj <= 1; ++jj) {
			q1.y = q.y - jj;
			int j1 = j + jj;
			for (int kk = -1; kk <= 1; ++kk) {
				if (!ii && !jj && !kk) continue;
				q1.z = q.z - kk;
				int k1 = k + kk;
				index = fold(i1, j1, k1);
				float t = distance2(CellSampleTable[index], q1);
				if (minDist > t) {
					minDist2 = minDist;
					minDist = t;
					k0 = index;
				}
				else if (minDist2 > t) {
					minDist2 = t;
				}
			}
		}
    }
    f0 = sqrtf(minDist);
    f1 = sqrtf(minDist2);
    n0 = k0/(float)(N_CELLS);
}

float Sphere::distance(const Vector3D& p) const
{
    float dst2 = distance2(centre, p);

    if(dst2 > rayon*rayon)
        return sqrt(dst2)-rayon;
    else
        return 0;
}

real dist2_from_atoms( PyObject *atom1, PyObject *atom2)
{
  PyObject *Ox1 = PyObject_GetAttrString(atom1, "x");
  PyObject *Ox2 = PyObject_GetAttrString(atom2, "x");
  rvec x1, x2;
  Pyvec2rvec( Ox1, x1);
  Pyvec2rvec( Ox2, x2);
  return  distance2(x1, x2 );
}

static double helixDistance2(HelixInfo bot, HelixInfo top,
		double delta_z, double delta_phi)
{
	double dz = top.z - bot.z + delta_z;
	double dphi = top.phi - bot.phi + delta_phi;
	Vec3 low  = (Vec3) {bot.r, 0, 0};
	Vec3 high = (Vec3) {top.r * cos(dphi), top.r * sin(dphi), dz};
	return distance2(low, high);
}